mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-25 14:35:51 +08:00
[stabilization] INDI rewrite and rate control
This commit is contained in:
Freek van Tienen
@@ -20,7 +20,7 @@
|
||||
<subsystem name="imu" type="bebop"/>
|
||||
<subsystem name="gps" type="furuno"/>
|
||||
<subsystem name="stabilization" type="indi"/>
|
||||
<subsystem name="ahrs" type="float_mlkf"/>
|
||||
<subsystem name="stabilization" type="rate_indi"/>
|
||||
<subsystem name="ahrs" type="int_cmpl_quat">
|
||||
<configure name="USE_MAGNETOMETER" value="TRUE"/>
|
||||
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
|
||||
|
||||
@@ -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_att_indi.xml] settings/control/rotorcraft_speed.xml"
|
||||
settings_modules="[modules/geo_mag.xml] modules/air_data.xml"
|
||||
gui_color="#ffffcccaccca"
|
||||
/>
|
||||
@@ -149,7 +149,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/stabilization_att_int.xml settings/modules/config_asctec_v2.xml settings/control/rotorcraft_guidance.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/modules/config_asctec_v2.xml settings/control/rotorcraft_guidance.xml"
|
||||
settings_modules="[modules/geo_mag.xml] [modules/air_data.xml] modules/servo_switch.xml modules/gps_ubx_ucenter.xml"
|
||||
gui_color="#8c63ffff8e00"
|
||||
/>
|
||||
|
||||
@@ -29,7 +29,7 @@
|
||||
<subsystem name="gps" type="ublox">
|
||||
<define name="USE_PIKSI_EXT_ANTENNA" value="TRUE"/>
|
||||
</subsystem>
|
||||
<subsystem name="stabilization" type="int_quat"/>
|
||||
<subsystem name="stabilization" type="indi"/>
|
||||
<subsystem name="ahrs" type="int_cmpl_quat"/>
|
||||
<subsystem name="ins" type="hff"/>
|
||||
</firmware>
|
||||
@@ -39,6 +39,10 @@
|
||||
<load name="air_data.xml"/>
|
||||
<load name="servo_switch.xml"/>
|
||||
<load name="gps_ubx_ucenter.xml"/>
|
||||
<load name="logger_spi_link.xml">
|
||||
<configure name="HS_LOG_SPI_DEV" value="SPI1"/>
|
||||
<configure name="HS_LOG_SPI_SLAVE_IDX" value="SPI_SLAVE1"/>
|
||||
</load>
|
||||
</modules>
|
||||
|
||||
<servos driver="Asctec_v2">
|
||||
@@ -112,26 +116,6 @@
|
||||
<define name="H_Z" value="0.9259921"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_P" value="140" unit="deg/s"/>
|
||||
<define name="SP_MAX_Q" value="140" unit="deg/s"/>
|
||||
<define name="SP_MAX_R" value="140" unit="deg/s"/>
|
||||
<define name="DEADBAND_P" value="20"/>
|
||||
<define name="DEADBAND_Q" value="20"/>
|
||||
<define name="DEADBAND_R" value="200"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="GAIN_P" value="400"/>
|
||||
<define name="GAIN_Q" value="400"/>
|
||||
<define name="GAIN_R" value="350"/>
|
||||
|
||||
<define name="IGAIN_P" value="75"/>
|
||||
<define name="IGAIN_Q" value="75"/>
|
||||
<define name="IGAIN_R" value="50"/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="65." unit="deg"/>
|
||||
@@ -170,6 +154,37 @@
|
||||
<define name="PSI_DDGAIN" value=" 200"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<define name="G1_P" value="0.073"/>
|
||||
<define name="G1_Q" value="0.073"/>
|
||||
<define name="G1_R" value="0.0045"/>
|
||||
<define name="G2_R" value="0.2"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="900.0"/>
|
||||
<define name="REF_ERR_Q" value="900.0"/>
|
||||
<define name="REF_ERR_R" value="600.0"/>
|
||||
<define name="REF_RATE_P" value="33.0"/>
|
||||
<define name="REF_RATE_Q" value="33.0"/>
|
||||
<define name="REF_RATE_R" value="28.0"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_OMEGA" value="50.0"/>
|
||||
<define name="FILT_ZETA" value="0.7"/>
|
||||
<define name="FILT_OMEGA_R" value="20.0"/>
|
||||
<define name="FILT_ZETA_R" value="0.55"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN_P" value="0.15"/>
|
||||
<define name="ACT_DYN_Q" value="0.15"/>
|
||||
<define name="ACT_DYN_R" value="0.15"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="150"/>
|
||||
<define name="HOVER_KD" value="80"/>
|
||||
|
||||
@@ -104,26 +104,6 @@
|
||||
<section name="INS" prefix="INS_">
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_P" value="140" unit="deg/s"/>
|
||||
<define name="SP_MAX_Q" value="140" unit="deg/s"/>
|
||||
<define name="SP_MAX_R" value="140" unit="deg/s"/>
|
||||
<define name="DEADBAND_P" value="20"/>
|
||||
<define name="DEADBAND_Q" value="20"/>
|
||||
<define name="DEADBAND_R" value="200"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="GAIN_P" value="400"/>
|
||||
<define name="GAIN_Q" value="400"/>
|
||||
<define name="GAIN_R" value="350"/>
|
||||
|
||||
<define name="IGAIN_P" value="75"/>
|
||||
<define name="IGAIN_Q" value="75"/>
|
||||
<define name="IGAIN_R" value="50"/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="45." unit="deg"/>
|
||||
|
||||
@@ -33,6 +33,7 @@
|
||||
<subsystem name="imu" type="lisa_mx_v2.1"/>
|
||||
<subsystem name="gps" type="ublox"/>
|
||||
<subsystem name="stabilization" type="int_quat"/>
|
||||
<subsystem name="stabilization" type="rate"/>
|
||||
<subsystem name="ahrs" type="float_mlkf">
|
||||
<!-- Uncomment to enable all axis update from the Magnetometer. Do
|
||||
it only if you have a very well calibrated and tested
|
||||
|
||||
@@ -116,7 +116,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/estimation/ahrs_float_mlkf.xml settings/nps.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_float_mlkf.xml settings/nps.xml settings/control/stabilization_rate.xml"
|
||||
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
@@ -127,7 +127,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/nps.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/nps.xml"
|
||||
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
|
||||
gui_color="white"
|
||||
/>
|
||||
|
||||
@@ -99,7 +99,6 @@ include $(CFG_SHARED)/baro_board.makefile
|
||||
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization.c
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_none.c
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c
|
||||
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
STAB_ATT_CFLAGS = -DSTABILIZATION_ATTITUDE_TYPE_INT
|
||||
STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_indi.h\"
|
||||
|
||||
STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_indi.c
|
||||
STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_indi.c
|
||||
STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_transformations.c
|
||||
STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c
|
||||
|
||||
@@ -0,0 +1,8 @@
|
||||
STAB_RATE_CFLAGS = -DUSE_STABILIZATION_RATE
|
||||
STAB_RATE_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_rate.c
|
||||
|
||||
ap.CFLAGS += $(STAB_RATE_CFLAGS)
|
||||
ap.srcs += $(STAB_RATE_SRCS)
|
||||
|
||||
nps.CFLAGS += $(STAB_RATE_CFLAGS)
|
||||
nps.srcs += $(STAB_RATE_SRCS)
|
||||
@@ -0,0 +1,9 @@
|
||||
STAB_RATE_CFLAGS = -DUSE_STABILIZATION_RATE
|
||||
STAB_RATE_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_indi.c
|
||||
STAB_RATE_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_rate_indi.c
|
||||
|
||||
ap.CFLAGS += $(STAB_RATE_CFLAGS)
|
||||
ap.srcs += $(STAB_RATE_SRCS)
|
||||
|
||||
nps.CFLAGS += $(STAB_RATE_CFLAGS)
|
||||
nps.srcs += $(STAB_RATE_SRCS)
|
||||
@@ -15,6 +15,7 @@
|
||||
<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>
|
||||
|
||||
@@ -41,9 +41,12 @@
|
||||
|
||||
#include "firmwares/rotorcraft/stabilization.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
|
||||
|
||||
#if USE_STABILIZATION_RATE
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
|
||||
#endif
|
||||
|
||||
#include "generated/settings.h"
|
||||
|
||||
#if USE_GPS
|
||||
@@ -306,7 +309,9 @@ void autopilot_init(void)
|
||||
|
||||
stabilization_init();
|
||||
stabilization_none_init();
|
||||
#if USE_STABILIZATION_RATE
|
||||
stabilization_rate_init();
|
||||
#endif
|
||||
stabilization_attitude_init();
|
||||
|
||||
/* set startup mode, propagates through to guidance h/v */
|
||||
@@ -417,10 +422,12 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
|
||||
case AP_MODE_RC_DIRECT:
|
||||
guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
|
||||
break;
|
||||
#if USE_STABILIZATION_RATE
|
||||
case AP_MODE_RATE_DIRECT:
|
||||
case AP_MODE_RATE_Z_HOLD:
|
||||
guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
|
||||
break;
|
||||
#endif
|
||||
case AP_MODE_ATTITUDE_RC_CLIMB:
|
||||
case AP_MODE_ATTITUDE_DIRECT:
|
||||
case AP_MODE_ATTITUDE_CLIMB:
|
||||
|
||||
@@ -229,9 +229,11 @@ void guidance_h_mode_changed(uint8_t new_mode)
|
||||
stabilization_none_enter();
|
||||
break;
|
||||
|
||||
#if USE_STABILIZATION_RATE
|
||||
case GUIDANCE_H_MODE_RATE:
|
||||
stabilization_rate_enter();
|
||||
break;
|
||||
#endif
|
||||
|
||||
case GUIDANCE_H_MODE_CARE_FREE:
|
||||
stabilization_attitude_reset_care_free_heading();
|
||||
@@ -299,6 +301,7 @@ void guidance_h_read_rc(bool_t in_flight)
|
||||
stabilization_none_read_rc();
|
||||
break;
|
||||
|
||||
#if USE_STABILIZATION_RATE
|
||||
case GUIDANCE_H_MODE_RATE:
|
||||
#if SWITCH_STICKS_FOR_RATE_CONTROL
|
||||
stabilization_rate_read_rc_switched_sticks();
|
||||
@@ -306,6 +309,8 @@ void guidance_h_read_rc(bool_t in_flight)
|
||||
stabilization_rate_read_rc();
|
||||
#endif
|
||||
break;
|
||||
#endif
|
||||
|
||||
case GUIDANCE_H_MODE_CARE_FREE:
|
||||
stabilization_attitude_read_rc(in_flight, TRUE, FALSE);
|
||||
break;
|
||||
@@ -352,9 +357,11 @@ void guidance_h_run(bool_t in_flight)
|
||||
stabilization_none_run(in_flight);
|
||||
break;
|
||||
|
||||
#if USE_STABILIZATION_RATE
|
||||
case GUIDANCE_H_MODE_RATE:
|
||||
stabilization_rate_run(in_flight);
|
||||
break;
|
||||
#endif
|
||||
|
||||
case GUIDANCE_H_MODE_FORWARD:
|
||||
if (transition_percentage < (100 << INT32_PERCENTAGE_FRAC)) {
|
||||
|
||||
@@ -30,331 +30,39 @@
|
||||
*/
|
||||
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
|
||||
|
||||
#include "state.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "paparazzi.h"
|
||||
|
||||
#if !defined(STABILIZATION_INDI_ACT_DYN_P) && !defined(STABILIZATION_INDI_ACT_DYN_Q) && !defined(STABILIZATION_INDI_ACT_DYN_R)
|
||||
#error You have to define the first order time constant of the actuator dynamics!
|
||||
#endif
|
||||
|
||||
int32_t stabilization_att_indi_cmd[COMMANDS_NB];
|
||||
|
||||
struct FloatRates g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R};
|
||||
float g2 = STABILIZATION_INDI_G2_R;
|
||||
struct ReferenceSystem reference_acceleration = {STABILIZATION_INDI_REF_ERR_P,
|
||||
STABILIZATION_INDI_REF_ERR_Q,
|
||||
STABILIZATION_INDI_REF_ERR_R,
|
||||
STABILIZATION_INDI_REF_RATE_P,
|
||||
STABILIZATION_INDI_REF_RATE_Q,
|
||||
STABILIZATION_INDI_REF_RATE_R,
|
||||
};
|
||||
|
||||
struct IndiVariables indi = {
|
||||
{0., 0., 0.},
|
||||
{0., 0., 0.},
|
||||
{0., 0., 0.},
|
||||
{0., 0., 0.},
|
||||
{0., 0., 0.},
|
||||
{0., 0., 0.},
|
||||
{0., 0., 0.},
|
||||
{0., 0., 0.},
|
||||
{0., 0., 0.},
|
||||
{0., 0., 0.}
|
||||
};
|
||||
|
||||
struct Int32Eulers stab_att_sp_euler;
|
||||
struct Int32Quat stab_att_sp_quat;
|
||||
|
||||
// Variables for adaptation
|
||||
struct FloatRates filtered_rate_estimation = {0., 0., 0.};
|
||||
struct FloatRates filtered_rate_deriv_estimation = {0., 0., 0.};
|
||||
struct FloatRates filtered_rate_2deriv_estimation = {0., 0., 0.};
|
||||
struct FloatRates indi_u_estimation = {0., 0., 0.};
|
||||
struct FloatRates udot_estimation = {0., 0., 0.};
|
||||
struct FloatRates udotdot_estimation = {0., 0., 0.};
|
||||
#define INDI_EST_SCALE 0.001 //The G values are scaled to avoid numerical problems during the estimation
|
||||
struct FloatRates g_est = {STABILIZATION_INDI_G1_P / INDI_EST_SCALE, STABILIZATION_INDI_G1_Q / INDI_EST_SCALE, STABILIZATION_INDI_G1_R / INDI_EST_SCALE};
|
||||
float g2_est = STABILIZATION_INDI_G2_R / INDI_EST_SCALE;
|
||||
float mu = STABILIZATION_INDI_ADAPTIVE_MU;
|
||||
|
||||
#if STABILIZATION_INDI_USE_ADAPTIVE
|
||||
#warning "Use caution with adaptive indi. See the wiki for more info"
|
||||
bool_t use_adaptive_indi = TRUE;
|
||||
#else
|
||||
bool_t use_adaptive_indi = FALSE;
|
||||
#endif
|
||||
|
||||
#ifndef STABILIZATION_INDI_FILT_OMEGA
|
||||
#define STABILIZATION_INDI_FILT_OMEGA 50.0
|
||||
#endif
|
||||
|
||||
#ifndef STABILIZATION_INDI_FILT_ZETA
|
||||
#define STABILIZATION_INDI_FILT_ZETA 0.55
|
||||
#endif
|
||||
|
||||
#define STABILIZATION_INDI_FILT_OMEGA2 (STABILIZATION_INDI_FILT_OMEGA*STABILIZATION_INDI_FILT_OMEGA)
|
||||
|
||||
#ifndef STABILIZATION_INDI_FILT_OMEGA_R
|
||||
#define STABILIZATION_INDI_FILT_OMEGA_R STABILIZATION_INDI_FILT_OMEGA
|
||||
#define STABILIZATION_INDI_FILT_ZETA_R STABILIZATION_INDI_FILT_ZETA
|
||||
#endif
|
||||
|
||||
#define STABILIZATION_INDI_FILT_OMEGA2_R (STABILIZATION_INDI_FILT_OMEGA_R*STABILIZATION_INDI_FILT_OMEGA_R)
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
|
||||
static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
|
||||
{
|
||||
//The estimated G values are scaled, so scale them back before sending
|
||||
struct FloatRates g_est_disp;
|
||||
RATES_SMUL(g_est_disp, g_est, INDI_EST_SCALE);
|
||||
float g2_est_disp = g2_est * INDI_EST_SCALE;
|
||||
|
||||
pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID,
|
||||
&indi.filtered_rate_deriv.p,
|
||||
&indi.filtered_rate_deriv.q,
|
||||
&indi.filtered_rate_deriv.r,
|
||||
&indi.angular_accel_ref.p,
|
||||
&indi.angular_accel_ref.q,
|
||||
&indi.angular_accel_ref.r,
|
||||
&g_est_disp.p,
|
||||
&g_est_disp.q,
|
||||
&g_est_disp.r,
|
||||
&g2_est_disp);
|
||||
}
|
||||
#endif
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
|
||||
|
||||
void stabilization_attitude_init(void)
|
||||
{
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INDI, send_att_indi);
|
||||
#endif
|
||||
stabilization_indi_init();
|
||||
}
|
||||
|
||||
void stabilization_attitude_enter(void)
|
||||
{
|
||||
|
||||
/* reset psi setpoint to current psi angle */
|
||||
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
|
||||
|
||||
FLOAT_RATES_ZERO(indi.filtered_rate);
|
||||
FLOAT_RATES_ZERO(indi.filtered_rate_deriv);
|
||||
FLOAT_RATES_ZERO(indi.filtered_rate_2deriv);
|
||||
FLOAT_RATES_ZERO(indi.angular_accel_ref);
|
||||
FLOAT_RATES_ZERO(indi.u);
|
||||
FLOAT_RATES_ZERO(indi.du);
|
||||
FLOAT_RATES_ZERO(indi.u_act_dyn);
|
||||
FLOAT_RATES_ZERO(indi.u_in);
|
||||
FLOAT_RATES_ZERO(indi.udot);
|
||||
FLOAT_RATES_ZERO(indi.udotdot);
|
||||
|
||||
stabilization_indi_enter();
|
||||
}
|
||||
|
||||
void stabilization_attitude_set_failsafe_setpoint(void)
|
||||
{
|
||||
/* set failsafe to zero roll/pitch and current heading */
|
||||
int32_t heading2 = stabilization_attitude_get_heading_i() / 2;
|
||||
PPRZ_ITRIG_COS(stab_att_sp_quat.qi, heading2);
|
||||
stab_att_sp_quat.qx = 0;
|
||||
stab_att_sp_quat.qy = 0;
|
||||
PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
|
||||
stabilization_indi_set_failsafe_setpoint();
|
||||
}
|
||||
|
||||
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
|
||||
{
|
||||
// stab_att_sp_euler.psi still used in ref..
|
||||
stab_att_sp_euler = *rpy;
|
||||
|
||||
int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
|
||||
stabilization_indi_set_rpy_setpoint_i(rpy);
|
||||
}
|
||||
|
||||
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
|
||||
{
|
||||
// stab_att_sp_euler.psi still used in ref..
|
||||
stab_att_sp_euler.psi = heading;
|
||||
|
||||
// compute sp_euler phi/theta for debugging/telemetry
|
||||
/* Rotate horizontal commands to body frame by psi */
|
||||
int32_t psi = stateGetNedToBodyEulers_i()->psi;
|
||||
int32_t s_psi, c_psi;
|
||||
PPRZ_ITRIG_SIN(s_psi, psi);
|
||||
PPRZ_ITRIG_COS(c_psi, psi);
|
||||
stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
|
||||
stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
|
||||
|
||||
quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
|
||||
}
|
||||
|
||||
static void attitude_run_indi(int32_t indi_commands[], struct Int32Quat *att_err, bool_t in_flight __attribute__((unused)))
|
||||
{
|
||||
//Calculate required angular acceleration
|
||||
struct FloatRates *body_rate = stateGetBodyRates_f();
|
||||
#if STABILIZATION_INDI_FILTER_ROLL_RATE
|
||||
indi.angular_accel_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
|
||||
- reference_acceleration.rate_p * indi.filtered_rate.p;
|
||||
#else
|
||||
indi.angular_accel_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
|
||||
- reference_acceleration.rate_p * body_rate->p;
|
||||
#endif
|
||||
#if STABILIZATION_INDI_FILTER_PITCH_RATE
|
||||
indi.angular_accel_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
|
||||
- reference_acceleration.rate_q * indi.filtered_rate.q;
|
||||
#else
|
||||
indi.angular_accel_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
|
||||
- reference_acceleration.rate_q * body_rate->q;
|
||||
#endif
|
||||
#if STABILIZATION_INDI_FILTER_YAW_RATE
|
||||
indi.angular_accel_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
|
||||
- reference_acceleration.rate_r * indi.filtered_rate.r;
|
||||
#else
|
||||
indi.angular_accel_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
|
||||
- reference_acceleration.rate_r * body_rate->r;
|
||||
#endif
|
||||
|
||||
//Incremented in angular acceleration requires increment in control input
|
||||
//G1 is the actuator effectiveness. In the yaw axis, we need something additional: G2.
|
||||
//It takes care of the angular acceleration caused by the change in rotation rate of the propellers
|
||||
//(they have significant inertia, see the paper mentioned in the header for more explanation)
|
||||
indi.du.p = 1.0 / g1.p * (indi.angular_accel_ref.p - indi.filtered_rate_deriv.p);
|
||||
indi.du.q = 1.0 / g1.q * (indi.angular_accel_ref.q - indi.filtered_rate_deriv.q);
|
||||
indi.du.r = 1.0 / (g1.r + g2) * (indi.angular_accel_ref.r - indi.filtered_rate_deriv.r + g2 * indi.du.r);
|
||||
|
||||
//add the increment to the total control input
|
||||
indi.u_in.p = indi.u.p + indi.du.p;
|
||||
indi.u_in.q = indi.u.q + indi.du.q;
|
||||
indi.u_in.r = indi.u.r + indi.du.r;
|
||||
|
||||
//bound the total control input
|
||||
Bound(indi.u_in.p, -4500, 4500);
|
||||
Bound(indi.u_in.q, -4500, 4500);
|
||||
Bound(indi.u_in.r, -4500, 4500);
|
||||
|
||||
//Propagate input filters
|
||||
//first order actuator dynamics
|
||||
indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
|
||||
indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
|
||||
indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
|
||||
|
||||
//sensor filter
|
||||
stabilization_indi_second_order_filter(&indi.u_act_dyn, &indi.udotdot, &indi.udot, &indi.u,
|
||||
STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);
|
||||
|
||||
//Don't increment if thrust is off
|
||||
if (stabilization_cmd[COMMAND_THRUST] < 300) {
|
||||
FLOAT_RATES_ZERO(indi.u);
|
||||
FLOAT_RATES_ZERO(indi.du);
|
||||
FLOAT_RATES_ZERO(indi.u_act_dyn);
|
||||
FLOAT_RATES_ZERO(indi.u_in);
|
||||
FLOAT_RATES_ZERO(indi.udot);
|
||||
FLOAT_RATES_ZERO(indi.udotdot);
|
||||
} else {
|
||||
lms_estimation();
|
||||
}
|
||||
|
||||
/* INDI feedback */
|
||||
indi_commands[COMMAND_ROLL] = indi.u_in.p;
|
||||
indi_commands[COMMAND_PITCH] = indi.u_in.q;
|
||||
indi_commands[COMMAND_YAW] = indi.u_in.r;
|
||||
stabilization_indi_set_earth_cmd_i(cmd, heading);
|
||||
}
|
||||
|
||||
void stabilization_attitude_run(bool_t enable_integrator)
|
||||
{
|
||||
|
||||
/* Propagate the second order filter on the gyroscopes */
|
||||
struct FloatRates *body_rates = stateGetBodyRates_f();
|
||||
stabilization_indi_second_order_filter(body_rates, &indi.filtered_rate_2deriv, &indi.filtered_rate_deriv,
|
||||
&indi.filtered_rate, STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);
|
||||
|
||||
/* attitude error */
|
||||
struct Int32Quat att_err;
|
||||
struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
|
||||
// INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_sp_quat);
|
||||
int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
|
||||
/* wrap it in the shortest direction */
|
||||
int32_quat_wrap_shortest(&att_err);
|
||||
int32_quat_normalize(&att_err);
|
||||
|
||||
/* compute the INDI command */
|
||||
attitude_run_indi(stabilization_att_indi_cmd, &att_err, enable_integrator);
|
||||
|
||||
stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL];
|
||||
stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH];
|
||||
stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_cmd[COMMAND_YAW];
|
||||
|
||||
/* bound the result */
|
||||
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
|
||||
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
|
||||
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
|
||||
stabilization_indi_run(enable_integrator, FALSE);
|
||||
}
|
||||
|
||||
// This function reads rc commands
|
||||
void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
|
||||
{
|
||||
struct FloatQuat q_sp;
|
||||
#if USE_EARTH_BOUND_RC_SETPOINT
|
||||
stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
|
||||
#else
|
||||
stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
|
||||
#endif
|
||||
QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
|
||||
}
|
||||
|
||||
// This is a simple second order low pass filter
|
||||
void stabilization_indi_second_order_filter(struct FloatRates *input, struct FloatRates *filter_ddx,
|
||||
struct FloatRates *filter_dx, struct FloatRates *filter_x, float omega, float zeta, float omega_r)
|
||||
{
|
||||
float_rates_integrate_fi(filter_x, filter_dx, 1.0 / PERIODIC_FREQUENCY);
|
||||
float_rates_integrate_fi(filter_dx, filter_ddx, 1.0 / PERIODIC_FREQUENCY);
|
||||
float omega2 = omega * omega;
|
||||
float omega2_r = omega_r * omega_r;
|
||||
|
||||
filter_ddx->p = -filter_dx->p * 2 * zeta * omega + (input->p - filter_x->p) * omega2; \
|
||||
filter_ddx->q = -filter_dx->q * 2 * zeta * omega + (input->q - filter_x->q) * omega2; \
|
||||
filter_ddx->r = -filter_dx->r * 2 * zeta * omega_r + (input->r - filter_x->r) * omega2_r;
|
||||
}
|
||||
|
||||
// This is a Least Mean Squares adaptive filter
|
||||
// It estiamtes the actuator effectiveness online by comparing the expected angular acceleration based on the inputs with the measured angular acceleration
|
||||
void lms_estimation(void)
|
||||
{
|
||||
|
||||
// Only pass really low frequencies so you don't adapt to noise
|
||||
float omega = 10.0;
|
||||
float zeta = 0.8;
|
||||
stabilization_indi_second_order_filter(&indi.u_act_dyn, &udotdot_estimation, &udot_estimation, &indi_u_estimation,
|
||||
omega, zeta, omega);
|
||||
struct FloatRates *body_rates = stateGetBodyRates_f();
|
||||
stabilization_indi_second_order_filter(body_rates, &filtered_rate_2deriv_estimation, &filtered_rate_deriv_estimation,
|
||||
&filtered_rate_estimation, omega, zeta, omega);
|
||||
|
||||
// The inputs are scaled in order to avoid overflows
|
||||
float du = udot_estimation.p * INDI_EST_SCALE;
|
||||
g_est.p = g_est.p - (g_est.p * du - filtered_rate_2deriv_estimation.p) * du * mu;
|
||||
du = udot_estimation.q * INDI_EST_SCALE;
|
||||
g_est.q = g_est.q - (g_est.q * du - filtered_rate_2deriv_estimation.q) * du * mu;
|
||||
du = udot_estimation.r * INDI_EST_SCALE;
|
||||
float ddu = udotdot_estimation.r * INDI_EST_SCALE / PERIODIC_FREQUENCY;
|
||||
float error = (g_est.r * du + g2_est * ddu - filtered_rate_2deriv_estimation.r);
|
||||
g_est.r = g_est.r - error * du * mu / 3;
|
||||
g2_est = g2_est - error * 1000 * ddu * mu / 3;
|
||||
|
||||
//the g values should be larger than zero, otherwise there is positive feedback, the command will go to max and there is nothing to learn anymore...
|
||||
if (g_est.p < 0.01) { g_est.p = 0.01; }
|
||||
if (g_est.q < 0.01) { g_est.q = 0.01; }
|
||||
if (g_est.r < 0.01) { g_est.r = 0.01; }
|
||||
if (g2_est < 0.01) { g2_est = 0.01; }
|
||||
|
||||
if (use_adaptive_indi) {
|
||||
//Commit the estimated G values and apply the scaling
|
||||
g1.p = g_est.p * INDI_EST_SCALE;
|
||||
g1.q = g_est.q * INDI_EST_SCALE;
|
||||
g1.r = g_est.r * INDI_EST_SCALE;
|
||||
g2 = g2_est * INDI_EST_SCALE;
|
||||
}
|
||||
stabilization_indi_read_rc(in_flight, in_carefree, coordinated_turn);
|
||||
}
|
||||
|
||||
@@ -33,43 +33,7 @@
|
||||
#ifndef STABILIZATION_ATTITUDE_QUAT_INDI_H
|
||||
#define STABILIZATION_ATTITUDE_QUAT_INDI_H
|
||||
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
|
||||
|
||||
struct ReferenceSystem {
|
||||
float err_p;
|
||||
float err_q;
|
||||
float err_r;
|
||||
float rate_p;
|
||||
float rate_q;
|
||||
float rate_r;
|
||||
};
|
||||
|
||||
struct IndiVariables {
|
||||
struct FloatRates filtered_rate;
|
||||
struct FloatRates filtered_rate_deriv;
|
||||
struct FloatRates filtered_rate_2deriv;
|
||||
struct FloatRates angular_accel_ref;
|
||||
struct FloatRates du;
|
||||
struct FloatRates u_act_dyn;
|
||||
struct FloatRates u_in;
|
||||
struct FloatRates u;
|
||||
struct FloatRates udot;
|
||||
struct FloatRates udotdot;
|
||||
};
|
||||
|
||||
extern struct FloatRates g1;
|
||||
extern float g2;
|
||||
extern struct ReferenceSystem reference_acceleration;
|
||||
|
||||
extern struct FloatRates g_est;
|
||||
extern bool_t use_adaptive_indi;
|
||||
|
||||
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
|
||||
extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
|
||||
|
||||
void stabilization_indi_second_order_filter(struct FloatRates *input, struct FloatRates *filter_ddx,
|
||||
struct FloatRates *filter_dx, struct FloatRates *filter_x, float omega, float zeta, float omega_r);
|
||||
void lms_estimation(void);
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
|
||||
|
||||
#endif /* STABILIZATION_ATTITUDE_QUAT_INT_H */
|
||||
|
||||
|
||||
@@ -0,0 +1,370 @@
|
||||
/*
|
||||
* Copyright (C) Ewoud Smeur <ewoud_smeur@msn.com>
|
||||
* MAVLab Delft University of Technology
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @file stabilization_attitude_quat_indi.c
|
||||
* MAVLab Delft University of Technology
|
||||
* This control algorithm is Incremental Nonlinear Dynamic Inversion (INDI)
|
||||
*
|
||||
* This is a simplified implementation of the (soon to be) publication in the
|
||||
* journal of Control Guidance and Dynamics: Adaptive Incremental Nonlinear
|
||||
* Dynamic Inversion for Attitude Control of Micro Aerial Vehicles
|
||||
*/
|
||||
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
|
||||
|
||||
#include "state.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "paparazzi.h"
|
||||
#include "subsystems/radio_control.h"
|
||||
|
||||
#if !defined(STABILIZATION_INDI_ACT_DYN_P) && !defined(STABILIZATION_INDI_ACT_DYN_Q) && !defined(STABILIZATION_INDI_ACT_DYN_R)
|
||||
#error You have to define the first order time constant of the actuator dynamics!
|
||||
#endif
|
||||
|
||||
#ifndef STABILIZATION_INDI_FILT_OMEGA
|
||||
#define STABILIZATION_INDI_FILT_OMEGA 50.0
|
||||
#endif
|
||||
|
||||
#ifndef STABILIZATION_INDI_FILT_ZETA
|
||||
#define STABILIZATION_INDI_FILT_ZETA 0.55
|
||||
#endif
|
||||
|
||||
#ifndef STABILIZATION_INDI_FILT_OMEGA_R
|
||||
#define STABILIZATION_INDI_FILT_OMEGA_R STABILIZATION_INDI_FILT_OMEGA
|
||||
#endif
|
||||
|
||||
#ifndef STABILIZATION_INDI_FILT_ZETA_R
|
||||
#define STABILIZATION_INDI_FILT_ZETA_R STABILIZATION_INDI_FILT_ZETA
|
||||
#endif
|
||||
|
||||
#ifndef STABILIZATION_INDI_MAX_RATE
|
||||
#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;
|
||||
struct Int32Quat stab_att_sp_quat;
|
||||
|
||||
static int32_t stabilization_att_indi_cmd[COMMANDS_NB];
|
||||
static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct Int32Quat *att_err, bool_t rate_control);
|
||||
static void stabilization_indi_second_order_filter_init(struct IndiFilter *filter, float omega, float zeta, float omega_r);
|
||||
static void stabilization_indi_second_order_filter(struct IndiFilter *filter, struct FloatRates *input);
|
||||
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 = {
|
||||
.max_rate = STABILIZATION_INDI_MAX_RATE,
|
||||
|
||||
.g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
|
||||
.g2 = STABILIZATION_INDI_G2_R,
|
||||
.reference_acceleration = {
|
||||
STABILIZATION_INDI_REF_ERR_P,
|
||||
STABILIZATION_INDI_REF_ERR_Q,
|
||||
STABILIZATION_INDI_REF_ERR_R,
|
||||
STABILIZATION_INDI_REF_RATE_P,
|
||||
STABILIZATION_INDI_REF_RATE_Q,
|
||||
STABILIZATION_INDI_REF_RATE_R},
|
||||
|
||||
/* Estimation parameters for adaptive INDI */
|
||||
.est = {
|
||||
.g1 = {
|
||||
STABILIZATION_INDI_G1_P / INDI_EST_SCALE,
|
||||
STABILIZATION_INDI_G1_Q / INDI_EST_SCALE,
|
||||
STABILIZATION_INDI_G1_R / INDI_EST_SCALE},
|
||||
.g2 = STABILIZATION_INDI_G2_R / INDI_EST_SCALE,
|
||||
.mu = STABILIZATION_INDI_ADAPTIVE_MU,
|
||||
}
|
||||
};
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
|
||||
static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
|
||||
{
|
||||
//The estimated G values are scaled, so scale them back before sending
|
||||
struct FloatRates g1_disp;
|
||||
RATES_SMUL(g1_disp, indi.est.g1, INDI_EST_SCALE);
|
||||
float g2_disp = indi.est.g2 * INDI_EST_SCALE;
|
||||
|
||||
pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID,
|
||||
&indi.rate.dx.p,
|
||||
&indi.rate.dx.q,
|
||||
&indi.rate.dx.r,
|
||||
&indi.angular_accel_ref.p,
|
||||
&indi.angular_accel_ref.q,
|
||||
&indi.angular_accel_ref.r,
|
||||
&g1_disp.p,
|
||||
&g1_disp.q,
|
||||
&g1_disp.r,
|
||||
&g2_disp);
|
||||
}
|
||||
#endif
|
||||
|
||||
void stabilization_indi_init(void)
|
||||
{
|
||||
// Initialize filters
|
||||
stabilization_indi_second_order_filter_init(&indi.rate, STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);
|
||||
stabilization_indi_second_order_filter_init(&indi.u, STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);
|
||||
stabilization_indi_second_order_filter_init(&indi.est.rate, 10.0, 0.8, 10.0); //FIXME: no magic number
|
||||
stabilization_indi_second_order_filter_init(&indi.est.u, 10.0, 0.8, 10.0); //FIXME: no magic number
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INDI, send_att_indi);
|
||||
#endif
|
||||
}
|
||||
|
||||
void stabilization_indi_enter(void)
|
||||
{
|
||||
/* reset psi setpoint to current psi angle */
|
||||
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
|
||||
|
||||
FLOAT_RATES_ZERO(indi.rate.x);
|
||||
FLOAT_RATES_ZERO(indi.rate.dx);
|
||||
FLOAT_RATES_ZERO(indi.rate.ddx);
|
||||
FLOAT_RATES_ZERO(indi.angular_accel_ref);
|
||||
FLOAT_RATES_ZERO(indi.du);
|
||||
FLOAT_RATES_ZERO(indi.u_act_dyn);
|
||||
FLOAT_RATES_ZERO(indi.u_in);
|
||||
FLOAT_RATES_ZERO(indi.u.x);
|
||||
FLOAT_RATES_ZERO(indi.u.dx);
|
||||
FLOAT_RATES_ZERO(indi.u.ddx);
|
||||
}
|
||||
|
||||
void stabilization_indi_set_failsafe_setpoint(void)
|
||||
{
|
||||
/* set failsafe to zero roll/pitch and current heading */
|
||||
int32_t heading2 = stabilization_attitude_get_heading_i() / 2;
|
||||
PPRZ_ITRIG_COS(stab_att_sp_quat.qi, heading2);
|
||||
stab_att_sp_quat.qx = 0;
|
||||
stab_att_sp_quat.qy = 0;
|
||||
PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
|
||||
}
|
||||
|
||||
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
|
||||
{
|
||||
// stab_att_sp_euler.psi still used in ref..
|
||||
stab_att_sp_euler = *rpy;
|
||||
|
||||
quat_from_rpy_cmd_i(&stab_att_sp_quat, &stab_att_sp_euler);
|
||||
}
|
||||
|
||||
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
|
||||
{
|
||||
// stab_att_sp_euler.psi still used in ref..
|
||||
stab_att_sp_euler.psi = heading;
|
||||
|
||||
// compute sp_euler phi/theta for debugging/telemetry
|
||||
/* Rotate horizontal commands to body frame by psi */
|
||||
int32_t psi = stateGetNedToBodyEulers_i()->psi;
|
||||
int32_t s_psi, c_psi;
|
||||
PPRZ_ITRIG_SIN(s_psi, psi);
|
||||
PPRZ_ITRIG_COS(c_psi, psi);
|
||||
stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
|
||||
stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
|
||||
|
||||
quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
|
||||
}
|
||||
|
||||
static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct Int32Quat *att_err, bool_t rate_control)
|
||||
{
|
||||
/* Propagate the second order filter on the gyroscopes */
|
||||
struct FloatRates *body_rates = stateGetBodyRates_f();
|
||||
stabilization_indi_second_order_filter(&indi.rate, body_rates);
|
||||
|
||||
#if STABILIZATION_INDI_FILTER_ROLL_RATE
|
||||
indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
|
||||
- indi.reference_acceleration.rate_p * indi.rate.x.p;
|
||||
#else
|
||||
indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
|
||||
- indi.reference_acceleration.rate_p * body_rates->p;
|
||||
#endif
|
||||
#if STABILIZATION_INDI_FILTER_PITCH_RATE
|
||||
indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
|
||||
- indi.reference_acceleration.rate_q * indi.rate.x.q;
|
||||
#else
|
||||
indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
|
||||
- indi.reference_acceleration.rate_q * body_rates->q;
|
||||
#endif
|
||||
#if STABILIZATION_INDI_FILTER_YAW_RATE
|
||||
indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
|
||||
- indi.reference_acceleration.rate_r * indi.rate.x.r;
|
||||
#else
|
||||
indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
|
||||
- indi.reference_acceleration.rate_r * body_rates->r;
|
||||
#endif
|
||||
|
||||
/* Check if we are running the rate controller and overwrite */
|
||||
if(rate_control) {
|
||||
indi.angular_accel_ref.p = indi.reference_acceleration.rate_p * ((float)radio_control.values[RADIO_ROLL] / MAX_PPRZ * indi.max_rate - body_rates->p);
|
||||
indi.angular_accel_ref.q = indi.reference_acceleration.rate_q * ((float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * indi.max_rate - body_rates->q);
|
||||
indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * ((float)radio_control.values[RADIO_YAW] / MAX_PPRZ * indi.max_rate - body_rates->r);
|
||||
}
|
||||
|
||||
//Incremented in angular acceleration requires increment in control input
|
||||
//G1 is the actuator effectiveness. In the yaw axis, we need something additional: G2.
|
||||
//It takes care of the angular acceleration caused by the change in rotation rate of the propellers
|
||||
//(they have significant inertia, see the paper mentioned in the header for more explanation)
|
||||
indi.du.p = 1.0 / indi.g1.p * (indi.angular_accel_ref.p - indi.rate.dx.p);
|
||||
indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate.dx.q);
|
||||
indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate.dx.r + indi.g2 * indi.du.r);
|
||||
|
||||
//add the increment to the total control input
|
||||
indi.u_in.p = indi.u.x.p + indi.du.p;
|
||||
indi.u_in.q = indi.u.x.q + indi.du.q;
|
||||
indi.u_in.r = indi.u.x.r + indi.du.r;
|
||||
|
||||
//bound the total control input
|
||||
Bound(indi.u_in.p, -4500, 4500);
|
||||
Bound(indi.u_in.q, -4500, 4500);
|
||||
Bound(indi.u_in.r, -4500, 4500);
|
||||
|
||||
//Propagate input filters
|
||||
//first order actuator dynamics
|
||||
indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
|
||||
indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
|
||||
indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
|
||||
|
||||
//sensor filter
|
||||
stabilization_indi_second_order_filter(&indi.u, &indi.u_act_dyn);
|
||||
|
||||
//Don't increment if thrust is off
|
||||
if (stabilization_cmd[COMMAND_THRUST] < 300) {
|
||||
FLOAT_RATES_ZERO(indi.du);
|
||||
FLOAT_RATES_ZERO(indi.u_act_dyn);
|
||||
FLOAT_RATES_ZERO(indi.u_in);
|
||||
FLOAT_RATES_ZERO(indi.u.x);
|
||||
FLOAT_RATES_ZERO(indi.u.dx);
|
||||
FLOAT_RATES_ZERO(indi.u.ddx);
|
||||
} else {
|
||||
lms_estimation();
|
||||
}
|
||||
|
||||
/* INDI feedback */
|
||||
indi_commands[COMMAND_ROLL] = indi.u_in.p;
|
||||
indi_commands[COMMAND_PITCH] = indi.u_in.q;
|
||||
indi_commands[COMMAND_YAW] = indi.u_in.r;
|
||||
}
|
||||
|
||||
void stabilization_indi_run(bool_t enable_integrator __attribute__((unused)), bool_t rate_control)
|
||||
{
|
||||
/* attitude error */
|
||||
struct Int32Quat att_err;
|
||||
struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
|
||||
int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
|
||||
/* wrap it in the shortest direction */
|
||||
int32_quat_wrap_shortest(&att_err);
|
||||
int32_quat_normalize(&att_err);
|
||||
|
||||
/* compute the INDI command */
|
||||
stabilization_indi_calc_cmd(stabilization_att_indi_cmd, &att_err, rate_control);
|
||||
|
||||
/* copy the INDI command */
|
||||
stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL];
|
||||
stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH];
|
||||
stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_cmd[COMMAND_YAW];
|
||||
|
||||
/* bound the result */
|
||||
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
|
||||
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
|
||||
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
|
||||
}
|
||||
|
||||
// This function reads rc commands
|
||||
void stabilization_indi_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
|
||||
{
|
||||
struct FloatQuat q_sp;
|
||||
#if USE_EARTH_BOUND_RC_SETPOINT
|
||||
stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
|
||||
#else
|
||||
stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
|
||||
#endif
|
||||
QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
|
||||
}
|
||||
|
||||
// Initialize a second order low pass filter
|
||||
static void stabilization_indi_second_order_filter_init(struct IndiFilter *filter, float omega, float zeta, float omega_r)
|
||||
{
|
||||
filter->omega = omega;
|
||||
filter->omega2 = omega * omega;
|
||||
filter->zeta = zeta;
|
||||
filter->omega_r = omega_r;
|
||||
filter->omega2_r = omega_r * omega_r;
|
||||
}
|
||||
|
||||
// This is a simple second order low pass filter
|
||||
static void stabilization_indi_second_order_filter(struct IndiFilter *filter, struct FloatRates *input)
|
||||
{
|
||||
float_rates_integrate_fi(&filter->x, &filter->dx, 1.0 / PERIODIC_FREQUENCY);
|
||||
float_rates_integrate_fi(&filter->dx, &filter->ddx, 1.0 / PERIODIC_FREQUENCY);
|
||||
|
||||
filter->ddx.p = -filter->dx.p * 2 * filter->zeta * filter->omega + (input->p - filter->x.p) * filter->omega2;
|
||||
filter->ddx.q = -filter->dx.q * 2 * filter->zeta * filter->omega + (input->q - filter->x.q) * filter->omega2;
|
||||
filter->ddx.r = -filter->dx.r * 2 * filter->zeta * filter->omega_r + (input->r - filter->x.r) * filter->omega2_r;
|
||||
}
|
||||
|
||||
// This is a Least Mean Squares adaptive filter
|
||||
// It estiamtes the actuator effectiveness online by comparing the expected angular acceleration based on the inputs with the measured angular acceleration
|
||||
static inline void lms_estimation(void)
|
||||
{
|
||||
static struct IndiEstimation *est = &indi.est;
|
||||
// Only pass really low frequencies so you don't adapt to noise
|
||||
stabilization_indi_second_order_filter(&est->u, &indi.u_act_dyn);
|
||||
struct FloatRates *body_rates = stateGetBodyRates_f();
|
||||
stabilization_indi_second_order_filter(&est->rate, body_rates);
|
||||
|
||||
// The inputs are scaled in order to avoid overflows
|
||||
float du = est->u.dx.p * INDI_EST_SCALE;
|
||||
est->g1.p = est->g1.p - (est->g1.p * du - est->rate.ddx.p) * du * est->mu;
|
||||
du = est->u.dx.q * INDI_EST_SCALE;
|
||||
est->g1.q = est->g1.q - (est->g1.q * du - est->rate.ddx.q) * du * est->mu;
|
||||
du = est->u.dx.r * INDI_EST_SCALE;
|
||||
float ddu = est->u.ddx.r * INDI_EST_SCALE / PERIODIC_FREQUENCY;
|
||||
float error = (est->g1.r * du + est->g2 * ddu - est->rate.ddx.r);
|
||||
est->g1.r = est->g1.r - error * du * est->mu / 3;
|
||||
est->g2 = est->g2 - error * 1000 * ddu * est->mu / 3;
|
||||
|
||||
//the g values should be larger than zero, otherwise there is positive feedback, the command will go to max and there is nothing to learn anymore...
|
||||
if (est->g1.p < 0.01) { est->g1.p = 0.01; }
|
||||
if (est->g1.q < 0.01) { est->g1.q = 0.01; }
|
||||
if (est->g1.r < 0.01) { est->g1.r = 0.01; }
|
||||
if (est->g2 < 0.01) { est->g2 = 0.01; }
|
||||
|
||||
if (use_adaptive_indi) {
|
||||
//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;
|
||||
indi.g1.r = est->g1.r * INDI_EST_SCALE;
|
||||
indi.g2 = est->g2 * INDI_EST_SCALE;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
/*
|
||||
* Copyright (C) Ewoud Smeur <ewoud_smeur@msn.com>
|
||||
* MAVLab Delft University of Technology
|
||||
*
|
||||
* This control algorithm is Incremental Nonlinear Dynamic Inversion (INDI)
|
||||
*
|
||||
* This is a simplified implementation of the (soon to be) publication in the
|
||||
* journal of Control Guidance and Dynamics: Adaptive Incremental Nonlinear
|
||||
* Dynamic Inversion for Attitude Control of Micro Aerial Vehicles
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @file stabilization_attitude_quat_indi.h
|
||||
* Stabilization based on INDI for multicopters.
|
||||
* It supports both rate and attidue control.
|
||||
*/
|
||||
|
||||
#ifndef STABILIZATION_INDI_H
|
||||
#define STABILIZATION_INDI_H
|
||||
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
|
||||
|
||||
extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
|
||||
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
|
||||
|
||||
struct ReferenceSystem {
|
||||
float err_p;
|
||||
float err_q;
|
||||
float err_r;
|
||||
float rate_p;
|
||||
float rate_q;
|
||||
float rate_r;
|
||||
};
|
||||
|
||||
struct IndiFilter {
|
||||
struct FloatRates ddx;
|
||||
struct FloatRates dx;
|
||||
struct FloatRates x;
|
||||
|
||||
float zeta;
|
||||
float omega;
|
||||
float omega_r;
|
||||
float omega2;
|
||||
float omega2_r;
|
||||
};
|
||||
|
||||
struct IndiEstimation {
|
||||
struct IndiFilter u;
|
||||
struct IndiFilter rate;
|
||||
struct FloatRates g1;
|
||||
float g2;
|
||||
float mu;
|
||||
};
|
||||
|
||||
struct IndiVariables {
|
||||
struct FloatRates angular_accel_ref;
|
||||
struct FloatRates du;
|
||||
struct FloatRates u_in;
|
||||
struct FloatRates u_act_dyn;
|
||||
|
||||
struct IndiFilter u;
|
||||
struct IndiFilter rate;
|
||||
struct FloatRates g1;
|
||||
float g2;
|
||||
|
||||
struct ReferenceSystem reference_acceleration;
|
||||
|
||||
float max_rate; ///< Maximum rate in rate control
|
||||
struct IndiEstimation est; ///< Estimation parameters for adaptive INDI
|
||||
};
|
||||
|
||||
|
||||
void stabilization_indi_init(void);
|
||||
void stabilization_indi_enter(void);
|
||||
void stabilization_indi_set_failsafe_setpoint(void);
|
||||
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
|
||||
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
|
||||
void stabilization_indi_run(bool_t enable_integrator, bool_t rate_control);
|
||||
void stabilization_indi_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn);
|
||||
|
||||
#endif /* STABILIZATION_INDI_H */
|
||||
|
||||
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Freek van Tienen <freek.v.tienen@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @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"
|
||||
|
||||
void stabilization_rate_init(void)
|
||||
{
|
||||
stabilization_indi_init();
|
||||
}
|
||||
|
||||
|
||||
void stabilization_rate_read_rc(void)
|
||||
{
|
||||
//FIXME: make a new indi function
|
||||
}
|
||||
|
||||
//Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
|
||||
void stabilization_rate_read_rc_switched_sticks(void)
|
||||
{
|
||||
//FIXME: make a new indi function
|
||||
}
|
||||
|
||||
void stabilization_rate_enter(void)
|
||||
{
|
||||
stabilization_indi_enter();
|
||||
}
|
||||
|
||||
void stabilization_rate_run(bool_t in_flight)
|
||||
{
|
||||
stabilization_indi_run(in_flight, TRUE);
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Freek van Tienen <freek.v.tienen@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @file stabilization_rate_indi.h
|
||||
* Rate stabilization for rotorcrafts based on INDI.
|
||||
*/
|
||||
|
||||
#ifndef STABILIZATION_RATE_INDI
|
||||
#define STABILIZATION_RATE_INDI
|
||||
|
||||
#include "math/pprz_algebra_int.h"
|
||||
|
||||
extern void stabilization_rate_init(void);
|
||||
extern void stabilization_rate_read_rc(void);
|
||||
extern void stabilization_rate_read_rc_switched_sticks(void);
|
||||
extern void stabilization_rate_run(bool_t in_flight);
|
||||
extern void stabilization_rate_enter(void);
|
||||
|
||||
#endif /* STABILIZATION_RATE_INDI */
|
||||
Reference in New Issue
Block a user