Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

[stabilization] Repair INDI settings

Browse Source
This commit is contained in:
Freek van Tienen
2016-01-27 13:19:38 +01:00
parent 2d92c1f765
commit 58ce0582b4
6 changed files with 47 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TUDELFT/tudelft_conf.xml
+13 -13
View File
@@ -17,7 +17,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml settings/control/stabilization_att_indi.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml settings/control/stabilization_indi.xml"
settings_modules="modules/air_data.xml"
gui_color="#ffffcd49cd49"
/>
@@ -28,7 +28,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml settings/control/stabilization_att_indi.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml settings/control/stabilization_indi.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml"
gui_color="#ffffc457c457"
/>
@@ -50,7 +50,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_optitrack.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml settings/control/stabilization_att_indi.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml settings/control/stabilization_indi.xml"
settings_modules=""
gui_color="#fffff996b847"
/>
@@ -61,7 +61,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/TUDELFT/tudelft_delft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_indi.xml] settings/control/rotorcraft_speed.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_speed.xml"
settings_modules="[modules/geo_mag.xml] modules/air_data.xml"
gui_color="#ffffcccaccca"
/>
@@ -72,7 +72,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/TUDELFT/tudelft_rotorcraft_survey_imav2015_competition.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_indi.xml settings/control/rotorcraft_speed.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_speed.xml"
settings_modules="[modules/geo_mag.xml] modules/air_data.xml modules/video_thread.xml modules/cv_blob_locator.xml modules/video_rtp_stream.xml [modules/nav_survey_rectangle_rotorcraft.xml] modules/nav_survey_poly_rotorcraft.xml modules/digital_cam_video.xml"
gui_color="#f84eb57ae827"
/>
@@ -171,7 +171,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int.xml] [settings/control/stabilization_rate.xml] settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_att_indi.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int.xml] [settings/control/stabilization_rate.xml] settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml"
settings_modules="modules/gps_ubx_ucenter.xml"
gui_color="#f1d3c1b7ffff"
/>
@@ -193,7 +193,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/TUDELFT/tudelft_delft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/control/stabilization_att_indi.xml [settings/control/stabilization_att_int_quat.xml]"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/control/stabilization_indi.xml [settings/control/stabilization_att_int_quat.xml]"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
gui_color="#baa3d698b729"
/>
@@ -204,7 +204,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/control/stabilization_att_indi.xml [settings/control/stabilization_att_int_quat.xml]"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/control/stabilization_indi.xml [settings/control/stabilization_att_int_quat.xml]"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
gui_color="#f6d5baaaffff"
/>
@@ -215,7 +215,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_indi.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_indi.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml"
gui_color="#7d7fd7e7ffff"
/>
@@ -226,7 +226,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/TUDELFT/tudelft_rotorcraft_survey_imav2015_competition.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_indi.xml settings/control/rotorcraft_speed.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_speed.xml"
settings_modules="[modules/geo_mag.xml] modules/air_data.xml modules/video_thread.xml modules/cv_blob_locator.xml modules/video_rtp_stream.xml [modules/nav_survey_rectangle_rotorcraft.xml] modules/nav_survey_poly_rotorcraft.xml modules/digital_cam_video.xml"
gui_color="#68f5eb116e0c"
/>
@@ -237,7 +237,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/TUDELFT/tudelft_rotorcraft_survey_imav2015_competition.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_indi.xml settings/control/rotorcraft_speed.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_speed.xml"
settings_modules="[modules/geo_mag.xml] modules/air_data.xml modules/video_thread.xml modules/cv_blob_locator.xml modules/video_rtp_stream.xml [modules/nav_survey_rectangle_rotorcraft.xml] modules/nav_survey_poly_rotorcraft.xml modules/digital_cam_video.xml"
gui_color="#ffff957fcc75"
/>
@@ -248,7 +248,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_indi.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_indi.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml"
gui_color="#b777d813cd0b"
/>
@@ -259,7 +259,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_indi.xml settings/modules/config_asctec_v2_new.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_indi.xml settings/modules/config_asctec_v2_new.xml"
settings_modules="modules/gps_ubx_ucenter.xml"
gui_color="#ffff9027e8ad"
/>
conf/settings/control/stabilization_att_indi.xml
-22
View File
@@ -1,22 +0,0 @@
<!DOCTYPE settings SYSTEM "../settings.dtd">
<settings>
<dl_settings>
<dl_settings NAME="indi">
<dl_setting var="reference_acceleration.err_p" min="0" step="1" max="2500" module="stabilization/stabilization_attitude_quat_indi" shortname="kp_p" param="STABILIZATION_INDI_REF_ERR_P" persistent="true"/>
<dl_setting var="reference_acceleration.rate_p" min="0" step="0.1" max="100" module="stabilization/stabilization_attitude_quat_indi" shortname="kd_p" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="g1.p" min="0" step="0.001" max="10" module="stabilization/stabilization_attitude_quat_indi" shortname="ctl_eff_p" param="STABILIZATION_INDI_G1_P" persistent="true"/>
<dl_setting var="reference_acceleration.err_q" min="0" step="1" max="2500" module="stabilization/stabilization_attitude_quat_indi" shortname="kp_q" param="STABILIZATION_INDI_REF_ERR_Q" persistent="true"/>
<dl_setting var="reference_acceleration.rate_q" min="0" step="0.1" max="100" module="stabilization/stabilization_attitude_quat_indi" shortname="kd_q" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="g1.q" min="0" step="0.001" max="10" module="stabilization/stabilization_attitude_quat_indi" shortname="ctl_eff_q" param="STABILIZATION_INDI_G1_Q" persistent="true"/>
<dl_setting var="reference_acceleration.err_r" min="0" step="1" max="2500" module="stabilization/stabilization_attitude_quat_indi" shortname="kp_r" param="STABILIZATION_INDI_REF_ERR_R" persistent="true"/>
<dl_setting var="reference_acceleration.rate_r" min="0" step="0.1" max="100" module="stabilization/stabilization_attitude_quat_indi" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="g1.r" min="0" step="0.001" max="10" module="stabilization/stabilization_attitude_quat_indi" shortname="ctl_eff_r" param="STABILIZATION_INDI_G1_R" persistent="true"/>
<dl_setting var="g2" min="0" step="0.01" max="10" module="stabilization/stabilization_attitude_quat_indi" shortname="g2" param="STABILIZATION_INDI_G2_R" persistent="true"/>
<dl_setting var="use_adaptive_indi" min="0" step="1" max="1" module="stabilization/stabilization_attitude_quat_indi" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8" persistent="true"/>
<dl_setting var="max_rate_cmd" min="0" step="0.0001" max="10.0" module="stabilization/stabilization_attitude_quat_indi" shortname="max_rate"/>
</dl_settings>
</dl_settings>
</settings>
conf/settings/control/stabilization_indi.xml
+22
View File
@@ -0,0 +1,22 @@
<!DOCTYPE settings SYSTEM "../settings.dtd">
<settings>
<dl_settings>
<dl_settings NAME="indi">
<dl_setting var="indi.reference_acceleration.err_p" min="0" step="1" max="2500" module="stabilization/stabilization_indi" shortname="kp_p" param="STABILIZATION_INDI_REF_ERR_P" persistent="true"/>
<dl_setting var="indi.reference_acceleration.rate_p" min="0" step="0.1" max="100" module="stabilization/stabilization_indi" shortname="kd_p" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.p" min="0" step="0.001" max="10" module="stabilization/stabilization_indi" shortname="ctl_eff_p" param="STABILIZATION_INDI_G1_P" persistent="true"/>
<dl_setting var="indi.reference_acceleration.err_q" min="0" step="1" max="2500" module="stabilization/stabilization_indi" shortname="kp_q" param="STABILIZATION_INDI_REF_ERR_Q" persistent="true"/>
<dl_setting var="indi.reference_acceleration.rate_q" min="0" step="0.1" max="100" module="stabilization/stabilization_indi" shortname="kd_q" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.q" min="0" step="0.001" max="10" module="stabilization/stabilization_indi" shortname="ctl_eff_q" param="STABILIZATION_INDI_G1_Q" persistent="true"/>
<dl_setting var="indi.reference_acceleration.err_r" min="0" step="1" max="2500" module="stabilization/stabilization_indi" shortname="kp_r" param="STABILIZATION_INDI_REF_ERR_R" persistent="true"/>
<dl_setting var="indi.reference_acceleration.rate_r" min="0" step="0.1" max="100" module="stabilization/stabilization_indi" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.r" min="0" step="0.001" max="10" module="stabilization/stabilization_indi" shortname="ctl_eff_r" param="STABILIZATION_INDI_G1_R" persistent="true"/>
<dl_setting var="indi.g2" min="0" step="0.01" max="10" module="stabilization/stabilization_indi" shortname="g2" param="STABILIZATION_INDI_G2_R" persistent="true"/>
<dl_setting var="indi.adaptive" min="0" step="1" max="1" module="stabilization/stabilization_indi" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8" persistent="true"/>
<dl_setting var="indi.max_rate" min="0" step="0.0001" max="80.0" module="stabilization/stabilization_indi" shortname="max_rate"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+9 -9
View File
@@ -63,14 +63,8 @@
#define STABILIZATION_INDI_MAX_RATE 6.0
#endif
#define STABILIZATION_INDI_FILT_OMEGA2 (STABILIZATION_INDI_FILT_OMEGA*STABILIZATION_INDI_FILT_OMEGA)
#define STABILIZATION_INDI_FILT_OMEGA2_R (STABILIZATION_INDI_FILT_OMEGA_R*STABILIZATION_INDI_FILT_OMEGA_R)
#if STABILIZATION_INDI_USE_ADAPTIVE
#warning "Use caution with adaptive indi. See the wiki for more info"
static bool_t use_adaptive_indi = TRUE;
#else
static bool_t use_adaptive_indi = FALSE;
#endif
struct Int32Eulers stab_att_sp_euler;
@@ -83,7 +77,7 @@ static void stabilization_indi_second_order_filter(struct IndiFilter *filter, st
static inline void lms_estimation(void);
#define INDI_EST_SCALE 0.001 //The G values are scaled to avoid numerical problems during the estimation
static struct IndiVariables indi = {
struct IndiVariables indi = {
.max_rate = STABILIZATION_INDI_MAX_RATE,
.g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
@@ -104,7 +98,13 @@ static struct IndiVariables indi = {
STABILIZATION_INDI_G1_R / INDI_EST_SCALE},
.g2 = STABILIZATION_INDI_G2_R / INDI_EST_SCALE,
.mu = STABILIZATION_INDI_ADAPTIVE_MU,
}
},
#if STABILIZATION_INDI_USE_ADAPTIVE
.adaptive = TRUE,
#else
.adaptive = FALSE,
#endif
};
#if PERIODIC_TELEMETRY
@@ -360,7 +360,7 @@ static inline void lms_estimation(void)
if (est->g1.r < 0.01) { est->g1.r = 0.01; }
if (est->g2 < 0.01) { est->g2 = 0.01; }
if (use_adaptive_indi) {
if (indi.adaptive) {
//Commit the estimated G values and apply the scaling
indi.g1.p = est->g1.p * INDI_EST_SCALE;
indi.g1.q = est->g1.q * INDI_EST_SCALE;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
+2
View File
@@ -81,11 +81,13 @@ struct IndiVariables {
struct ReferenceSystem reference_acceleration;
bool_t adaptive; ///< Enable adataptive estimation
float max_rate; ///< Maximum rate in rate control
struct IndiEstimation est; ///< Estimation parameters for adaptive INDI
};
extern struct IndiVariables indi;
extern void stabilization_indi_init(void);
extern void stabilization_indi_enter(void);
extern void stabilization_indi_set_failsafe_setpoint(void);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c
+1 -3
View File
@@ -22,11 +22,9 @@
/** @file stabilization_rate_indi.c
* Rate stabilization for rotorcrafts based on INDI by Ewoud Smeur.
*/
#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
void stabilization_rate_init(void)
{