diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c index 9586dd353f..d547dec3c8 100644 --- a/sw/airborne/lisa/lisa_stm_passthrough_main.c +++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c @@ -1,7 +1,7 @@ /* * $Id$ * - * Copyright (C) 2010 Piotr Esden-Tempski + * Copyright (C) 2010 The Paparazzi Team * * This file is part of paparazzi. * @@ -27,8 +27,8 @@ #include "booz/booz2_commands.h" #include "booz/booz_actuators.h" #include "booz/booz_imu.h" -#include "booz_radio_control.h" -#include "actuators/booz_actuators_pwm.h" +#include "booz/booz_radio_control.h" +#include "booz/actuators/booz_actuators_pwm.h" #include "lisa/lisa_overo_link.h" static inline void main_init(void); @@ -61,17 +61,18 @@ int main(void) { static inline void main_init(void) { - hw_init(); - sys_time_init(); - booz_imu_init(); - radio_control_init(); - booz_actuators_pwm_hw_init(); - overo_link_init(); - new_radio_msg = FALSE; + hw_init(); + sys_time_init(); + booz_imu_init(); + radio_control_init(); + booz_actuators_pwm_hw_init(); + overo_link_init(); + new_radio_msg = FALSE; + } static inline void main_periodic(void) { - + booz_imu_periodic(); OveroLinkPeriodic(on_overo_link_lost); RunOnceEvery(10, { @@ -83,10 +84,10 @@ static inline void main_periodic(void) { } static inline void main_event(void) { - - BoozImuEvent(on_gyro_accel_event, on_mag_event); - OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed); - RadioControlEvent(on_rc_message); + + BoozImuEvent(on_gyro_accel_event, on_mag_event); + OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed); + RadioControlEvent(on_rc_message); } @@ -96,35 +97,38 @@ static inline void on_rc_message(void) { static inline void on_overo_link_msg_received(void) { - if (new_radio_msg) overo_link.up.msg.valid.rc = 1; - else overo_link.up.msg.valid.rc = 0; - new_radio_msg = FALSE; - - overo_link.up.msg.valid.imu = 1; - - RATES_COPY(overo_link.up.msg.gyro, booz_imu.gyro); - - VECT3_COPY(overo_link.up.msg.accel, booz_imu.accel); - - VECT3_COPY(overo_link.up.msg.mag, booz_imu.mag); - - overo_link.up.msg.rc_pitch = radio_control.values[RADIO_CONTROL_PITCH]; - overo_link.up.msg.rc_roll = radio_control.values[RADIO_CONTROL_ROLL]; - overo_link.up.msg.rc_yaw = radio_control.values[RADIO_CONTROL_YAW]; - overo_link.up.msg.rc_thrust = radio_control.values[RADIO_CONTROL_THROTTLE]; - overo_link.up.msg.rc_mode = radio_control.values[RADIO_CONTROL_MODE]; - overo_link.up.msg.rc_kill = radio_control.values[RADIO_CONTROL_KILL]; - overo_link.up.msg.rc_gear = radio_control.values[RADIO_CONTROL_GEAR]; - overo_link.up.msg.rc_aux3 = radio_control.values[RADIO_CONTROL_AUX3]; - overo_link.up.msg.rc_aux4 = radio_control.values[RADIO_CONTROL_AUX4]; - overo_link.up.msg.rc_status = radio_control.status; - - overo_link.up.msg.stm_msg_cnt = overo_link.msg_cnt; - overo_link.up.msg.stm_crc_err_cnt = overo_link.crc_err_cnt; - - for (int i = 0; i < LISA_PWM_OUTPUT_NB; i++) - booz_actuators_pwm_values[i] = overo_link.down.msg.pwm_outputs_usecs[i]; - booz_actuators_pwm_commit(); + overo_link.up.msg.valid.rc = new_radio_msg; + new_radio_msg = FALSE; + + overo_link.up.msg.valid.imu = 1; + + RATES_COPY(overo_link.up.msg.gyro, booz_imu.gyro); + + VECT3_COPY(overo_link.up.msg.accel, booz_imu.accel); + + VECT3_COPY(overo_link.up.msg.mag, booz_imu.mag); + + overo_link.up.msg.rc_pitch = radio_control.values[RADIO_CONTROL_PITCH]; + overo_link.up.msg.rc_roll = radio_control.values[RADIO_CONTROL_ROLL]; + overo_link.up.msg.rc_yaw = radio_control.values[RADIO_CONTROL_YAW]; + overo_link.up.msg.rc_thrust = radio_control.values[RADIO_CONTROL_THROTTLE]; + overo_link.up.msg.rc_mode = radio_control.values[RADIO_CONTROL_MODE]; +#ifdef RADIO_CONTROL_KILL + overo_link.up.msg.rc_kill = radio_control.values[RADIO_CONTROL_KILL]; +#endif +#ifdef RADIO_CONTROL_GEAR + overo_link.up.msg.rc_gear = radio_control.values[RADIO_CONTROL_GEAR]; +#endif + overo_link.up.msg.rc_aux3 = radio_control.values[RADIO_CONTROL_AUX3]; + overo_link.up.msg.rc_aux4 = radio_control.values[RADIO_CONTROL_AUX4]; + overo_link.up.msg.rc_status = radio_control.status; + + overo_link.up.msg.stm_msg_cnt = overo_link.msg_cnt; + overo_link.up.msg.stm_crc_err_cnt = overo_link.crc_err_cnt; + + for (int i = 0; i < LISA_PWM_OUTPUT_NB; i++) + booz_actuators_pwm_values[i] = overo_link.down.msg.pwm_outputs_usecs[i]; + booz_actuators_pwm_commit(); } static inline void on_overo_link_lost(void) {