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