Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Rotwing guidance bank fix #3271

Merged
merged 7 commits into from
May 23, 2024
Merged

Rotwing guidance bank fix #3271

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
329c254
max bank in deg
EwoudSmeur Apr 23, 2024
5d7d30a
Fix allocation switching problem
EwoudSmeur Apr 4, 2024
4429a51
[rot_wing_v3] Updated max bank in all airframes
Dennis-Wijngaarden Apr 25, 2024
cbc50ac
Update sw/airborne/modules/checks/preflight_checks.c
dewagter May 22, 2024
33ce07a
use unit and alt_unit for conversions instead
dewagter May 22, 2024
7bc65a5
remove needless conversion
EwoudSmeur May 22, 2024
ad671a2
Also fix normal INDI to fit the code
dewagter May 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
13 changes: 8 additions & 5 deletions conf/airframes/tudelft/rot_wing_25kg.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/>

<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target>

<!-- Herelink datalink -->
Expand Down Expand Up @@ -178,13 +180,13 @@
<axis name="THRUST" failsafe_value="0"/>
<axis name="THRUST_X" failsafe_value="0"/>
</commands>


<command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let VAR="hover_off" VALUE="Or($th_hold, bool_disable_hover_motors)"/>

<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<!-- Main bus -->
<set servo="MOTOR_FRONT" value="($hover_off? -9600 : actuators_pprz[0])"/>
Expand Down Expand Up @@ -253,7 +255,7 @@
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-131.497033952591,1.32098226269777,-0.0000291178067445214}"/>

<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
Expand Down Expand Up @@ -369,7 +371,7 @@

<!-- Rate INDI -->
<define name="MAX_RATE" value="1.5"/>

<!-- Reference -->
<define name="REF_ERR_P" value="40.0"/>
<define name="REF_ERR_Q" value="25.0"/>
Expand Down Expand Up @@ -428,7 +430,7 @@
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{10., 10., 100., 1.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>

<!-- Gains -->
<define name="POS_GAIN" value="0.3"/>
Expand All @@ -455,6 +457,7 @@

<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section>

<section name="FORWARD">
Expand Down
7 changes: 5 additions & 2 deletions conf/airframes/tudelft/rot_wing_v3b.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/>

<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target>

<!-- Kongsberg datalink -->
Expand Down Expand Up @@ -465,7 +467,7 @@
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="MAX_BANK" value="50" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>

<!-- Gains -->
Expand All @@ -479,7 +481,7 @@
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{10., 10., 100., 1.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>

<!-- Gains -->
<define name="POS_GAIN" value="0.3"/>
Expand All @@ -505,6 +507,7 @@

<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section>

<section name="FORWARD">
Expand Down
7 changes: 5 additions & 2 deletions conf/airframes/tudelft/rot_wing_v3d.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/>

<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target>

<!-- Herelink datalink -->
Expand Down Expand Up @@ -452,7 +454,7 @@
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="MAX_BANK" value="50" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>

<!-- Gains -->
Expand All @@ -466,7 +468,7 @@
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{10., 10., 100., 1.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>

<!-- Gains -->
<define name="POS_GAIN" value="0.3"/>
Expand All @@ -492,6 +494,7 @@

<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section>

<section name="FORWARD">
Expand Down
7 changes: 5 additions & 2 deletions conf/airframes/tudelft/rot_wing_v3e.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/>

<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target>

<!-- Herelink datalink -->
Expand Down Expand Up @@ -452,7 +454,7 @@
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="MAX_BANK" value="50" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>

<!-- Gains -->
Expand All @@ -466,7 +468,7 @@
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{10., 10., 100., 1.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>

<!-- Gains -->
<define name="POS_GAIN" value="0.3"/>
Expand All @@ -492,6 +494,7 @@

<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section>

<section name="FORWARD">
Expand Down
7 changes: 5 additions & 2 deletions conf/airframes/tudelft/rot_wing_v3f.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/>

<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target>

<!-- Herelink datalink -->
Expand Down Expand Up @@ -452,7 +454,7 @@
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="MAX_BANK" value="50" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>

<!-- Gains -->
Expand All @@ -466,7 +468,7 @@
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{10., 10., 100., 1.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>

<!-- Gains -->
<define name="POS_GAIN" value="0.3"/>
Expand All @@ -492,6 +494,7 @@

<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section>

<section name="FORWARD">
Expand Down
7 changes: 5 additions & 2 deletions conf/airframes/tudelft/rot_wing_v3g.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/>

<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target>

<!-- Herelink datalink -->
Expand Down Expand Up @@ -452,7 +454,7 @@
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="MAX_BANK" value="50" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>

<!-- Gains -->
Expand All @@ -466,7 +468,7 @@
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{10., 10., 100., 1.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>

<!-- Gains -->
<define name="POS_GAIN" value="0.3"/>
Expand All @@ -492,6 +494,7 @@

<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section>

<section name="FORWARD">
Expand Down
7 changes: 5 additions & 2 deletions conf/airframes/tudelft/rot_wing_v3h.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/>

<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target>

<!-- Herelink datalink -->
Expand Down Expand Up @@ -452,7 +454,7 @@
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="MAX_BANK" value="50" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>

<!-- Gains -->
Expand All @@ -466,7 +468,7 @@
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{10., 10., 100., 1.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>

<!-- Gains -->
<define name="POS_GAIN" value="0.3"/>
Expand All @@ -492,6 +494,7 @@

<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section>

<section name="FORWARD">
Expand Down
2 changes: 1 addition & 1 deletion conf/modules/guidance_indi.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
<dl_settings NAME="guidance_indi">
<dl_setting var="guidance_indi_pos_gain" min="0" step="0.1" max="10.0" shortname="kp" param="GUIDANCE_INDI_POS_GAIN" persistent="true"/>
<dl_setting var="guidance_indi_speed_gain" min="0" step="0.1" max="10.0" shortname="kd" param="GUIDANCE_INDI_SPEED_GAIN" persistent="true"/>
<dl_setting var="guidance_indi_max_bank" min="0" step="0.1" max="80.0" shortname="max_bank" param="GUIDANCE_H_MAX_BANK"/>
<dl_setting var="guidance_indi_max_bank" min="0" step="0.1" max="80.0" shortname="max_bank" param="GUIDANCE_H_MAX_BANK" unit="rad" alt_unit="deg"/>
</dl_settings>
</dl_settings>
</settings>
Expand Down
2 changes: 1 addition & 1 deletion conf/modules/guidance_indi_hybrid.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
<dl_setting var="gih_params.speed_gainz" min="0" step="0.1" max="10.0" shortname="kd_z" param="GUIDANCE_INDI_SPEED_GAINZ" persistent="true"/>
<dl_setting var="gih_params.heading_bank_gain" min="0" step="0.1" max="30.0" shortname="bank gain" param="GUIDANCE_INDI_HEADING_BANK_GAIN" persistent="true"/>
<dl_setting var="guidance_indi_max_airspeed" min="12.0" step="1.0" max="30.0" shortname="cruise_airspeed" param="GUIDANCE_INDI_MAX_AIRSPEED"/>
<dl_setting var="guidance_indi_max_bank" min="10.0" step="1.0" max="50.0" shortname="max_bank" param="GUIDANCE_H_MAX_BANK"/>
<dl_setting var="guidance_indi_max_bank" min="10.0" step="1.0" max="50.0" shortname="max_bank" param="GUIDANCE_H_MAX_BANK" unit="rad" alt_unit="deg"/>
<dl_setting var="guidance_indi_min_pitch" min="-130.0" step="1.0" max="-30.0" shortname="min_pitch" param="GUIDANCE_INDI_MIN_PITCH"/>
<dl_setting var="take_heading_control" min="0" step="1" max="1" values="OFF|ON" shortname="take_heading"/>
<dl_setting var="guidance_indi_airspeed_filtering" min="0" step="1" max="1" values="OFF|ON" shortname="as_filt"/>
Expand Down
18 changes: 13 additions & 5 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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;

Expand Down
7 changes: 4 additions & 3 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down
7 changes: 6 additions & 1 deletion sw/airborne/modules/checks/preflight_checks.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down