static void attitude_run_indi(int32_t indi_commands[], struct Int32Quat *att_err, bool_t in_flight)
{
//Calculate required angular acceleration
struct FloatRates *body_rate = stateGetBodyRates_f();
indi.angular_accel_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
- reference_acceleration.rate_p * body_rate->p;
indi.angular_accel_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
- reference_acceleration.rate_q * body_rate->q;
indi.angular_accel_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
- reference_acceleration.rate_r * body_rate->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 / 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 {
#if STABILIZATION_INDI_USE_ADAPTIVE
#warning "Use caution with adaptive indi. See the wiki for more info"
lms_estimation();
#endif
}
/* 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;
}
static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err,
struct Int32Rates *rate_err, struct Int32Quat *sum_err)
{
/* PID feedback */
fb_commands[COMMAND_ROLL] =
GAIN_PRESCALER_P * gains->p.x * QUAT1_FLOAT_OF_BFP(att_err->qx) +
GAIN_PRESCALER_D * gains->d.x * RATE_FLOAT_OF_BFP(rate_err->p) +
GAIN_PRESCALER_I * gains->i.x * QUAT1_FLOAT_OF_BFP(sum_err->qx);
fb_commands[COMMAND_PITCH] =
GAIN_PRESCALER_P * gains->p.y * QUAT1_FLOAT_OF_BFP(att_err->qy) +
GAIN_PRESCALER_D * gains->d.y * RATE_FLOAT_OF_BFP(rate_err->q) +
GAIN_PRESCALER_I * gains->i.y * QUAT1_FLOAT_OF_BFP(sum_err->qy);
fb_commands[COMMAND_YAW] =
GAIN_PRESCALER_P * gains->p.z * QUAT1_FLOAT_OF_BFP(att_err->qz) +
GAIN_PRESCALER_D * gains->d.z * RATE_FLOAT_OF_BFP(rate_err->r) +
GAIN_PRESCALER_I * gains->i.z * QUAT1_FLOAT_OF_BFP(sum_err->qz);
}
static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct Int32Quat *att_err, bool 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);
//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));
//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.x.p;
#endif
#if STABILIZATION_INDI_FILTER_PITCH_RATE
rates_for_feedback.q = indi.rate.x.q;
#endif
#if STABILIZATION_INDI_FILTER_YAW_RATE
rates_for_feedback.r = indi.rate.x.r;
#endif
indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
- indi.reference_acceleration.rate_p * rates_for_feedback.p;
indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
- 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 = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)/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);
/* 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);
}
//Increment in angular acceleration requires increment in control input
//G1 is the control 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
//TODO: this should be something more elegant, but without this the inputs will increment to the maximum before
//even getting in the air.
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 {
// only run the estimation if the commands are not zero.
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;
}
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;
}
