diff --git a/sw/airborne/modules/rotwing_drone/rotwing_state.c b/sw/airborne/modules/rotwing_drone/rotwing_state.c index 1c69f112d3..ca03850eff 100644 --- a/sw/airborne/modules/rotwing_drone/rotwing_state.c +++ b/sw/airborne/modules/rotwing_drone/rotwing_state.c @@ -150,6 +150,20 @@ static void send_rotating_wing_state(struct transport_tx *trans, struct link_dev } #endif // PERIODIC_TELEMETRY +#if PREFLIGHT_CHECKS +/* Preflight checks */ +#include "modules/checks/preflight_checks.h" +static struct preflight_check_t rotwing_state_skew_pfc; + +static void rotwing_state_skew_preflight(struct preflight_result_t *result) { + if(fabsf(rotwing_state.meas_skew_angle_deg - rotwing_state.sp_skew_angle_deg) < ROTWING_SKEW_REF_MODEL_MAX_DIFF) { + preflight_success(result, "Rotwing skew success"); + } else { + preflight_error(result, "Rotwing skew error [meas: %.1f, sp: %.1f]", rotwing_state.meas_skew_angle_deg, rotwing_state.sp_skew_angle_deg); + } +} +#endif // PREFLIGHT_CHECKS + void rotwing_state_init(void) { // Initialize rotwing state @@ -177,6 +191,11 @@ void rotwing_state_init(void) #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTATING_WING_STATE, send_rotating_wing_state); #endif + + /* Register preflight checks */ +#if PREFLIGHT_CHECKS + preflight_check_register(&rotwing_state_skew_pfc, rotwing_state_skew_preflight); +#endif } /** @@ -310,7 +329,7 @@ void rotwing_state_periodic(void) } /* Bound max bank angle, climb speed and descend speed if the rotwing drone is transitioning and not in FREE mode */ - if (rotwing_state.meas_skew_angle_deg < ROTWING_FW_SKEW_ANGLE && rotwing_state.meas_skew_angle_deg > ROTWING_QUAD_SKEW_ANGLE + if (meas_skew_angle < ROTWING_FW_SKEW_ANGLE && meas_skew_angle > ROTWING_QUAD_SKEW_ANGLE && rotwing_state.state != ROTWING_STATE_FREE) { guidance_set_max_bank_angle(ROTWING_TRANSITION_MAX_BANK); guidance_set_max_climb_speed(ROTWING_TRANSITION_MAX_CLIMB_SPEED, ROTWING_TRANSITION_MAX_CLIMB_SPEED);