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

Rotwing guidance bank fix (#3271)

Browse Source 
* 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 <gautier.hattenberger@enac.fr>

* use unit and alt_unit for conversions instead

* remove needless conversion

* Also fix normal INDI to fit the code

---------

Co-authored-by: Ewoud Smeur <e.j.j.smeur@tudelft.nl>
Co-authored-by: Christophe De Wagter <dewagter@gmail.com>
Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>
This commit is contained in:
Dennis-Wijngaarden
2024-05-23 21:55:24 +02:00
committed by GitHub
parent b87ab09536
commit d748af1afb
14 changed files with 70 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rot_wing_25kg.xml
+4 -1
View File
@@ -56,6 +56,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control --> <define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/> <define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -428,7 +430,7 @@
<!--WLS settings--> <!--WLS settings-->
<define name="USE_WLS" value="TRUE"/> <define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/> <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 --> <!-- Gains -->
<define name="POS_GAIN" value="0.3"/> <define name="POS_GAIN" value="0.3"/>
@@ -455,6 +457,7 @@
<define name="MAX_PITCH" value="12"/> <define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/> <define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section> </section>
<section name="FORWARD"> <section name="FORWARD">
conf/airframes/tudelft/rot_wing_v3b.xml
+5 -2
View File
@@ -67,6 +67,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control --> <define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/> <define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target> </target>
<!-- Kongsberg datalink --> <!-- Kongsberg datalink -->
@@ -465,7 +467,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <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"/> <define name="USE_SPEED_REF" value="TRUE"/>
<!-- Gains --> <!-- Gains -->
@@ -479,7 +481,7 @@
<!--WLS settings--> <!--WLS settings-->
<define name="USE_WLS" value="TRUE"/> <define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/> <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 --> <!-- Gains -->
<define name="POS_GAIN" value="0.3"/> <define name="POS_GAIN" value="0.3"/>
@@ -505,6 +507,7 @@
<define name="MAX_PITCH" value="12"/> <define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/> <define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section> </section>
<section name="FORWARD"> <section name="FORWARD">
conf/airframes/tudelft/rot_wing_v3d.xml
+5 -2
View File
@@ -54,6 +54,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control --> <define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/> <define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -452,7 +454,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <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"/> <define name="USE_SPEED_REF" value="TRUE"/>
<!-- Gains --> <!-- Gains -->
@@ -466,7 +468,7 @@
<!--WLS settings--> <!--WLS settings-->
<define name="USE_WLS" value="TRUE"/> <define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/> <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 --> <!-- Gains -->
<define name="POS_GAIN" value="0.3"/> <define name="POS_GAIN" value="0.3"/>
@@ -492,6 +494,7 @@
<define name="MAX_PITCH" value="12"/> <define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/> <define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section> </section>
<section name="FORWARD"> <section name="FORWARD">
conf/airframes/tudelft/rot_wing_v3e.xml
+5 -2
View File
@@ -54,6 +54,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control --> <define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/> <define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -452,7 +454,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <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"/> <define name="USE_SPEED_REF" value="TRUE"/>
<!-- Gains --> <!-- Gains -->
@@ -466,7 +468,7 @@
<!--WLS settings--> <!--WLS settings-->
<define name="USE_WLS" value="TRUE"/> <define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/> <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 --> <!-- Gains -->
<define name="POS_GAIN" value="0.3"/> <define name="POS_GAIN" value="0.3"/>
@@ -492,6 +494,7 @@
<define name="MAX_PITCH" value="12"/> <define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/> <define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section> </section>
<section name="FORWARD"> <section name="FORWARD">
conf/airframes/tudelft/rot_wing_v3f.xml
+5 -2
View File
@@ -54,6 +54,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control --> <define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/> <define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -452,7 +454,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <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"/> <define name="USE_SPEED_REF" value="TRUE"/>
<!-- Gains --> <!-- Gains -->
@@ -466,7 +468,7 @@
<!--WLS settings--> <!--WLS settings-->
<define name="USE_WLS" value="TRUE"/> <define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/> <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 --> <!-- Gains -->
<define name="POS_GAIN" value="0.3"/> <define name="POS_GAIN" value="0.3"/>
@@ -492,6 +494,7 @@
<define name="MAX_PITCH" value="12"/> <define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/> <define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section> </section>
<section name="FORWARD"> <section name="FORWARD">
conf/airframes/tudelft/rot_wing_v3g.xml
+5 -2
View File
@@ -54,6 +54,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control --> <define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/> <define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -452,7 +454,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <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"/> <define name="USE_SPEED_REF" value="TRUE"/>
<!-- Gains --> <!-- Gains -->
@@ -466,7 +468,7 @@
<!--WLS settings--> <!--WLS settings-->
<define name="USE_WLS" value="TRUE"/> <define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/> <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 --> <!-- Gains -->
<define name="POS_GAIN" value="0.3"/> <define name="POS_GAIN" value="0.3"/>
@@ -492,6 +494,7 @@
<define name="MAX_PITCH" value="12"/> <define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/> <define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section> </section>
<section name="FORWARD"> <section name="FORWARD">
conf/airframes/tudelft/rot_wing_v3h.xml
+5 -2
View File
@@ -54,6 +54,8 @@
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control --> <define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/> <define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -452,7 +454,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <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"/> <define name="USE_SPEED_REF" value="TRUE"/>
<!-- Gains --> <!-- Gains -->
@@ -466,7 +468,7 @@
<!--WLS settings--> <!--WLS settings-->
<define name="USE_WLS" value="TRUE"/> <define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/> <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 --> <!-- Gains -->
<define name="POS_GAIN" value="0.3"/> <define name="POS_GAIN" value="0.3"/>
@@ -492,6 +494,7 @@
<define name="MAX_PITCH" value="12"/> <define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/> <define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</section> </section>
<section name="FORWARD"> <section name="FORWARD">
conf/modules/guidance_indi.xml
+1 -1
View File
@@ -11,7 +11,7 @@
<dl_settings NAME="guidance_indi"> <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_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_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>
</dl_settings> </dl_settings>
</settings> </settings>
conf/modules/guidance_indi_hybrid.xml
+1 -1
View File
@@ -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.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="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_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="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="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"/> <dl_setting var="guidance_indi_airspeed_filtering" min="0" step="1" max="1" values="OFF|ON" shortname="as_filt"/>
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+12 -4
View File
@@ -162,6 +162,10 @@ static void guidance_indi_filter_thrust(void);
#define GUIDANCE_INDI_DESCEND_SPEED_FWD -4.0 #define GUIDANCE_INDI_DESCEND_SPEED_FWD -4.0
#endif #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 climb_vspeed_fwd = GUIDANCE_INDI_CLIMB_SPEED_FWD;
float descend_vspeed_fwd = GUIDANCE_INDI_DESCEND_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]; float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U];
struct FloatVect3 euler_cmd; struct FloatVect3 euler_cmd;
float du_gih[GUIDANCE_INDI_HYBRID_U]; // = {0.0f, 0.0f, 0.0f};
#if GUIDANCE_INDI_HYBRID_USE_WLS #if GUIDANCE_INDI_HYBRID_USE_WLS
#include "math/wls/wls_alloc.h" #include "math/wls/wls_alloc.h"
float du_min_gih[GUIDANCE_INDI_HYBRID_U]; float du_min_gih[GUIDANCE_INDI_HYBRID_U];
float du_max_gih[GUIDANCE_INDI_HYBRID_U]; float du_max_gih[GUIDANCE_INDI_HYBRID_U];
float du_pref_gih[GUIDANCE_INDI_HYBRID_U]; float du_pref_gih[GUIDANCE_INDI_HYBRID_U];
float *Bwls_gih[GUIDANCE_INDI_HYBRID_V]; float *Bwls_gih[GUIDANCE_INDI_HYBRID_V];
#ifdef GUIDANCE_INDI_HYBRID_WLS_PRIORITIES #ifdef GUIDANCE_INDI_WLS_PRIORITIES
float Wv_gih[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_HYBRID_WLS_PRIORITIES; float Wv_gih[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES;
#else #else
float Wv_gih[GUIDANCE_INDI_HYBRID_V] = { 100.f, 100.f, 1.f }; // X,Y accel, Z accel float Wv_gih[GUIDANCE_INDI_HYBRID_V] = { 100.f, 100.f, 1.f }; // X,Y accel, Z accel
#endif #endif
#ifdef GUIDANCE_INDI_HYBRID_WLS_WU #ifdef GUIDANCE_INDI_WLS_WU
float Wu_gih[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_HYBRID_WLS_WU; float Wu_gih[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU;
#else #else
float Wu_gih[GUIDANCE_INDI_HYBRID_U] = { 1.f, 1.f, 1.f }; float Wu_gih[GUIDANCE_INDI_HYBRID_U] = { 1.f, 1.f, 1.f };
#endif #endif
@@ -640,6 +646,8 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
FLOAT_ANGLE_NORMALIZE(sp_accel_b.y); FLOAT_ANGLE_NORMALIZE(sp_accel_b.y);
sp_accel_b.y *= gih_params.heading_bank_gain; sp_accel_b.y *= gih_params.heading_bank_gain;
BoundAbs(sp_accel_b.y, GUIDANCE_INDI_MAX_LAT_ACCEL);
// Control the airspeed // Control the airspeed
sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain; sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain;
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
+4 -3
View File
@@ -117,6 +117,8 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H
Gmat[0][3] = ctheta; Gmat[0][3] = ctheta;
Gmat[1][3] = 0; Gmat[1][3] = 0;
Gmat[2][3] = -stheta; Gmat[2][3] = -stheta;
// Make this term zero to prevent switching 'exploits'
// Gmat[2][3] = 0;
// Convert acceleration error to body axis system // Convert acceleration error to body axis system
body_v[0] = cpsi * a_diff.x + spsi * a_diff.y; 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) 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 max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH); float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);
float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg); float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg);
// Set lower limits // 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[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[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]*guidance_indi_thrust_x_eff;
// Set upper limits limits // 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[1] = max_pitch_limit_rad - pitch_angle; // pitch
du_max_gih[2] = -stabilization.cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff; 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])*guidance_indi_thrust_x_eff;
sw/airborne/modules/checks/preflight_checks.c
+6 -1
View File
@@ -43,8 +43,13 @@
#define PREFLIGHT_CHECK_INFO_TIMEOUT 5 #define PREFLIGHT_CHECK_INFO_TIMEOUT 5
#endif #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; static struct preflight_check_t *preflight_head = NULL;
bool preflight_bypass = FALSE;
/** /**
* @brief Register a preflight check and add it to the linked list * @brief Register a preflight check and add it to the linked list
sw/airborne/modules/rot_wing_drone/rotwing_state.c
+5 -18
View File
@@ -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) 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 // adjust weights
float thrust_command = (actuator_state_filt_vect[0] + actuator_state_filt_vect[1] + actuator_state_filt_vect[2] + float thrust_command = (actuator_state_filt_vect[0] + actuator_state_filt_vect[1] + actuator_state_filt_vect[2] +
actuator_state_filt_vect[3]) / 4; 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); Bound(fixed_wing_percentage, 0, 1);
#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16 #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) 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; struct FloatEulers eulers_zxy;
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); 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]); actuator_state_filt_vect[2] * g1g2[3][2] + actuator_state_filt_vect[3] * g1g2[3][3]);
Bound(du_max_thrust_z, 0., 50.); 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 max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH); float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);
sw/airborne/state.h
+1 -1
View File
@@ -332,7 +332,7 @@ struct State {
uint8_t accel_status; uint8_t accel_status;
/** /**
* Acceleration in North East Down coordinates. * Acceleration in Body coordinates.
* Units: m/s^2 in BFP with #INT32_ACCEL_FRAC * Units: m/s^2 in BFP with #INT32_ACCEL_FRAC
*/ */
struct Int32Vect3 body_accel_i; struct Int32Vect3 body_accel_i;