Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
lisa_stm_passthrough_main.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 The Paparazzi Team
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with Paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
23 #include "mcu.h"
24 #include "mcu_periph/uart.h"
25 #include "mcu_periph/sys_time.h"
27 #include "subsystems/commands.h"
28 #include "subsystems/actuators.h"
30 #include "subsystems/imu.h"
32 #include "autopilot.h"
33 #include "subsystems/ins.h"
34 #include "guidance.h"
35 #include "navigation.h"
36 #include "lisa/lisa_overo_link.h"
37 #include "generated/airframe.h"
38 #include "subsystems/ahrs.h"
39 #ifdef PASSTHROUGH_CYGNUS
40 #include "stabilization.h"
41 #endif
42 
43 #include "stm32/can.h"
44 #include "csc_msg_def.h"
45 #include "csc_protocol.h"
46 
48 
49 #include "mcu_periph/adc.h"
50 
51 static inline void main_init(void);
52 static inline void main_periodic(void);
53 static inline void main_event(void);
54 
55 static inline void on_gyro_accel_event(void);
56 static inline void on_accel_event(void);
57 static inline void on_mag_event(void);
58 
59 static inline void on_overo_link_msg_received(void);
60 static inline void on_overo_link_lost(void);
61 static inline void on_overo_link_crc_failed(void);
62 
63 static inline void on_rc_message(void);
64 static inline void on_vane_msg(void *data);
65 
66 static bool_t new_radio_msg;
67 static bool_t new_baro_diff;
68 static bool_t new_baro_abs;
69 static bool_t new_vane;
70 static bool_t new_adc;
71 
72 
73 struct CscVaneMsg csc_vane_msg;
74 
75 static struct adc_buf adc0_buf;
76 static struct adc_buf adc1_buf;
77 static struct adc_buf adc2_buf;
78 static struct adc_buf adc3_buf;
79 
81 
82 struct CscServoCmd csc_servo_cmd;
83 
84 #define ActuatorsCommit() actuators_pwm_commit();
85 #define actuators actuators_pwm_values
86 
87 #define ChopServo(x,a,b) Chop(x, a, b)
88 #define Actuator(i) actuators[i]
89 #define SERVOS_TICS_OF_USEC(_s) (_s)
90 
91 int main(void) {
92 
93  main_init();
94 
95  while (1) {
97  main_periodic();
98  main_event();
99  }
100 
101  return 0;
102 }
103 
104 static inline void main_init(void) {
105 
106  mcu_init();
108  imu_init();
109  baro_init();
111  actuators_init();
112  overo_link_init();
113  cscp_init();
114 
115 #ifdef PASSTHROUGH_CYGNUS
116  autopilot_init();
117  nav_init();
118  guidance_h_init();
119  guidance_v_init();
121 
123  ahrs_init();
124 
125  ins_init();
126 #endif
127 
128  adc_buf_channel(0, &adc0_buf, 8);
129  adc_buf_channel(1, &adc1_buf, 8);
130  adc_buf_channel(2, &adc2_buf, 8);
131  adc_buf_channel(3, &adc3_buf, 8);
132 
133  cscp_register_callback(CSC_VANE_MSG_ID, on_vane_msg, (void *)&csc_vane_msg);
137  new_vane = FALSE;
138  new_adc = FALSE;
139 
140  overo_link.up.msg.imu_tick = 0;
141 }
142 
143 static void check_radio_lost(void)
144 {
145 #ifdef PASSTHROUGH_CYGNUS
146  return;
147 #endif
149  const static int32_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
150  SetActuatorsFromCommands(commands_failsafe);
151  }
152 }
153 
154 static inline void main_periodic(void) {
155  uint16_t v1 = 123;
156  uint16_t v2 = 123;
157 
158  imu_periodic();
159 #ifdef PASSTHROUGH_CYGNUS
161 #endif
163 
164  RunOnceEvery(10, {
165  LED_PERIODIC();
166  DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
167  radio_control_periodic();
169  DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &baro.absolute, &baro.differential);
170  });
171 
172  RunOnceEvery(2, {baro_periodic();});
173 
174  if (adc_new_data_trigger) {
176  new_adc = 1;
178  v2 = adc1_buf.values[0];
179 
180  RunOnceEvery(10, { DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, DefaultDevice, &v1, &v2) });
181  }
182 }
183 
184 static inline void on_rc_message(void) {
186  if (radio_control.values[RADIO_MODE] >= 150) {
187 #ifdef PASSTHROUGH_CYGNUS
189 #else
190  static int32_t commands[COMMANDS_NB];
191  SetCommandsFromRC(commands, radio_control.values);
192  SetActuatorsFromCommands(commands);
193 #endif
194  }
195 #ifndef PASSTHROUGH_CYGNUS
196  if (radio_control.values[RADIO_KILL] > 150) {
197  actuators[SERVO_THROTTLE] = SERVO_THROTTLE_MIN;
198  ActuatorsCommit();
199  }
200 #endif
201 }
202 
203 static inline void on_overo_link_msg_received(void) {
204 
205  /* IMU up */
206  overo_link.up.msg.valid.imu = 1;
210 
211  /* RC up */
212  overo_link.up.msg.valid.rc = new_radio_msg;
214 
220 #ifdef RADIO_CONTROL_KILL
222 #endif
223 #ifdef RADIO_CONTROL_GEAR
225 #endif
226 
229  overo_link.up.msg.rc_status = radio_control.status;
230 
231  overo_link.up.msg.stm_msg_cnt = overo_link.msg_cnt;
232  overo_link.up.msg.stm_crc_err_cnt = overo_link.crc_err_cnt;
233 
234  /* baro up */
235  overo_link.up.msg.valid.pressure_differential = new_baro_diff;
236  overo_link.up.msg.valid.pressure_absolute = new_baro_abs;
239  overo_link.up.msg.pressure_differential = baro.differential;
240  overo_link.up.msg.pressure_absolute = baro.absolute;
241 
242  /* vane up */
243  overo_link.up.msg.valid.vane = new_vane;
244  new_vane = FALSE;
245  overo_link.up.msg.vane_angle1 = csc_vane_msg.vane_angle1;
246  overo_link.up.msg.vane_angle2 = csc_vane_msg.vane_angle2;
247 
248  /* adc up */
249  overo_link.up.msg.adc.channels[0] = adc0_buf.sum / adc0_buf.av_nb_sample;
250  overo_link.up.msg.adc.channels[1] = adc1_buf.sum / adc1_buf.av_nb_sample;
251  overo_link.up.msg.adc.channels[2] = adc2_buf.sum / adc2_buf.av_nb_sample;
252  overo_link.up.msg.adc.channels[3] = adc3_buf.sum / adc3_buf.av_nb_sample;
253  overo_link.up.msg.valid.adc = new_adc;
254  new_adc = FALSE;
255 
256 #ifdef PASSTHROUGH_CYGNUS
257  for (int i = 0; i < 6; i++) {
258  actuators_pwm_values[i] = overo_link.down.msg.pwm_outputs_usecs[i];
259  }
261 
262  for (int i = 6; i < 10; i++) {
263  csc_servo_cmd.servos[i-6] = overo_link.down.msg.pwm_outputs_usecs[i];
264  }
265  cscp_transmit(0, CSC_SERVO_CMD_ID, (uint8_t *)&csc_servo_cmd, sizeof(csc_servo_cmd));
266 #else
267  /* pwm acuators down */
268  if (radio_control.values[RADIO_MODE] <= 150) {
269  for (int i = 0; i < 6; i++) {
270  actuators_pwm_values[i] = overo_link.down.msg.pwm_outputs_usecs[i];
271  }
272  if (radio_control.values[RADIO_KILL] > 150) {
273  actuators[SERVO_THROTTLE] = SERVO_THROTTLE_MIN;
274  }
276  }
277 #endif
278 }
279 
280 static inline void on_overo_link_lost(void) {
281 }
282 
283 static inline void on_overo_link_crc_failed(void) {
284 }
285 
286 static inline void on_accel_event(void) {
287 }
288 
289 static inline void on_gyro_accel_event(void) {
290  ImuScaleGyro(imu);
292  overo_link.up.msg.imu_tick++;
293 
294 #ifdef PASSTHROUGH_CYGNUS
295  if (ahrs.status == AHRS_UNINIT) {
298  ahrs_align();
299  } else {
300  ahrs_propagate();
302  ins_propagate();
303  }
304 #endif
305 }
306 
307 static inline void on_mag_event(void) {
308  ImuScaleMag(imu);
309 
310 #ifdef PASSTHROUGH_CYGNUS
311  if (ahrs.status == AHRS_RUNNING)
312  ahrs_update_mag();
313 #endif
314 }
315 
316 static inline void on_vane_msg(void *data) {
317  new_vane = TRUE;
318  int zero = 0;
319  DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, DefaultDevice,
320  &(csc_vane_msg.vane_angle1),
321  &zero,
322  &zero,
323  &zero,
324  &zero,
325  &csc_vane_msg.vane_angle2,
326  &zero,
327  &zero,
328  &zero,
329  &zero);
330 }
331 
332 static inline void main_on_baro_diff(void) {
334 }
335 
336 static inline void main_on_baro_abs(void) {
337  new_baro_abs = TRUE;
338 }
339 
340 static inline void main_event(void) {
341 
346  cscp_event();
347 }
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:61
Attitude and Heading Reference System interface.
Common barometric sensor implementation.
void autopilot_init(void)
Autopilot inititalization.
Definition: autopilot.c:48
static struct adc_buf adc3_buf
#define RADIO_GEAR
Definition: spektrum_arch.h:42
static void on_accel_event(void)
void adc_buf_channel(uint8_t adc_channel, struct adc_buf *s, uint8_t av_nb_sample)
Registers a buffer to be used to store the specified converted channel Usage:
Definition: adc_arch.c:59
#define BaroEvent(_b_abs_handler, _b_diff_handler)
Definition: baro_board.h:55
void ahrs_propagate(void)
Propagation.
static bool_t new_baro_diff
int main(void)
void guidance_v_init(void)
Definition: guidance_v.c:97
static void on_overo_link_msg_received(void)
#define RADIO_KILL
Definition: joby_9ch.h:35
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:301
static void main_init(void)
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
static struct adc_buf adc1_buf
void actuators_pwm_commit(void)
Set pulse widths from actuator values, assumed to be in us.
uint16_t values[MAX_AV_NB_SAMPLE]
Definition: adc.h:62
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define ImuScaleMag(_imu)
Definition: ahrs_gx3.h:64
void imu_periodic(void)
Definition: imu_apogee.c:79
Integrated Navigation System interface.
void baro_init(void)
Definition: baro_board.c:42
void stabilization_init(void)
Definition: stabilization.c:30
uint8_t status
Definition: ahrs_aligner.h:45
uint8_t av_nb_sample
Definition: adc.h:64
void imu_init(void)
Definition: ins_xsens700.c:86
#define RADIO_AUX2
Definition: spektrum_arch.h:45
#define actuators
arch independent ADC (Analog to Digital Converter) API
uint32_t sum
Definition: adc.h:61
struct CscServoCmd csc_servo_cmd
#define FALSE
Definition: imu_chimu.h:141
static bool_t new_radio_msg
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
#define RADIO_PITCH
Definition: spektrum_arch.h:40
void baro_periodic(void)
Definition: baro_board.c:50
#define RADIO_AUX3
Definition: spektrum_arch.h:46
static void on_gyro_accel_event(void)
#define RadioControlEvent(_received_frame_handler)
Definition: dummy.h:28
int32_t absolute
Definition: baro.h:41
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:55
int32_t actuators_pwm_values[ACTUATORS_PWM_NB]
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
static void on_mag_event(void)
Architecture independent timing functions.
static void on_vane_msg(void *data)
int32_t differential
Definition: baro.h:42
struct CscVaneMsg csc_vane_msg
static void check_radio_lost(void)
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
#define ImuScaleAccel(_imu)
Definition: ahrs_gx3.h:59
Generic interface for all ADC hardware drivers, independent from microcontroller architecture.
Definition: adc.h:60
static void radio_control_init(void)
Definition: radio_control.h:62
#define AHRS_ALIGNER_LOCKED
Definition: ahrs_aligner.h:37
#define SetActuatorsFromCommands(commands, AP_MODE)
Definition: actuators_at.h:34
static bool_t new_vane
static void on_rc_message(void)
void autopilot_on_rc_frame(void)
Definition: autopilot.c:309
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:25
Hardware independent API for actuators (servos, motor controllers).
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler)
Definition: ins_xsens.h:63
Inertial Measurement Unit interface.
static void main_on_baro_diff(void)
int sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:37
#define RADIO_YAW
Definition: spektrum_arch.h:41
static bool_t sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:111
static void main_on_baro_abs(void)
void autopilot_periodic(void)
Definition: autopilot.c:141
signed long int32_t
Definition: types.h:19
#define RC_REALLY_LOST
Definition: radio_control.h:47
#define AHRS_UNINIT
Definition: ahrs.h:35
#define TRUE
Definition: imu_chimu.h:144
uint8_t status
Definition: radio_control.h:50
struct Int32Vect3 mag
magnetometer measurements
Definition: imu.h:42
#define RADIO_MODE
Definition: spektrum_arch.h:61
#define RADIO_THROTTLE
Definition: spektrum_arch.h:38
struct Baro baro
Definition: baro_board.c:36
uint8_t adc_new_data_trigger
Definition: adc_arch.c:104
#define LED_PERIODIC()
Definition: led_hw.h:8
Arch independent mcu ( Micro Controller Unit ) utilities.
void ins_propagate()
Propagation.
Definition: ins_alt_float.c:84
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
static void main_event(void)
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
General stabilization interface for rotorcrafts.
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
void ins_init(void)
INS initialization.
Definition: ins_xsens700.c:178
#define PERIODIC_FREQUENCY
Definition: imu_aspirin2.c:54
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:111
void actuators_init(void)
Definition: actuators_at.c:32
static bool_t new_adc
static struct adc_buf adc0_buf
#define RADIO_ROLL
Definition: spektrum_arch.h:39
#define ActuatorsCommit()
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:45
void ahrs_align(void)
Aligns the AHRS.
static bool_t new_baro_abs
void ahrs_aligner_run(void)
Definition: ahrs_aligner.c:63
void guidance_h_init(void)
Definition: guidance_h.c:93
static void main_periodic(void)
#define ImuScaleGyro(_imu)
Definition: ahrs_gx3.h:54
#define AHRS_RUNNING
Definition: ahrs.h:36
static struct adc_buf adc2_buf
static void on_overo_link_crc_failed(void)
void ahrs_init(void)
AHRS initialization.
static void on_overo_link_lost(void)