diff --git a/conf/airframes/tudelft/rot_wing_v3b.xml b/conf/airframes/tudelft/rot_wing_v3b.xml
index f4bb6c01c34..efd8f9f5b7c 100644
--- a/conf/airframes/tudelft/rot_wing_v3b.xml
+++ b/conf/airframes/tudelft/rot_wing_v3b.xml
@@ -203,21 +203,21 @@
-
-
-
-
-
+
+
+
+
+
-
-
-
-
+
+
+
+
@@ -297,11 +297,11 @@
-
-
+
+
-
-
+
+
@@ -311,7 +311,7 @@
-
+
@@ -367,7 +367,7 @@
-
+
@@ -437,7 +437,7 @@
-
+
diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml
index 9960eb2464c..b0b6310fb1a 100644
--- a/conf/airframes/tudelft/rot_wing_v3d.xml
+++ b/conf/airframes/tudelft/rot_wing_v3d.xml
@@ -288,21 +288,21 @@
-
-
+
+
-
-
+
+
-
+
-
+
@@ -358,7 +358,7 @@
-
+
@@ -429,7 +429,7 @@
-
+
diff --git a/conf/airframes/tudelft/rot_wing_v3e.xml b/conf/airframes/tudelft/rot_wing_v3e.xml
index eebe8af3ed7..8da6f7cb579 100644
--- a/conf/airframes/tudelft/rot_wing_v3e.xml
+++ b/conf/airframes/tudelft/rot_wing_v3e.xml
@@ -187,11 +187,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -203,10 +203,10 @@
-
-
-
-
+
+
+
+
@@ -288,21 +288,21 @@
-
-
+
+
-
-
+
+
-
+
-
+
@@ -358,7 +358,7 @@
-
+
@@ -429,7 +429,7 @@
-
+
diff --git a/conf/airframes/tudelft/rot_wing_v3f.xml b/conf/airframes/tudelft/rot_wing_v3f.xml
index fdbeeb60c85..e2e8c43792f 100644
--- a/conf/airframes/tudelft/rot_wing_v3f.xml
+++ b/conf/airframes/tudelft/rot_wing_v3f.xml
@@ -187,11 +187,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -203,10 +203,10 @@
-
-
-
-
+
+
+
+
@@ -288,21 +288,21 @@
-
-
+
+
-
-
+
+
-
+
-
+
@@ -358,7 +358,7 @@
-
+
@@ -429,7 +429,7 @@
-
+
diff --git a/conf/airframes/tudelft/rot_wing_v3g.xml b/conf/airframes/tudelft/rot_wing_v3g.xml
index a25894af9c2..c9b00fe7b36 100644
--- a/conf/airframes/tudelft/rot_wing_v3g.xml
+++ b/conf/airframes/tudelft/rot_wing_v3g.xml
@@ -187,11 +187,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -203,10 +203,10 @@
-
-
-
-
+
+
+
+
@@ -288,21 +288,21 @@
-
-
+
+
-
-
+
+
-
+
-
+
@@ -358,7 +358,7 @@
-
+
@@ -429,7 +429,7 @@
-
+
diff --git a/conf/airframes/tudelft/rot_wing_v3h.xml b/conf/airframes/tudelft/rot_wing_v3h.xml
index e88138d7b4f..ec146109b06 100644
--- a/conf/airframes/tudelft/rot_wing_v3h.xml
+++ b/conf/airframes/tudelft/rot_wing_v3h.xml
@@ -187,12 +187,12 @@
-
-
-
-
-
-
+
+
+
+
+
+
@@ -203,11 +203,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -288,21 +288,21 @@
-
-
+
+
-
-
+
+
-
+
-
+
@@ -358,7 +358,7 @@
-
+
@@ -429,7 +429,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/rotating_wing_EHVB.xml b/conf/flight_plans/tudelft/rotating_wing_EHVB.xml
index ed0345bb60d..be1e0a0ab99 100644
--- a/conf/flight_plans/tudelft/rotating_wing_EHVB.xml
+++ b/conf/flight_plans/tudelft/rotating_wing_EHVB.xml
@@ -9,14 +9,14 @@
-
+
-
-
+
+
@@ -90,10 +90,14 @@
-
+
+
+
+
+
@@ -122,22 +126,22 @@
-
+
+
@@ -146,24 +150,24 @@
-
+
-
+
-
+
diff --git a/conf/modules/stabilization_indi.xml b/conf/modules/stabilization_indi.xml
index 6706cf80cd9..f18cc8bfbaf 100644
--- a/conf/modules/stabilization_indi.xml
+++ b/conf/modules/stabilization_indi.xml
@@ -61,6 +61,7 @@
+
diff --git a/conf/simulator/jsbsim/aircraft/rotwing3.xml b/conf/simulator/jsbsim/aircraft/rotwing3.xml
index 6b827ace1a6..eb5e3601532 100644
--- a/conf/simulator/jsbsim/aircraft/rotwing3.xml
+++ b/conf/simulator/jsbsim/aircraft/rotwing3.xml
@@ -917,7 +917,7 @@
2.45
fcs/rudder_lag
0.7376
- 0.002
+ 0.01
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
index 974776d4da5..7d906bf89c0 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -105,9 +105,9 @@
#endif
#endif
-float du_min_stab_indi[INDI_NUM_ACT];
-float du_max_stab_indi[INDI_NUM_ACT];
-float du_pref_stab_indi[INDI_NUM_ACT];
+float u_min_stab_indi[INDI_NUM_ACT];
+float u_max_stab_indi[INDI_NUM_ACT];
+float u_pref_stab_indi[INDI_NUM_ACT];
float indi_v[INDI_OUTPUTS];
float *Bwls[INDI_OUTPUTS];
int num_iter = 0;
@@ -197,6 +197,15 @@ float indi_Wu[INDI_NUM_ACT] = STABILIZATION_INDI_WLS_WU;
float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT - 1] = 1.0};
#endif
+/**
+ * Limit the maximum specific moment that can be compensated (units rad/s^2)
+*/
+#ifdef STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT
+float stablization_indi_yaw_dist_limit = STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT
+#else // Put a rediculously high limit
+float stablization_indi_yaw_dist_limit = 99999.f;
+#endif
+
// variables needed for control
float actuator_state_filt_vect[INDI_NUM_ACT];
struct FloatRates angular_accel_ref = {0., 0., 0.};
@@ -204,8 +213,7 @@ struct FloatRates angular_rate_ref = {0., 0., 0.};
float angular_acceleration[3] = {0., 0., 0.};
float actuator_state[INDI_NUM_ACT];
float indi_u[INDI_NUM_ACT];
-float indi_du[INDI_NUM_ACT];
-float g2_times_du;
+float rate_vect_prev[3] = {0., 0., 0.};
float q_filt = 0.0;
float r_filt = 0.0;
@@ -426,6 +434,8 @@ void stabilization_indi_enter(void)
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
+ float_vect_zero(rate_vect_prev, 3);
+
float_vect_zero(du_estimation, INDI_NUM_ACT);
float_vect_zero(ddu_estimation, INDI_NUM_ACT);
}
@@ -554,6 +564,31 @@ void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
*/
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
{
+
+ // Propagate actuator filters
+ get_actuator_state();
+ float actuator_state_filt_vect_prev[INDI_NUM_ACT];
+ for (int i = 0; i < INDI_NUM_ACT; i++) {
+ update_butterworth_2_low_pass(&actuator_lowpass_filters[i], actuator_state[i]);
+ update_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], actuator_state[i]);
+ actuator_state_filt_vect[i] = actuator_lowpass_filters[i].o[0];
+ actuator_state_filt_vect_prev[i] = actuator_lowpass_filters[i].o[1];
+
+ // calculate derivatives for estimation
+ float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
+ actuator_state_filt_vectd[i] = (estimation_input_lowpass_filters[i].o[0] - estimation_input_lowpass_filters[i].o[1]) *
+ PERIODIC_FREQUENCY;
+ actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
+ }
+
+ // Use the last actuator state for this computation
+ float g2_times_u_act_filt = float_vect_dot_product(g2, actuator_state_filt_vect_prev, INDI_NUM_ACT)/INDI_G_SCALING;
+
+ // Predict angular acceleration u*B
+ float angular_acc_prediction_filt[INDI_OUTPUTS];
+ float_mat_vect_mul(angular_acc_prediction_filt, Bwls, actuator_state_filt_vect, INDI_OUTPUTS, INDI_NUM_ACT);
+ angular_acc_prediction_filt[2] -= g2_times_u_act_filt;
+
/* Propagate the filter on the gyroscopes */
struct FloatRates *body_rates = stateGetBodyRates_f();
float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
@@ -573,6 +608,10 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
}
+ // subtract u*B from angular acceleration
+ float angular_acc_disturbance_estimate[INDI_OUTPUTS];
+ float_vect_diff(angular_acc_disturbance_estimate, angular_acceleration, angular_acc_prediction_filt, 3);
+
//The rates used for feedback are by default the measured rates.
//If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
//Note that due to the delay, the PD controller may need relaxed gains.
@@ -610,89 +649,83 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi_gains.rate.q;
angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi_gains.rate.r;
- g2_times_du = 0.0;
- for (i = 0; i < INDI_NUM_ACT; i++) {
- g2_times_du += g2[i] * indi_du[i];
- }
- //G2 is scaled by INDI_G_SCALING to make it readable
- g2_times_du = g2_times_du / INDI_G_SCALING;
-
- float use_increment = 0.0;
- if (in_flight) {
- use_increment = 1.0;
- }
-
struct FloatVect3 v_thrust;
v_thrust.x = 0.0;
v_thrust.y = 0.0;
v_thrust.z = 0.0;
if (indi_thrust_increment_set) {
- v_thrust = indi_thrust_increment;
-
//update thrust command such that the current is correctly estimated
stabilization_cmd[COMMAND_THRUST] = 0;
+ float current_thrust_filt_z = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_is_thruster_z[i];
+ current_thrust_filt_z += Bwls[3][i]* actuator_lowpass_filters[i].o[0] * (int32_t) act_is_thruster_z[i];
}
stabilization_cmd[COMMAND_THRUST] /= num_thrusters;
+ // Add the increment to the current estimated thrust
+ v_thrust.z = current_thrust_filt_z + indi_thrust_increment.z;
+
#if INDI_OUTPUTS == 5
stabilization_cmd[COMMAND_THRUST_X] = 0;
+ float current_thrust_filt_x = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
stabilization_cmd[COMMAND_THRUST_X] += actuator_state[i] * (int32_t) act_is_thruster_x[i];
+ current_thrust_filt_x += Bwls[4][i]* actuator_lowpass_filters[i].o[0] * (int32_t) act_is_thruster_x[i];
}
stabilization_cmd[COMMAND_THRUST_X] /= num_thrusters_x;
+
+ v_thrust.x = current_thrust_filt_x + indi_thrust_increment.x;
#endif
} else {
// incremental thrust
for (i = 0; i < INDI_NUM_ACT; i++) {
- v_thrust.z +=
- (stabilization_cmd[COMMAND_THRUST] - use_increment * actuator_state_filt_vect[i]) * Bwls[3][i];
+ v_thrust.z += stabilization_cmd[COMMAND_THRUST] * Bwls[3][i];
#if INDI_OUTPUTS == 5
stabilization_cmd[COMMAND_THRUST_X] = radio_control.values[RADIO_CONTROL_THRUST_X];
- v_thrust.x +=
- (stabilization_cmd[COMMAND_THRUST_X] - use_increment * actuator_state_filt_vect[i]) * Bwls[4][i];
+ v_thrust.x += stabilization_cmd[COMMAND_THRUST_X] * Bwls[4][i];
#endif
}
}
+ // This term compensates for the spinup torque in the yaw axis
+ float g2_times_u = float_vect_dot_product(g2, indi_u, INDI_NUM_ACT)/INDI_G_SCALING;
+
+ if (in_flight) {
+ // Limit the estimated disturbance in yaw for drones that are stable in sideslip
+ BoundAbs(angular_acc_disturbance_estimate[2], stablization_indi_yaw_dist_limit);
+ } else {
+ // Not in flight, so don't estimate disturbance
+ float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS);
+ }
+
// The control objective in array format
- indi_v[0] = (angular_accel_ref.p - use_increment * angular_acceleration[0]);
- indi_v[1] = (angular_accel_ref.q - use_increment * angular_acceleration[1]);
- indi_v[2] = (angular_accel_ref.r - use_increment * angular_acceleration[2] + g2_times_du);
+ indi_v[0] = (angular_accel_ref.p - angular_acc_disturbance_estimate[0]);
+ indi_v[1] = (angular_accel_ref.q - angular_acc_disturbance_estimate[1]);
+ indi_v[2] = (angular_accel_ref.r - angular_acc_disturbance_estimate[2]) + g2_times_u;
indi_v[3] = v_thrust.z;
#if INDI_OUTPUTS == 5
indi_v[4] = v_thrust.x;
#endif
-
#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
// Calculate the increment for each actuator
for (i = 0; i < INDI_NUM_ACT; i++) {
- indi_du[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
- + (g1g2_pseudo_inv[i][1] * indi_v[1])
- + (g1g2_pseudo_inv[i][2] * indi_v[2])
- + (g1g2_pseudo_inv[i][3] * indi_v[3]);
+ indi_u[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
+ + (g1g2_pseudo_inv[i][1] * indi_v[1])
+ + (g1g2_pseudo_inv[i][2] * indi_v[2])
+ + (g1g2_pseudo_inv[i][3] * indi_v[3]);
}
#else
- stabilization_indi_set_wls_settings(use_increment);
-
+ stabilization_indi_set_wls_settings();
// WLS Control Allocator
num_iter =
- wls_alloc(indi_du, indi_v, du_min_stab_indi, du_max_stab_indi, Bwls, 0, 0, Wv, indi_Wu, du_pref_stab_indi, 10000, 10,
+ wls_alloc(indi_u, indi_v, u_min_stab_indi, u_max_stab_indi, Bwls, 0, 0, Wv, indi_Wu, u_pref_stab_indi, 10000, 10,
INDI_NUM_ACT, INDI_OUTPUTS);
#endif
- if (in_flight) {
- // Add the increments to the actuators
- float_vect_sum(indi_u, actuator_state_filt_vect, indi_du, INDI_NUM_ACT);
- } else {
- // Not in flight, so don't increment
- float_vect_copy(indi_u, indi_du, INDI_NUM_ACT);
- }
-
// Bound the inputs to the actuators
for (i = 0; i < INDI_NUM_ACT; i++) {
if (act_is_servo[i]) {
@@ -706,20 +739,6 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
}
}
- // Propagate actuator filters
- get_actuator_state();
- for (i = 0; i < INDI_NUM_ACT; i++) {
- update_butterworth_2_low_pass(&actuator_lowpass_filters[i], actuator_state[i]);
- update_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], actuator_state[i]);
- actuator_state_filt_vect[i] = actuator_lowpass_filters[i].o[0];
-
- // calculate derivatives for estimation
- float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
- actuator_state_filt_vectd[i] = (estimation_input_lowpass_filters[i].o[0] - estimation_input_lowpass_filters[i].o[1]) *
- PERIODIC_FREQUENCY;
- actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
- }
-
// Use online effectiveness estimation only when flying
if (in_flight && indi_use_adaptive) {
lms_estimation();
@@ -732,17 +751,15 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
}
/**
- * @param use_increment
- *
* Function that sets the du_min, du_max and du_pref if function not elsewhere defined
*/
-void WEAK stabilization_indi_set_wls_settings(float use_increment)
+void WEAK stabilization_indi_set_wls_settings(void)
{
// Calculate the min and max increments
for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
- du_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i] - use_increment * actuator_state_filt_vect[i];
- du_max_stab_indi[i] = MAX_PPRZ - use_increment * actuator_state_filt_vect[i];
- du_pref_stab_indi[i] = act_pref[i] - use_increment * actuator_state_filt_vect[i];
+ u_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i];
+ u_max_stab_indi[i] = MAX_PPRZ;
+ u_pref_stab_indi[i] = act_pref[i];
#ifdef GUIDANCE_INDI_MIN_THROTTLE
float airspeed = stateGetAirspeed_f();
@@ -750,9 +767,9 @@ void WEAK stabilization_indi_set_wls_settings(float use_increment)
if (!act_is_servo[i]) {
if ((guidance_h.mode == GUIDANCE_H_MODE_HOVER) || (guidance_h.mode == GUIDANCE_H_MODE_NAV)) {
if (airspeed < STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD) {
- du_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE - use_increment * actuator_state_filt_vect[i];
+ u_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE;
} else {
- du_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - use_increment * actuator_state_filt_vect[i];
+ u_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD;
}
}
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
index 1196e34feab..3b6d02f93e3 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
@@ -38,9 +38,9 @@ extern bool act_is_servo[INDI_NUM_ACT];
extern bool indi_use_adaptive;
-extern float du_min_stab_indi[INDI_NUM_ACT];
-extern float du_max_stab_indi[INDI_NUM_ACT];
-extern float du_pref_stab_indi[INDI_NUM_ACT];
+extern float u_min_stab_indi[INDI_NUM_ACT];
+extern float u_max_stab_indi[INDI_NUM_ACT];
+extern float u_pref_stab_indi[INDI_NUM_ACT];
extern float *Bwls[INDI_OUTPUTS];
extern float thrust_bx_eff;
@@ -57,6 +57,8 @@ struct Indi_gains {
struct FloatRates rate;
};
+extern float stablization_indi_yaw_dist_limit;
+
extern struct Indi_gains indi_gains;
extern void stabilization_indi_init(void);
@@ -67,7 +69,7 @@ extern void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat);
extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp);
extern void stabilization_indi_rate_run(struct FloatRates rate_ref, bool in_flight);
-extern void stabilization_indi_set_wls_settings(float use_increment);
+extern void stabilization_indi_set_wls_settings(void);
extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c
index 72539df01e5..8827a1c35eb 100644
--- a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c
+++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c
@@ -128,6 +128,7 @@ struct rot_wing_eff_sched_param_t eff_sched_p = {
.Ixx_wing = ROT_WING_EFF_SCHED_IXX_WING,
.Iyy_wing = ROT_WING_EFF_SCHED_IYY_WING,
.m = ROT_WING_EFF_SCHED_M,
+ .DMdpprz_hover_roll = ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL,
.hover_roll_pitch_coef = ROT_WING_EFF_SCHED_HOVER_ROLL_PITCH_COEF,
.hover_roll_roll_coef = ROT_WING_EFF_SCHED_HOVER_ROLL_ROLL_COEF,
.k_elevator = ROT_WING_EFF_SCHED_K_ELEVATOR,
@@ -158,7 +159,7 @@ inline void eff_scheduling_rot_wing_update_pusher_effectiveness(void);
inline void eff_scheduling_rot_wing_schedule_liftd(void);
inline float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED);
-inline void stabilization_indi_set_wls_settings(float use_increment);
+void stabilization_indi_set_wls_settings(void);
/** ABI binding wing position data.
@@ -198,9 +199,10 @@ void eff_scheduling_rot_wing_init(void)
// Set moment derivative variables
eff_sched_var.pitch_motor_dMdpprz = ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH;
- eff_sched_var.roll_motor_dMdpprz = ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL;
+ eff_sched_var.roll_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_roll[0] + (MAX_PPRZ/2.) * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; // Dmdpprz hover roll for hover thrust
eff_sched_var.cmd_elevator = 0;
+ eff_sched_var.cmd_pusher = 0;
eff_sched_var.cmd_pusher_scaled = 0;
eff_sched_var.cmd_T_mean_scaled = 0;
@@ -258,9 +260,10 @@ void eff_scheduling_rot_wing_update_MMOI(void)
void eff_scheduling_rot_wing_update_cmd(void)
{
// These indexes depend on the INDI sequence, not the actuator IDX
- eff_sched_var.cmd_elevator = actuators_pprz[5];
- eff_sched_var.cmd_pusher_scaled = actuators_pprz[8] * 0.000853229; // Scaled with 8181 / 9600 / 1000
- eff_sched_var.cmd_T_mean_scaled = (actuators_pprz[0] + actuators_pprz[1] + actuators_pprz[2] + actuators_pprz[3]) / 4. * 0.000853229; // Scaled with 8181 / 9600 / 1000
+ eff_sched_var.cmd_elevator = actuator_state_filt_vect[5];
+ eff_sched_var.cmd_pusher = actuator_state_filt_vect[8];
+ eff_sched_var.cmd_pusher_scaled = actuator_state_filt_vect[8] * 0.000853229; // Scaled with 8181 / 9600 / 1000
+ eff_sched_var.cmd_T_mean_scaled = (actuator_state_filt_vect[0] + actuator_state_filt_vect[1] + actuator_state_filt_vect[2] + actuator_state_filt_vect[3]) / 4. * 0.000853229; // Scaled with 8181 / 9600 / 1000
}
void eff_scheduling_rot_wing_update_airspeed(void)
@@ -278,12 +281,20 @@ void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void)
float pitch_motor_q_eff = eff_sched_var.pitch_motor_dMdpprz / eff_sched_var.Iyy;
// Roll motor effectiveness
+ float dM_dpprz_right = (eff_sched_p.DMdpprz_hover_roll[0] + actuator_state_filt_vect[1] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
+ float dM_dpprz_left = (eff_sched_p.DMdpprz_hover_roll[0] + actuator_state_filt_vect[3] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
- float roll_motor_p_eff = (eff_sched_var.roll_motor_dMdpprz * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
- Bound(roll_motor_p_eff, 0.00001, 1);
+ // Bound dM_dpprz to half and 2 times the hover effectiveness
+ Bound(dM_dpprz_right, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
+ Bound(dM_dpprz_left, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
+
+ float roll_motor_p_eff_right = -(dM_dpprz_right * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
+ Bound(roll_motor_p_eff_right, -1, -0.00001);
+
+ float roll_motor_p_eff_left = (dM_dpprz_left * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
float roll_motor_airspeed_compensation = eff_sched_p.hover_roll_roll_coef[1] * eff_sched_var.airspeed * eff_sched_var.cosr / eff_sched_var.Ixx;
- float roll_motor_p_eff_compensated = roll_motor_p_eff + roll_motor_airspeed_compensation;
- Bound(roll_motor_p_eff_compensated, 0.00001, 1);
+ roll_motor_p_eff_left += roll_motor_airspeed_compensation;
+ Bound(roll_motor_p_eff_left, 0.00001, 1);
float roll_motor_q_eff = (eff_sched_p.hover_roll_pitch_coef[0] * eff_sched_var.wing_rotation_rad + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.sinr) / eff_sched_var.Iyy;
Bound(roll_motor_q_eff, 0, 1);
@@ -295,11 +306,11 @@ void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void)
g1g2[1][2] = -pitch_motor_q_eff; // pitch effectiveness back motor
// Update right motor p and q effectiveness
- g1g2[0][1] = -roll_motor_p_eff; // roll effectiveness right motor (no airspeed compensation)
+ g1g2[0][1] = roll_motor_p_eff_right; // roll effectiveness right motor (no airspeed compensation)
g1g2[1][1] = roll_motor_q_eff; // pitch effectiveness right motor
// Update left motor p and q effectiveness
- g1g2[0][3] = roll_motor_p_eff_compensated; // roll effectiveness left motor
+ g1g2[0][3] = roll_motor_p_eff_left; // roll effectiveness left motor
g1g2[1][3] = -roll_motor_q_eff; // pitch effectiveness left motor
}
@@ -334,7 +345,7 @@ void eff_scheduling_rot_wing_update_rudder_effectiveness(void)
// Convert moment to effectiveness
float eff_z_rudder = dMzdpprz / eff_sched_p.Izz;
- Bound(eff_z_rudder, 0.00001, 0.1);
+ Bound(eff_z_rudder, 0.000001, 0.1);
g1g2[2][4] = eff_z_rudder;
}
@@ -357,10 +368,10 @@ void eff_scheduling_rot_wing_update_flaperon_effectiveness(void)
void eff_scheduling_rot_wing_update_pusher_effectiveness(void)
{
- float rpmP = eff_sched_p.k_rpm_pprz_pusher[0] + eff_sched_p.k_rpm_pprz_pusher[1] * eff_sched_var.cmd_pusher_scaled + eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled;
+ float rpmP = eff_sched_p.k_rpm_pprz_pusher[0] + eff_sched_p.k_rpm_pprz_pusher[1] * eff_sched_var.cmd_pusher + eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher * eff_sched_var.cmd_pusher;
float dFxdrpmP = eff_sched_p.k_pusher[0]*rpmP + eff_sched_p.k_pusher[1] * eff_sched_var.airspeed;
- float drpmPdpprz = eff_sched_p.k_rpm_pprz_pusher[1] + 2. * eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher_scaled;
+ float drpmPdpprz = eff_sched_p.k_rpm_pprz_pusher[1] + 2. * eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher;
float eff_pusher = (dFxdrpmP * drpmPdpprz / eff_sched_p.m) / 10000.;
@@ -389,39 +400,16 @@ float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED) {
return eff_scheduling_rot_wing_lift_d;
}
-void stabilization_indi_set_wls_settings(float use_increment)
+void stabilization_indi_set_wls_settings(void)
{
// Calculate the min and max increments
for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
- du_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i] - use_increment*actuator_state_filt_vect[i];
- du_max_stab_indi[i] = MAX_PPRZ - use_increment*actuator_state_filt_vect[i];
- du_pref_stab_indi[i] = act_pref[i] - use_increment*actuator_state_filt_vect[i];
+ u_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i];
+ u_max_stab_indi[i] = MAX_PPRZ;
+ u_pref_stab_indi[i] = act_pref[i];
if (i == 5) {
- du_pref_stab_indi[i] = 0; // Set change in prefered state to 0 for elevator
- du_min_stab_indi[i] = - use_increment*actuator_state_filt_vect[i]; // cmd 0 is lowest position for elevator
+ u_pref_stab_indi[i] = actuator_state_filt_vect[i]; // Set change in prefered state to 0 for elevator
+ u_min_stab_indi[i] = 0; // cmd 0 is lowest position for elevator
}
}
-
- #if ROTWING_V3B
- float min_pprz_cmd_ail = -MAX_PPRZ;
- if (eff_sched_var.wing_rotation_deg < 15) {
- min_pprz_cmd_ail = 0;
- }
- Bound(min_pprz_cmd_ail, -9600, 0);
-
- du_min_stab_indi[6] = min_pprz_cmd_ail - use_increment*actuators_pprz[6];
-
- float min_pprz_cmd_flap_ail = -MAX_PPRZ;
- if (eff_sched_var.wing_rotation_deg < 38) {
- min_pprz_cmd_flap_ail = -1000;
- } if (eff_sched_var.wing_rotation_deg > 50) {
- min_pprz_cmd_flap_ail = -9600;
- } else {
- min_pprz_cmd_flap_ail = -5.596578906693223 * eff_sched_var.wing_rotation_deg * eff_sched_var.wing_rotation_deg * eff_sched_var.wing_rotation_deg + 654.186408367317 * eff_sched_var.wing_rotation_deg * eff_sched_var.wing_rotation_deg - 25577.0135504177 * eff_sched_var.wing_rotation_deg + 333307.855118805;
- }
-
- Bound(min_pprz_cmd_flap_ail, -9600, 0);
-
- du_min_stab_indi[7] = min_pprz_cmd_flap_ail - use_increment*actuators_pprz[7];
- #endif //ROTWING_V3B
}
diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h
index 4c6616ed564..22771f39292 100644
--- a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h
+++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h
@@ -35,6 +35,7 @@ struct rot_wing_eff_sched_param_t {
float Ixx_wing; // wing MMOI around the chordwise direction of the wing [kgm²]
float Iyy_wing; // wing MMOI around the spanwise direction of the wing [kgm²]
float m; // mass [kg]
+ float DMdpprz_hover_roll[2]; // Moment coeficients for roll motors (Scaled by 10000)
float hover_roll_pitch_coef[2]; // Model coefficients to correct pitch effective for roll motors
float hover_roll_roll_coef[2]; // Model coefficients to correct roll effectiveness for roll motors
float k_elevator[3];
@@ -67,6 +68,7 @@ struct rot_wing_eff_sched_var_t {
// commands
float cmd_elevator;
+ float cmd_pusher;
float cmd_pusher_scaled;
float cmd_T_mean_scaled;