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

INDI code cleanup

Browse Source 
Renaming confusing variables
Fixing rate interface
This commit is contained in:
Ewoud Smeur
2020-08-25 23:25:16 +02:00
parent 733ddb06f9
commit a1b2d0ba65
11 changed files with 139 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/stabilization_indi.xml
+12 -12
View File
@@ -21,12 +21,12 @@
<define name="G1_YAW" value="{-1, 1, -1, 1}" description="control effectiveness of every actuator on the yaw axis"/>
<define name="G1_THRUST" value="{-.4, -.4, -.4, -.4}" description="control effectiveness of every actuator on the thrust axis"/>
<define name="G2" value="{-60.0, 60.0, -60.0, 60.0}" description="control effectiveness of every actuator derivative on the yaw axis (important for propellers with strong torque changes)"/>
<define name="REF_ERR_P" value="600.0" description="reference acceleration"/>
<define name="REF_ERR_Q" value="600.0" description="reference acceleration"/>
<define name="REF_ERR_R" value="600.0" description="reference acceleration"/>
<define name="REF_RATE_P" value="28.0" description="reference acceleration"/>
<define name="REF_RATE_Q" value="28.0" description="reference acceleration"/>
<define name="REF_RATE_R" value="28.0" description="reference acceleration"/>
<define name="REF_ERR_P" value="600.0" description="INDI gains linear controller"/>
<define name="REF_ERR_Q" value="600.0" description="INDI gains linear controller"/>
<define name="REF_ERR_R" value="600.0" description="INDI gains linear controller"/>
<define name="REF_RATE_P" value="28.0" description="INDI gains linear controller"/>
<define name="REF_RATE_Q" value="28.0" description="INDI gains linear controller"/>
<define name="REF_RATE_R" value="28.0" description="INDI gains linear controller"/>
<define name="MAX_R" value="120.0" description="max yaw rate" unit="deg/s"/>
<define name="FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
<define name="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
@@ -41,12 +41,12 @@
<settings>
<dl_settings>
<dl_settings NAME="indi">
<dl_setting var="reference_acceleration.err_p" min="0" step="1" max="2500" shortname="kp_p" param="STABILIZATION_INDI_REF_ERR_P"/>
<dl_setting var="reference_acceleration.rate_p" min="0" step="0.1" max="100" shortname="kd_p" param="STABILIZATION_INDI_REF_RATE_P"/>
<dl_setting var="reference_acceleration.err_q" min="0" step="1" max="2500" shortname="kp_q" param="STABILIZATION_INDI_REF_ERR_Q"/>
<dl_setting var="reference_acceleration.rate_q" min="0" step="0.1" max="100" shortname="kd_q" param="STABILIZATION_INDI_REF_RATE_P"/>
<dl_setting var="reference_acceleration.err_r" min="0" step="1" max="2500" shortname="kp_r" param="STABILIZATION_INDI_REF_ERR_R"/>
<dl_setting var="reference_acceleration.rate_r" min="0" step="0.1" max="100" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P"/>
<dl_setting var="indi_gains.att.p" min="0" step="1" max="2500" shortname="kp_p" param="STABILIZATION_INDI_REF_ERR_P"/>
<dl_setting var="indi_gains.rate.p" min="0" step="0.1" max="100" shortname="kd_p" param="STABILIZATION_INDI_REF_RATE_P"/>
<dl_setting var="indi_gains.att.q" min="0" step="1" max="2500" shortname="kp_q" param="STABILIZATION_INDI_REF_ERR_Q"/>
<dl_setting var="indi_gains.rate.q" min="0" step="0.1" max="100" shortname="kd_q" param="STABILIZATION_INDI_REF_RATE_P"/>
<dl_setting var="indi_gains.att.r" min="0" step="1" max="2500" shortname="kp_r" param="STABILIZATION_INDI_REF_ERR_R"/>
<dl_setting var="indi_gains.rate.r" min="0" step="0.1" max="100" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P"/>
<dl_setting var="indi_use_adaptive" min="0" step="1" max="1" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8"/>
</dl_settings>
</dl_settings>
conf/modules/stabilization_indi_simple.xml
+12 -12
View File
@@ -30,12 +30,12 @@
<define name="G1_Q" value="0.0361" description="control effectiveness G1 gain on pitch rate"/>
<define name="G1_R" value="0.0022" description="control effectiveness G1 gain on yaw rate"/>
<define name="G2_R" value="0.1450" description="control effectiveness G2 gain on yaw rate"/>
<define name="REF_ERR_P" value="600.0" description="reference acceleration"/>
<define name="REF_ERR_Q" value="600.0" description="reference acceleration"/>
<define name="REF_ERR_R" value="600.0" description="reference acceleration"/>
<define name="REF_RATE_P" value="28.0" description="reference acceleration"/>
<define name="REF_RATE_Q" value="28.0" description="reference acceleration"/>
<define name="REF_RATE_R" value="28.0" description="reference acceleration"/>
<define name="REF_ERR_P" value="600.0" description="linear gains for INDI reference"/>
<define name="REF_ERR_Q" value="600.0" description="linear gains for INDI reference"/>
<define name="REF_ERR_R" value="600.0" description="linear gains for INDI reference"/>
<define name="REF_RATE_P" value="28.0" description="linear gains for INDI reference"/>
<define name="REF_RATE_Q" value="28.0" description="linear gains for INDI reference"/>
<define name="REF_RATE_R" value="28.0" description="linear gains for INDI reference"/>
<define name="MAX_R" value="120.0" description="max yaw rate" unit="deg/s"/>
<define name="FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
<define name="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
@@ -50,14 +50,14 @@
<settings>
<dl_settings>
<dl_settings NAME="indi">
<dl_setting var="indi.reference_acceleration.err_p" min="0" step="1" max="2500" shortname="kp_p" param="STABILIZATION_INDI_REF_ERR_P" persistent="true" module="stabilization/stabilization_indi_simple"/>
<dl_setting var="indi.reference_acceleration.rate_p" min="0" step="0.1" max="100" shortname="kd_p" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.gains.att.p" min="0" step="1" max="2500" shortname="kp_p" param="STABILIZATION_INDI_REF_ERR_P" persistent="true" module="stabilization/stabilization_indi_simple"/>
<dl_setting var="indi.gains.rate.p" min="0" step="0.1" max="100" shortname="kd_p" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.p" min="0" step="0.001" max="10" 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" 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" shortname="kd_q" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.gains.att.q" min="0" step="1" max="2500" shortname="kp_q" param="STABILIZATION_INDI_REF_ERR_Q" persistent="true"/>
<dl_setting var="indi.gains.rate.q" min="0" step="0.1" max="100" shortname="kd_q" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.q" min="0" step="0.001" max="10" 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" 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" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.gains.att.r" min="0" step="1" max="2500" shortname="kp_r" param="STABILIZATION_INDI_REF_ERR_R" persistent="true"/>
<dl_setting var="indi.gains.rate.r" min="0" step="0.1" max="100" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.r" min="0" step="0.001" max="10" shortname="ctl_eff_r" param="STABILIZATION_INDI_G1_R" persistent="true"/>
<dl_setting var="indi.g2" min="0" step="0.01" max="10" shortname="g2" param="STABILIZATION_INDI_G2_R" persistent="true"/>
<dl_setting var="indi.adaptive" min="0" step="1" max="1" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8" persistent="true"/>
conf/modules/stabilization_rate_indi.xml
+1 -1
View File
@@ -15,6 +15,6 @@
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_rate_indi.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<define name="USE_STABILIZATION_RATE"/>
<define name="STABILIZATION_RATE_INDI" value="true"/>
<define name="USE_STABILIZATION_RATE_INDI" value="true"/>
</makefile>
</module>
conf/userconf/tudelft/conf.xml
+3 -3
View File
@@ -85,7 +85,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/delft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/air_data.xml [modules/geo_mag.xml] modules/guidance_indi.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml [modules/geo_mag.xml] modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="#ffffcccaccca"
/>
<aircraft
@@ -96,7 +96,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/delft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/air_data.xml [modules/geo_mag.xml] modules/guidance_indi.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml [modules/geo_mag.xml] modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
gui_color="#ffffcccaccca"
/>
<aircraft
@@ -107,7 +107,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/bebop_ae_awb.xml modules/video_rtp_stream.xml modules/bebop_cam.xml modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_float_mlkf.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
settings_modules="modules/ahrs_float_mlkf.xml modules/air_data.xml modules/bebop_ae_awb.xml modules/bebop_cam.xml modules/geo_mag.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml modules/video_rtp_stream.xml"
gui_color="#ffffbc3bce5b"
/>
<aircraft
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c
+2 -4
View File
@@ -35,7 +35,7 @@
void stabilization_attitude_init(void)
{
// Check if the indi init is already done for rate control
#ifndef STABILIZATION_RATE_INDI
#ifndef USE_STABILIZATION_RATE_INDI
stabilization_indi_init();
#endif
}
@@ -62,9 +62,7 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
void stabilization_attitude_run(bool in_flight)
{
struct Int32Quat quat_sp = {1, 0, 0, 0}; // always hover
stabilization_indi_attitude_run(in_flight, quat_sp);
stabilization_indi_attitude_run(stab_att_sp_quat, in_flight);
}
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+40 -46
View File
@@ -63,13 +63,17 @@ static void calc_g1g2_pseudo_inv(void);
static void bound_g_mat(void);
int32_t stabilization_att_indi_cmd[COMMANDS_NB];
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 Indi_gains indi_gains = {
.att = {
STABILIZATION_INDI_REF_ERR_P,
STABILIZATION_INDI_REF_ERR_Q,
STABILIZATION_INDI_REF_ERR_R
},
.rate = {
STABILIZATION_INDI_REF_RATE_P,
STABILIZATION_INDI_REF_RATE_Q,
STABILIZATION_INDI_REF_RATE_R
},
};
#if STABILIZATION_INDI_USE_ADAPTIVE
@@ -98,11 +102,6 @@ float act_pref[INDI_NUM_ACT] = {0.0};
float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
/** Maximum rate you can request in RC rate mode (rad/s)*/
#ifndef STABILIZATION_INDI_MAX_RATE
#define STABILIZATION_INDI_MAX_RATE 6.0
#endif
#ifdef STABILIZATION_INDI_WLS_PRIORITIES
static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
#else
@@ -345,20 +344,36 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
*
* Function that calculates the INDI commands
*/
void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight)
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
{
/* Propagate the filter on the gyroscopes */
struct FloatRates *body_rates = stateGetBodyRates_f();
float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
int8_t i;
for (i = 0; i < 3; i++) {
update_butterworth_2_low_pass(&measurement_lowpass_filters[i], rate_vect[i]);
update_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], rate_vect[i]);
//Calculate the angular acceleration via finite difference
angular_acceleration[i] = (measurement_lowpass_filters[i].o[0]
- measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
// Calculate derivatives for estimation
float estimation_rate_d_prev = estimation_rate_d[i];
estimation_rate_d[i] = (estimation_output_lowpass_filters[i].o[0] - estimation_output_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
}
// FIXME: this should be configurable!
q_filt = (q_filt*3+body_rates->q)/4;
r_filt = (r_filt*3+body_rates->r)/4;
//calculate the virtual control (reference acceleration) based on a PD controller
angular_accel_ref.p = (rate_ref.p - body_rates->p) * reference_acceleration.rate_p;
angular_accel_ref.q = (rate_ref.q - q_filt) * reference_acceleration.rate_q;
angular_accel_ref.r = (rate_ref.r - r_filt) * reference_acceleration.rate_r;
angular_accel_ref.p = (rate_sp.p - body_rates->p) * indi_gains.rate.p;
angular_accel_ref.q = (rate_sp.q - q_filt) * indi_gains.rate.q;
angular_accel_ref.r = (rate_sp.r - r_filt) * indi_gains.rate.r;
g2_times_du = 0.0;
int8_t i;
for (i = 0; i < INDI_NUM_ACT; i++) {
g2_times_du += g2[i] * indi_du[i];
}
@@ -478,50 +493,29 @@ void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight)
*
* Function that should be called to run the INDI controller
*/
void stabilization_indi_attitude_run(bool in_flight, struct Int32Quat quat_sp)
void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
{
/* Propagate the filter on the gyroscopes */
struct FloatRates *body_rates = stateGetBodyRates_f();
float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
int8_t i;
for (i = 0; i < 3; i++) {
update_butterworth_2_low_pass(&measurement_lowpass_filters[i], rate_vect[i]);
update_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], rate_vect[i]);
//Calculate the angular acceleration via finite difference
angular_acceleration[i] = (measurement_lowpass_filters[i].o[0]
- measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
// Calculate derivatives for estimation
float estimation_rate_d_prev = estimation_rate_d[i];
estimation_rate_d[i] = (estimation_output_lowpass_filters[i].o[0] - estimation_output_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
}
/* attitude error */
struct Int32Quat att_err;
struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
int32_quat_inv_comp(&att_err, att_quat, &quat_sp);
int32_quat_inv_comp(&att_err, att_quat, &quat_sp);
/* wrap it in the shortest direction */
int32_quat_wrap_shortest(&att_err);
int32_quat_normalize(&att_err);
// local variable to compute rate setpoints based on attitude error
struct FloatRates rate_ref;
struct FloatRates rate_sp;
// calculate the virtual control (reference acceleration) based on a PD controller
rate_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
/ reference_acceleration.rate_p;
rate_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
/ reference_acceleration.rate_q;
rate_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
/ reference_acceleration.rate_r;
rate_sp.p = indi_gains.att.p * QUAT1_FLOAT_OF_BFP(att_err.qx) / indi_gains.rate.p;
rate_sp.q = indi_gains.att.q * QUAT1_FLOAT_OF_BFP(att_err.qy) / indi_gains.rate.q;
rate_sp.r = indi_gains.att.r * QUAT1_FLOAT_OF_BFP(att_err.qz) / indi_gains.rate.r;
// Possibly we can use some bounding here
/*BoundAbs(rate_ref.r, 5.0);*/
/*BoundAbs(rate_sp.r, 5.0);*/
/* compute the INDI command */
stabilization_indi_calc_cmd(rate_ref, in_flight);
stabilization_indi_rate_run(rate_sp, in_flight);
// Reset thrust increment boolean
indi_thrust_increment_set = false;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
+6 -10
View File
@@ -38,24 +38,20 @@ extern bool indi_use_adaptive;
extern float *Bwls[INDI_OUTPUTS];
struct ReferenceSystem {
float err_p;
float err_q;
float err_r;
float rate_p;
float rate_q;
float rate_r;
struct Indi_gains {
struct FloatRates att;
struct FloatRates rate;
};
extern struct ReferenceSystem reference_acceleration;
extern struct Indi_gains indi_gains;
extern void stabilization_indi_init(void);
extern void stabilization_indi_enter(void);
extern void stabilization_indi_set_failsafe_setpoint(void);
extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
extern void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight);
extern void stabilization_indi_attitude_run(bool in_flight, struct Int32Quat quat_sp);
extern void stabilization_indi_rate_run(struct FloatRates rate_ref, bool in_flight);
extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
#endif /* STABILIZATION_INDI */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
+28 -35
View File
@@ -86,13 +86,17 @@ struct IndiVariables indi = {
.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
.gains = {
.att = {
STABILIZATION_INDI_REF_ERR_P,
STABILIZATION_INDI_REF_ERR_Q,
STABILIZATION_INDI_REF_ERR_R
},
.rate = {
STABILIZATION_INDI_REF_RATE_P,
STABILIZATION_INDI_REF_RATE_Q,
STABILIZATION_INDI_REF_RATE_R
},
},
/* Estimation parameters for adaptive INDI */
@@ -207,7 +211,6 @@ void stabilization_indi_set_failsafe_setpoint(void)
*
* @param rpy roll pitch yaw input
*/
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
{
// stab_att_sp_euler.psi still used in ref..
@@ -215,7 +218,7 @@ void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
}
/**
* @brief Set attitude setpoint from command in earth axes
*
@@ -285,9 +288,8 @@ static inline void finite_difference(float output[3], float new[3], float old[3]
*
* @param indi_commands[] Array of commands that the function will write to
* @param att_err quaternion attitude error
* @param rate_control rate control enabled, otherwise attitude control
*/
void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight __attribute__((unused)), bool rate_control)
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __attribute__((unused)))
{
// Propagate the filter on the gyroscopes and actuators
struct FloatRates *body_rates = stateGetBodyRates_f();
@@ -298,37 +300,28 @@ void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight __at
finite_difference_from_filter(indi.rate_d, indi.rate);
//The rates used for feedback are by default the measured rates. If needed they can be filtered (see below)
struct FloatRates rates_for_feedback;
RATES_COPY(rates_for_feedback, (*body_rates));
struct FloatRates rates_filt_fo;
RATES_COPY(rates_filt_fo, (*body_rates));
//If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
//Note that due to the delay, the PD controller can not be as aggressive.
#if STABILIZATION_INDI_FILTER_ROLL_RATE
rates_for_feedback.p = indi.rate[0].o[0];
rates_filt_fo.p = (rates_filt_fo.p*3+body_rates->p)/4;
#endif
#if STABILIZATION_INDI_FILTER_PITCH_RATE
rates_for_feedback.q = indi.rate[1].o[0];
rates_filt_fo.q = (rates_filt_fo.q*3+body_rates->q)/4;
#endif
#if STABILIZATION_INDI_FILTER_YAW_RATE
rates_for_feedback.r = indi.rate[2].o[0];
rates_filt_fo.r = (rates_filt_fo.r*3+body_rates->r)/4;
#endif
// Compute reference angular acceleration:
indi.angular_accel_ref.p = rate_ref.p - indi.reference_acceleration.rate_p * rates_for_feedback.p;
indi.angular_accel_ref.q = rate_ref.q - indi.reference_acceleration.rate_q * rates_for_feedback.q;
//This separates the P and D controller and lets you impose a maximum yaw rate.
float rate_ref_r = rate_ref.r / indi.reference_acceleration.rate_r;
BoundAbs(rate_ref_r, indi.attitude_max_yaw_rate);
indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (rate_ref_r - rates_for_feedback.r);
BoundAbs(rate_sp.r, indi.attitude_max_yaw_rate);
/* Check if we are running the rate controller and overwrite */
if (rate_control) {
indi.angular_accel_ref.p = indi.reference_acceleration.rate_p * (rate_ref.p - body_rates->p);
indi.angular_accel_ref.q = indi.reference_acceleration.rate_q * (rate_ref.q - body_rates->q);
indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (rate_ref.r - body_rates->r);
}
// Compute reference angular acceleration:
indi.angular_accel_ref.p = (rate_sp.p - rates_filt_fo.p) * indi.gains.rate.p;
indi.angular_accel_ref.q = (rate_sp.q - rates_filt_fo.q) * indi.gains.rate.q;
indi.angular_accel_ref.r = (rate_sp.r - rates_filt_fo.r) * indi.gains.rate.r;
//Increment in angular acceleration requires increment in control input
//G1 is the control effectiveness. In the yaw axis, we need something additional: G2.
@@ -391,7 +384,7 @@ void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight __at
* @param in_flight not used
* @param rate_control rate control enabled, otherwise attitude control
*/
void stabilization_indi_attitude_run(bool in_flight __attribute__((unused)), struct Int32Quat quat_sp)
void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight __attribute__((unused)))
{
/* attitude error */
struct Int32Quat att_err;
@@ -404,12 +397,12 @@ void stabilization_indi_attitude_run(bool in_flight __attribute__((unused)), str
struct FloatRates rate_sp;
// indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err.qx) - rate setpoint
rate_sp.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err.qx);
rate_sp.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err.qy);
rate_sp.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err.qz);
rate_sp.p = indi.gains.att.p * QUAT1_FLOAT_OF_BFP(att_err.qx) / indi.gains.rate.p;
rate_sp.q = indi.gains.att.q * QUAT1_FLOAT_OF_BFP(att_err.qy) / indi.gains.rate.q;
rate_sp.r = indi.gains.att.r * QUAT1_FLOAT_OF_BFP(att_err.qz) / indi.gains.rate.r;
/* compute the INDI command */
stabilization_indi_calc_cmd(rate_sp, in_flight, FALSE);
stabilization_indi_rate_run(rate_sp, in_flight);
}
/**
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
+6 -10
View File
@@ -40,13 +40,9 @@
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 Indi_gains {
struct FloatRates att;
struct FloatRates rate;
};
struct IndiEstimation {
@@ -73,7 +69,7 @@ struct IndiVariables {
struct FloatRates g1;
float g2;
struct ReferenceSystem reference_acceleration;
struct Indi_gains gains;
bool adaptive; ///< Enable adataptive estimation
float max_rate; ///< Maximum rate in rate control in rad/s
@@ -88,8 +84,8 @@ extern void stabilization_indi_enter(void);
extern void stabilization_indi_set_failsafe_setpoint(void);
extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
extern void stabilization_indi_calc_cmd(struct FloatRates rates_ref, bool in_flight, bool rate_control);
extern void stabilization_indi_attitude_run(bool in_flight, struct Int32Quat quat_sp);
extern void stabilization_indi_rate_run(struct FloatRates rates_sp, bool in_flight);
extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
#endif /* STABILIZATION_INDI_SIMPLE_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c
+29 -35
View File
@@ -1,6 +1,6 @@
/*
* Copyright (C) 2016 Freek van Tienen <freek.v.tienen@gmail.com>
* 2020 Rohan Chotalal
* 2020 Rohan Chotalal
*
* This file is part of paparazzi.
*
@@ -33,10 +33,17 @@
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#endif
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
/* -- define variables */
struct FloatRates stabilization_rate_sp;
/* -- RC deadbands */
/** Maximum rate you can request in RC rate mode (rad/s)*/
#ifndef STABILIZATION_INDI_MAX_RATE
#define STABILIZATION_INDI_MAX_RATE 6.0
#endif
/* -- RC deadbands */
#ifndef STABILIZATION_RATE_DEADBAND_P
#define STABILIZATION_RATE_DEADBAND_P 0
#endif
@@ -65,14 +72,20 @@ struct FloatRates stabilization_rate_sp;
static void send_rate(struct transport_tx *trans, struct link_device *dev)
{
float dummy = 0;
float dummy = 0;
float fb_p = stabilization_cmd[COMMAND_ROLL];
float fb_q = stabilization_cmd[COMMAND_PITCH];
float fb_r = stabilization_cmd[COMMAND_YAW];
pprz_msg_send_RATE_LOOP(trans, dev, AC_ID,
&stabilization_rate_sp.p,
&stabilization_rate_sp.q,
&stabilization_rate_sp.r,
&dummy, &dummy, &dummy, // [CHECK THIS!]
&stabilization_cmd[COMMAND_THRUST]); [// CHECK WITH EWOUD]
&dummy, &dummy, &dummy,
&fb_p,
&fb_q,
&fb_r,
&stabilization_cmd[COMMAND_THRUST]);
}
#endif
@@ -88,6 +101,14 @@ void stabilization_rate_init(void)
#endif
}
/**
* @brief Reset rate controller
*/
void stabilization_rate_enter(void)
{
stabilization_indi_enter();
}
/**
* @brief Read RC comands with roll and yaw sticks
*/
@@ -113,7 +134,7 @@ void stabilization_rate_read_rc(void)
}
/**
* @brief Read rc with roll and yaw sitcks switched if the default orientation is vertical
* @brief 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)
@@ -137,38 +158,11 @@ void stabilization_rate_read_rc_switched_sticks(void)
}
}
void stabilization_rate_indi_cmd(bool in_flight, struct Int32Rates rate_sp)
{
#ifdef STABILIZATION_ATTITUDE_INDI_SIMPLE
stabilization_indi_calc_cmd(rate_sp, in_flight, TRUE)
#else
stabilization_indi_calc_cmd(rate_sp, in_flight)
#endif
}
/**
* @brief Run this indi rate interface (when module mode!)
*/
void stabilization_rate_indi_run(bool in_flight, struct Int32Rates rates_sp)
{
/* compute the INDI rate command */
stabilization_indi_calc_cmd(rates_sp, in_flight);
}
/**
* @brief Run this indi rate interface from the "stabilization_rate_run" function
* (according to Gautier, when in RC mode)
* @brief Run indi rate interface from the "stabilization_rate_run" function
*/
void stabilization_rate_run(bool in_flight)
{
/* compute the INDI rate command */
stabilization_rate_indi_calc_run(in_flight, stabilization_rate_sp);
}
/**
* @brief Enter this indi rate module
*/
void stabilization_rate_indi_enter(void)
{
stabilization_indi_enter();
stabilization_indi_rate_run(stabilization_rate_sp, in_flight);
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.h
-8
View File
@@ -30,12 +30,4 @@
/* access declarations of basic functions from "stabilization_rate.h" file */
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
#include "math/pprz_algebra_int.h"
extern struct FloatRates stabilization_rate_sp;
extern void stabilization_rate_indi_cmd(bool in_flight, struct Int32Rates rate_sp);
extern void stabilization_rate_indi_run(bool in_flight, struct Int32Rates rates_sp);
extern void stabilization_rate_indi_enter(void);
#endif /* STABILIZATION_RATE_INDI */