diff --git a/sw/airborne/fms/fms_autopilot_msg.h b/sw/airborne/fms/fms_autopilot_msg.h index 79bacecc4a..7c90141c81 100644 --- a/sw/airborne/fms/fms_autopilot_msg.h +++ b/sw/airborne/fms/fms_autopilot_msg.h @@ -55,6 +55,16 @@ struct __attribute__ ((packed)) AutopilotMessageTWDown /* * Passthrough, aka biplan */ + +/* used to indicate parts of the message which actually represent a new measurement */ +struct PTUpValidFlags +{ + unsigned rc:1; + unsigned pressure:1; + unsigned vane:1; + unsigned imu:1; +}; + struct __attribute__ ((packed)) AutopilotMessagePTUp { struct Int32Rates gyro; @@ -70,6 +80,7 @@ struct __attribute__ ((packed)) AutopilotMessagePTUp int16_t rc_aux3; int16_t rc_aux4; uint8_t rc_status; + struct PTUpValidFlags valid; }; struct __attribute__ ((packed)) AutopilotMessagePTDown diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.c b/sw/airborne/fms/fms_spi_autopilot_msg.c index 71a31c14ef..3f2fb62382 100644 --- a/sw/airborne/fms/fms_spi_autopilot_msg.c +++ b/sw/airborne/fms/fms_spi_autopilot_msg.c @@ -97,11 +97,12 @@ int spi_ap_link_init() static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up) { - // FIXME: Should only callback when there is something new - // FIXME: placeholders since the vane and pressure data fields don't exist yet - if (vane_callback) + + if (msg_up->valid.vane && vane_callback) + // FIXME: placeholders since the vane and pressure data fields don't exist yet vane_callback(0, 0., 0.); - if (pressure_callback) + + if (msg_up->valid.pressure && pressure_callback) pressure_callback(0, 0, 0); // Fill radio data @@ -116,7 +117,7 @@ static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up) radio_control.values[RADIO_CONTROL_AUX4] = msg_up->rc_aux4; radio_control.status = msg_up->rc_status; - if (radio_control_callback) + if (msg_up->valid.rc && radio_control_callback) radio_control_callback(); // Fill IMU data @@ -132,7 +133,8 @@ static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up) imu.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y); imu.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z); - rdyb_booz_imu_update(&imu); + if (msg_up->valid.imu) + rdyb_booz_imu_update(&imu); } static void passthrough_down_fill(struct AutopilotMessagePTDown *msg_out) @@ -144,8 +146,8 @@ static void passthrough_down_fill(struct AutopilotMessagePTDown *msg_out) void spi_ap_link_periodic() { - struct AutopilotMessagePTUp msg_in; - struct AutopilotMessagePTDown msg_out; + static struct AutopilotMessagePTUp msg_in; + static struct AutopilotMessagePTDown msg_out; passthrough_down_fill(&msg_out); diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c index ad46828a55..64728f55d3 100644 --- a/sw/airborne/lisa/lisa_stm_passthrough_main.c +++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c @@ -41,6 +41,8 @@ static inline void on_mag_event(void); static inline void main_on_overo_msg_received(void); static inline void main_on_overo_link_lost(void); +static bool_t new_radio_msg; + int main(void) { main_init(); @@ -54,6 +56,10 @@ int main(void) { return 0; } +static void on_rc_message(void) { + new_radio_msg = TRUE; +} + static inline void main_init(void) { hw_init(); @@ -75,10 +81,17 @@ static inline void main_event(void) { BoozImuEvent(on_gyro_accel_event, on_mag_event); OveroLinkEvent(main_on_overo_msg_received); - RadioControlEvent(NULL); + RadioControlEvent(on_rc_message); } static inline void main_on_overo_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);