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;