Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 17:06:31 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Replace THRUST_X_EFF by G-matrix value + Max forward pitch in RotWing (#3301)

Browse Source 
* Update rotwing

fix merge

use same pusher effectiveness in guidance INDI

Correct spaces

Apply suggested fix

* Rotwing default altitude 25kg

* sensible values for max lat accel

---------

Co-authored-by: Ewoud Smeur <e.j.j.smeur@tudelft.nl>
This commit is contained in:
Christophe De Wagter
2024-06-18 11:10:40 +02:00
parent dc46222082
commit 7967299dad
12 changed files with 33 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+2 -2
View File
@@ -558,8 +558,8 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
float thrust_vect[3];
#if GUIDANCE_INDI_HYBRID_U > 3
thrust_vect[0] = du_gih[3];
if (thrust_vect[0] > GUIDANCE_INDI_MAX_PUSHER_INCREMENT*guidance_indi_thrust_x_eff) {
thrust_vect[0] = GUIDANCE_INDI_MAX_PUSHER_INCREMENT*guidance_indi_thrust_x_eff;
if (thrust_vect[0] > GUIDANCE_INDI_MAX_PUSHER_INCREMENT*g1g2[4][GUIDANCE_INDI_PUSHER_INDEX]) {
thrust_vect[0] = GUIDANCE_INDI_MAX_PUSHER_INCREMENT*g1g2[4][GUIDANCE_INDI_PUSHER_INDEX];
}
#else
thrust_vect[0] = 0;
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
-1
View File
@@ -94,7 +94,6 @@ extern float du_pref_gih[GUIDANCE_INDI_HYBRID_U];
#endif
extern float guidance_indi_thrust_z_eff;
extern float guidance_indi_thrust_x_eff;
extern struct guidance_indi_hybrid_params gih_params;
extern float guidance_indi_specific_force_gain;
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
+6 -8
View File
@@ -34,18 +34,16 @@
#error "Quadplanes require a forward thrust actuator"
#endif
#ifndef GUIDANCE_INDI_PUSHER_INDEX
#error "You need to define GUIDANCE_INDI_PUSHER_INDEX"
#endif
#ifndef GUIDANCE_INDI_THRUST_Z_EFF
#error "You need to define GUIDANCE_INDI_THRUST_Z_EFF"
#else
float guidance_indi_thrust_z_eff = GUIDANCE_INDI_THRUST_Z_EFF;
#endif
#ifndef GUIDANCE_INDI_THRUST_X_EFF
#error "You need to define GUIDANCE_INDI_THRUST_X_EFF"
#else
float guidance_indi_thrust_x_eff = GUIDANCE_INDI_THRUST_X_EFF;
#endif
float bodyz_filter_cutoff = 0.2;
Butterworth2LowPass accel_bodyz_filt;
@@ -138,13 +136,13 @@ void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angl
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;
du_min_gih[3] = -stabilization.cmd[COMMAND_THRUST_X]*g1g2[4][GUIDANCE_INDI_PUSHER_INDEX];
// Set upper limits limits
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;
du_max_gih[3] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST_X])*g1g2[4][GUIDANCE_INDI_PUSHER_INDEX];
// Set prefered states
du_pref_gih[0] = -roll_angle; // prefered delta roll angle
sw/airborne/modules/rot_wing_drone/rotwing_state.c
+7 -4
View File
@@ -727,9 +727,6 @@ bool rotwing_state_hover_motors_running(void) {
void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
{
// 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;
Bound(thrust_command, 0, MAX_PPRZ);
float fixed_wing_percentage = !hover_motors_active; // TODO: when hover props go below 40%, ...
Bound(fixed_wing_percentage, 0, 1);
#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16
@@ -768,18 +765,24 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl
float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);
float fwd_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
float quad_pitch_limit_rad = RadOfDeg(5.0);
float scheduled_pitch_angle = 0;
float pitch_angle_range = 3.;
if (rotwing_state_skewing.wing_angle_deg < 55) {
scheduled_pitch_angle = 0;
Wu_gih[1] = Wu_gih_original[1];
max_pitch_limit_rad = quad_pitch_limit_rad;
} else {
float pitch_progression = (rotwing_state_skewing.wing_angle_deg - 55) / 35.;
scheduled_pitch_angle = pitch_angle_range * pitch_progression;
Wu_gih[1] = Wu_gih_original[1] * (1.f - pitch_angle_range*0.9);
Wu_gih[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.9);
max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression;
}
if (!hover_motors_active) {
scheduled_pitch_angle = 8.;
max_pitch_limit_rad = fwd_pitch_limit_rad;
}
Bound(scheduled_pitch_angle, -5., 8.);
guidance_indi_pitch_pref_deg = scheduled_pitch_angle;