From d748af1afb03f05178a76348676a85ab2b5b5fa7 Mon Sep 17 00:00:00 2001 From: Dennis-Wijngaarden <32736330+Dennis-Wijngaarden@users.noreply.github.com> Date: Thu, 23 May 2024 21:55:24 +0200 Subject: [PATCH] Rotwing guidance bank fix (#3271) * max bank in deg * Fix allocation switching problem * [rot_wing_v3] Updated max bank in all airframes * Update sw/airborne/modules/checks/preflight_checks.c Co-authored-by: Gautier Hattenberger * use unit and alt_unit for conversions instead * remove needless conversion * Also fix normal INDI to fit the code --------- Co-authored-by: Ewoud Smeur Co-authored-by: Christophe De Wagter Co-authored-by: Gautier Hattenberger --- conf/airframes/tudelft/rot_wing_25kg.xml | 13 ++++++---- conf/airframes/tudelft/rot_wing_v3b.xml | 7 ++++-- conf/airframes/tudelft/rot_wing_v3d.xml | 7 ++++-- conf/airframes/tudelft/rot_wing_v3e.xml | 7 ++++-- conf/airframes/tudelft/rot_wing_v3f.xml | 7 ++++-- conf/airframes/tudelft/rot_wing_v3g.xml | 7 ++++-- conf/airframes/tudelft/rot_wing_v3h.xml | 7 ++++-- conf/modules/guidance_indi.xml | 2 +- conf/modules/guidance_indi_hybrid.xml | 2 +- .../guidance/guidance_indi_hybrid.c | 18 +++++++++---- .../guidance/guidance_indi_hybrid_quadplane.c | 7 +++--- sw/airborne/modules/checks/preflight_checks.c | 7 +++++- .../modules/rot_wing_drone/rotwing_state.c | 25 +++++-------------- sw/airborne/state.h | 2 +- 14 files changed, 70 insertions(+), 48 deletions(-) diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml index 0d9ed4e06b..bca40f5bd8 100644 --- a/conf/airframes/tudelft/rot_wing_25kg.xml +++ b/conf/airframes/tudelft/rot_wing_25kg.xml @@ -56,6 +56,8 @@ + + @@ -178,13 +180,13 @@ - + - + @@ -253,7 +255,7 @@ - + @@ -369,7 +371,7 @@ - + @@ -428,7 +430,7 @@ - + @@ -455,6 +457,7 @@ +
diff --git a/conf/airframes/tudelft/rot_wing_v3b.xml b/conf/airframes/tudelft/rot_wing_v3b.xml index 19595a8d0e..84383bff38 100644 --- a/conf/airframes/tudelft/rot_wing_v3b.xml +++ b/conf/airframes/tudelft/rot_wing_v3b.xml @@ -67,6 +67,8 @@ + + @@ -465,7 +467,7 @@
- + @@ -479,7 +481,7 @@ - + @@ -505,6 +507,7 @@ +
diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml index e83af977d7..e525d6a5bf 100644 --- a/conf/airframes/tudelft/rot_wing_v3d.xml +++ b/conf/airframes/tudelft/rot_wing_v3d.xml @@ -54,6 +54,8 @@ + + @@ -452,7 +454,7 @@
- + @@ -466,7 +468,7 @@ - + @@ -492,6 +494,7 @@ +
diff --git a/conf/airframes/tudelft/rot_wing_v3e.xml b/conf/airframes/tudelft/rot_wing_v3e.xml index 83fd58f37d..1499cab034 100644 --- a/conf/airframes/tudelft/rot_wing_v3e.xml +++ b/conf/airframes/tudelft/rot_wing_v3e.xml @@ -54,6 +54,8 @@ + + @@ -452,7 +454,7 @@
- + @@ -466,7 +468,7 @@ - + @@ -492,6 +494,7 @@ +
diff --git a/conf/airframes/tudelft/rot_wing_v3f.xml b/conf/airframes/tudelft/rot_wing_v3f.xml index 283444ae89..dc84eac2fa 100644 --- a/conf/airframes/tudelft/rot_wing_v3f.xml +++ b/conf/airframes/tudelft/rot_wing_v3f.xml @@ -54,6 +54,8 @@ + + @@ -452,7 +454,7 @@
- + @@ -466,7 +468,7 @@ - + @@ -492,6 +494,7 @@ +
diff --git a/conf/airframes/tudelft/rot_wing_v3g.xml b/conf/airframes/tudelft/rot_wing_v3g.xml index bee1435746..446319182f 100644 --- a/conf/airframes/tudelft/rot_wing_v3g.xml +++ b/conf/airframes/tudelft/rot_wing_v3g.xml @@ -54,6 +54,8 @@ + + @@ -452,7 +454,7 @@
- + @@ -466,7 +468,7 @@ - + @@ -492,6 +494,7 @@ +
diff --git a/conf/airframes/tudelft/rot_wing_v3h.xml b/conf/airframes/tudelft/rot_wing_v3h.xml index 07d505622c..04590b43cb 100644 --- a/conf/airframes/tudelft/rot_wing_v3h.xml +++ b/conf/airframes/tudelft/rot_wing_v3h.xml @@ -54,6 +54,8 @@ + + @@ -452,7 +454,7 @@
- + @@ -466,7 +468,7 @@ - + @@ -492,6 +494,7 @@ +
diff --git a/conf/modules/guidance_indi.xml b/conf/modules/guidance_indi.xml index 766522ed4a..038ef97457 100644 --- a/conf/modules/guidance_indi.xml +++ b/conf/modules/guidance_indi.xml @@ -11,7 +11,7 @@ - + diff --git a/conf/modules/guidance_indi_hybrid.xml b/conf/modules/guidance_indi_hybrid.xml index f15184c502..ffc50824b8 100644 --- a/conf/modules/guidance_indi_hybrid.xml +++ b/conf/modules/guidance_indi_hybrid.xml @@ -25,7 +25,7 @@ - + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c index 80c3e0d6a4..e68ae9dc97 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c @@ -162,6 +162,10 @@ static void guidance_indi_filter_thrust(void); #define GUIDANCE_INDI_DESCEND_SPEED_FWD -4.0 #endif +#ifndef GUIDANCE_INDI_MAX_LAT_ACCEL +#define GUIDANCE_INDI_MAX_LAT_ACCEL 9.81 +#endif + float climb_vspeed_fwd = GUIDANCE_INDI_CLIMB_SPEED_FWD; float descend_vspeed_fwd = GUIDANCE_INDI_DESCEND_SPEED_FWD; @@ -188,19 +192,21 @@ struct FloatVect2 desired_airspeed; float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U]; struct FloatVect3 euler_cmd; +float du_gih[GUIDANCE_INDI_HYBRID_U]; // = {0.0f, 0.0f, 0.0f}; + #if GUIDANCE_INDI_HYBRID_USE_WLS #include "math/wls/wls_alloc.h" float du_min_gih[GUIDANCE_INDI_HYBRID_U]; float du_max_gih[GUIDANCE_INDI_HYBRID_U]; float du_pref_gih[GUIDANCE_INDI_HYBRID_U]; float *Bwls_gih[GUIDANCE_INDI_HYBRID_V]; -#ifdef GUIDANCE_INDI_HYBRID_WLS_PRIORITIES -float Wv_gih[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_HYBRID_WLS_PRIORITIES; +#ifdef GUIDANCE_INDI_WLS_PRIORITIES +float Wv_gih[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES; #else float Wv_gih[GUIDANCE_INDI_HYBRID_V] = { 100.f, 100.f, 1.f }; // X,Y accel, Z accel #endif -#ifdef GUIDANCE_INDI_HYBRID_WLS_WU -float Wu_gih[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_HYBRID_WLS_WU; +#ifdef GUIDANCE_INDI_WLS_WU +float Wu_gih[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU; #else float Wu_gih[GUIDANCE_INDI_HYBRID_U] = { 1.f, 1.f, 1.f }; #endif @@ -319,7 +325,7 @@ void guidance_indi_init(void) init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, 0.0); init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0); init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0); - + float tau_guidance_indi_airspeed = 1.0/(2.0*M_PI*guidance_indi_airspeed_filt_cutoff); init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0); @@ -640,6 +646,8 @@ static struct FloatVect3 compute_accel_from_speed_sp(void) FLOAT_ANGLE_NORMALIZE(sp_accel_b.y); sp_accel_b.y *= gih_params.heading_bank_gain; + BoundAbs(sp_accel_b.y, GUIDANCE_INDI_MAX_LAT_ACCEL); + // Control the airspeed sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c index eed4cb0829..061e5a99ff 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c @@ -117,6 +117,8 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H Gmat[0][3] = ctheta; Gmat[1][3] = 0; Gmat[2][3] = -stheta; + // Make this term zero to prevent switching 'exploits' + // Gmat[2][3] = 0; // Convert acceleration error to body axis system body_v[0] = cpsi * a_diff.x + spsi * a_diff.y; @@ -127,20 +129,19 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle) { - float roll_limit_rad = GUIDANCE_H_MAX_BANK; float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH); float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH); float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg); // Set lower limits - du_min_gih[0] = -roll_limit_rad - roll_angle; //roll + du_min_gih[0] = -guidance_indi_max_bank - roll_angle; //roll du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch du_min_gih[2] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff; du_min_gih[3] = -stabilization.cmd[COMMAND_THRUST_X]*guidance_indi_thrust_x_eff; // Set upper limits limits - du_max_gih[0] = roll_limit_rad - roll_angle; //roll + du_max_gih[0] = guidance_indi_max_bank - roll_angle; //roll du_max_gih[1] = max_pitch_limit_rad - pitch_angle; // pitch du_max_gih[2] = -stabilization.cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff; du_max_gih[3] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST_X])*guidance_indi_thrust_x_eff; diff --git a/sw/airborne/modules/checks/preflight_checks.c b/sw/airborne/modules/checks/preflight_checks.c index f6b12ebbf2..8c0f12e834 100644 --- a/sw/airborne/modules/checks/preflight_checks.c +++ b/sw/airborne/modules/checks/preflight_checks.c @@ -43,8 +43,13 @@ #define PREFLIGHT_CHECK_INFO_TIMEOUT 5 #endif +#ifndef PREFLIGHT_CHECK_BYPASS +#define PREFLIGHT_CHECK_BYPASS FALSE +#endif + +bool preflight_bypass = PREFLIGHT_CHECK_BYPASS; + static struct preflight_check_t *preflight_head = NULL; -bool preflight_bypass = FALSE; /** * @brief Register a preflight check and add it to the linked list diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state.c b/sw/airborne/modules/rot_wing_drone/rotwing_state.c index 9651c8fc5b..f3c295e3b2 100644 --- a/sw/airborne/modules/rot_wing_drone/rotwing_state.c +++ b/sw/airborne/modules/rot_wing_drone/rotwing_state.c @@ -63,7 +63,7 @@ // FW state identification #ifndef ROTWING_MIN_FW_SKEW_ANGLE_DEG -#define ROTWING_MIN_FW_SKEW_ANGLE_DEG 80.0 // Minimum wing angle to fly in fixed wing state +#define ROTWING_MIN_FW_SKEW_ANGLE_DEG 80.0 // Minimum wing angle to fly in fixed wing state #endif #ifndef ROTWING_MIN_FW_COUNTER #define ROTWING_MIN_FW_COUNTER 10 // Minimum number of loops the skew angle is above the MIN_FW_SKEW_ANGLE @@ -710,20 +710,6 @@ static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id, void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle) { - float pitch_priority_factor = 11.; - float roll_priority_factor = 10.; - float thrust_priority_factor = 7.; - float pusher_priority_factor = 30.; - - float horizontal_accel_weight = 10.; - float vertical_accel_weight = 10.; - - // Set weights - Wu_gih[0] = roll_priority_factor * 10.414; - Wu_gih[1] = pitch_priority_factor * 27.53; - Wu_gih[2] = thrust_priority_factor * 0.626; - Wu_gih[3] = pusher_priority_factor * 1.0; - // adjust weights float thrust_command = (actuator_state_filt_vect[0] + actuator_state_filt_vect[1] + actuator_state_filt_vect[2] + actuator_state_filt_vect[3]) / 4; @@ -732,10 +718,11 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl Bound(fixed_wing_percentage, 0, 1); #define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16 - Wv_gih[0] = horizontal_accel_weight * (1.0f + fixed_wing_percentage * + float Wv_original[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES; + + // Increase importance of forward acceleration in forward flight + Wv_gih[0] = Wv_original[0] * (1.0f + fixed_wing_percentage * AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight) - Wv_gih[1] = horizontal_accel_weight; - Wv_gih[2] = vertical_accel_weight; struct FloatEulers eulers_zxy; float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); @@ -761,7 +748,7 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl actuator_state_filt_vect[2] * g1g2[3][2] + actuator_state_filt_vect[3] * g1g2[3][3]); Bound(du_max_thrust_z, 0., 50.); - float roll_limit_rad = 2.0; // big roll limit hacked in to overcome wls problems at roll limit + float roll_limit_rad = guidance_indi_max_bank; float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH); float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH); diff --git a/sw/airborne/state.h b/sw/airborne/state.h index 091994c0b4..53259bbf82 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -332,7 +332,7 @@ struct State { uint8_t accel_status; /** - * Acceleration in North East Down coordinates. + * Acceleration in Body coordinates. * Units: m/s^2 in BFP with #INT32_ACCEL_FRAC */ struct Int32Vect3 body_accel_i;