Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-06 02:52:42 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

[rotwing] preflight check for skew angle (#3507)

Browse Source
This commit is contained in:
NoahWe
2025-08-28 10:30:27 +02:00
committed by GitHub
parent c396d38321
commit 904eefb0fe
1 changed files with 20 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
sw/airborne/modules/rotwing_drone/rotwing_state.c
View File

@@ -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);