void parse_mf_daq_msg(void)
{
mf_daq.nb = dl_buffer[2];
if (mf_daq.nb > 0) {
if (mf_daq.nb > MF_DAQ_SIZE) {
mf_daq.nb = MF_DAQ_SIZE;
}
// Store data struct directly from dl_buffer
float *buf = (float*)(dl_buffer+3);
memcpy(mf_daq.values, buf, mf_daq.nb * sizeof(float));
// Log on SD card
if (log_started) {
DOWNLINK_SEND_PAYLOAD_FLOAT(pprzlog_tp, chibios_sdlog, mf_daq.nb, mf_daq.values);
DOWNLINK_SEND_MF_DAQ_STATE(pprzlog_tp, chibios_sdlog,
&autopilot_flight_time,
&stateGetBodyRates_f()->p,
&stateGetBodyRates_f()->q,
&stateGetBodyRates_f()->r,
&stateGetNedToBodyEulers_f()->phi,
&stateGetNedToBodyEulers_f()->theta,
&stateGetNedToBodyEulers_f()->psi,
&stateGetAccelNed_f()->x,
&stateGetAccelNed_f()->y,
&stateGetAccelNed_f()->z,
&stateGetSpeedEnu_f()->x,
&stateGetSpeedEnu_f()->y,
&stateGetSpeedEnu_f()->z,
&stateGetPositionLla_f()->lat,
&stateGetPositionLla_f()->lon,
&stateGetPositionLla_f()->alt,
&stateGetHorizontalWindspeed_f()->y,
&stateGetHorizontalWindspeed_f()->x);
}
}
}
/**
* Send the attitude
*/
static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev)
{
mavlink_msg_attitude_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetNedToBodyEulers_f()->phi, // Phi
stateGetNedToBodyEulers_f()->theta, // Theta
stateGetNedToBodyEulers_f()->psi, // Psi
stateGetBodyRates_f()->p, // p
stateGetBodyRates_f()->q, // q
stateGetBodyRates_f()->r); // r
MAVLinkSendMessage();
}
/**
* Send the attitude
*/
static inline void mavlink_send_attitude(void)
{
mavlink_msg_attitude_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetNedToBodyEulers_f()->phi, // Phi
stateGetNedToBodyEulers_f()->theta, // Theta
stateGetNedToBodyEulers_f()->psi, // Psi
stateGetBodyRates_f()->p, // p
stateGetBodyRates_f()->q, // q
stateGetBodyRates_f()->r); // r
MAVLinkSendMessage();
}
static inline void mavlink_send_attitude_quaternion(void)
{
mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetNedToBodyQuat_f()->qi,
stateGetNedToBodyQuat_f()->qx,
stateGetNedToBodyQuat_f()->qy,
stateGetNedToBodyQuat_f()->qz,
stateGetBodyRates_f()->p,
stateGetBodyRates_f()->q,
stateGetBodyRates_f()->r);
MAVLinkSendMessage();
}
static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev)
{
mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetNedToBodyQuat_f()->qi,
stateGetNedToBodyQuat_f()->qx,
stateGetNedToBodyQuat_f()->qy,
stateGetNedToBodyQuat_f()->qz,
stateGetBodyRates_f()->p,
stateGetBodyRates_f()->q,
stateGetBodyRates_f()->r);
MAVLinkSendMessage();
}
// 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 * SCALE;
g_est.p = g_est.p - (g_est.p * du - filtered_rate_2deriv_estimation.p) * du * mu;
du = udot_estimation.q * SCALE;
g_est.q = g_est.q - (g_est.q * du - filtered_rate_2deriv_estimation.q) * du * mu;
du = udot_estimation.r * SCALE;
float ddu = udotdot_estimation.r * 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; }
//Commit the estimated G values and apply the scaling
g1.p = g_est.p * SCALE;
g1.q = g_est.q * SCALE;
g1.r = g_est.r * SCALE;
g2 = g2_est * SCALE;
}
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);
}
static inline void h_ctl_roll_rate_loop()
{
float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint;
/* I term calculation */
static float roll_rate_sum_err = 0.;
static uint8_t roll_rate_sum_idx = 0;
static float roll_rate_sum_values[H_CTL_ROLL_RATE_SUM_NB_SAMPLES];
roll_rate_sum_err -= roll_rate_sum_values[roll_rate_sum_idx];
roll_rate_sum_values[roll_rate_sum_idx] = err;
roll_rate_sum_err += err;
roll_rate_sum_idx++;
if (roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) { roll_rate_sum_idx = 0; }
/* D term calculations */
static float last_err = 0;
float d_err = err - last_err;
last_err = err;
float throttle_dep_pgain =
Blend(h_ctl_hi_throttle_roll_rate_pgain, h_ctl_lo_throttle_roll_rate_pgain,
v_ctl_throttle_setpoint / ((float)MAX_PPRZ));
float cmd = throttle_dep_pgain * (err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES +
h_ctl_roll_rate_dgain * d_err);
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
// 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 (indi.adaptive) {
//Commit the estimated G values and apply the scaling
indi.g1.p = est->g1.p * INDI_EST_SCALE;
indi.g1.q = est->g1.q * INDI_EST_SCALE;
indi.g1.r = est->g1.r * INDI_EST_SCALE;
indi.g2 = est->g2 * INDI_EST_SCALE;
}
}
void stabilization_attitude_run(bool_t in_flight) {
stabilization_attitude_ref_update();
/* Compute feedforward */
stabilization_att_ff_cmd[COMMAND_ROLL] =
stabilization_gains.dd.x * stab_att_ref_accel.p / 32.;
stabilization_att_ff_cmd[COMMAND_PITCH] =
stabilization_gains.dd.y * stab_att_ref_accel.q / 32.;
stabilization_att_ff_cmd[COMMAND_YAW] =
stabilization_gains.dd.z * stab_att_ref_accel.r / 32.;
/* Compute feedback */
/* attitude error */
struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
struct FloatEulers att_err;
EULERS_DIFF(att_err, stab_att_ref_euler, *att_float);
FLOAT_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
/* update integrator */
EULERS_ADD(stabilization_att_sum_err, att_err);
EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
}
else {
FLOAT_EULERS_ZERO(stabilization_att_sum_err);
}
/* rate error */
struct FloatRates* rate_float = stateGetBodyRates_f();
struct FloatRates rate_err;
RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float);
/* PID */
stabilization_att_fb_cmd[COMMAND_ROLL] =
stabilization_gains.p.x * att_err.phi +
stabilization_gains.d.x * rate_err.p +
stabilization_gains.i.x * stabilization_att_sum_err.phi / 1024.;
stabilization_att_fb_cmd[COMMAND_PITCH] =
stabilization_gains.p.y * att_err.theta +
stabilization_gains.d.y * rate_err.q +
stabilization_gains.i.y * stabilization_att_sum_err.theta / 1024.;
stabilization_att_fb_cmd[COMMAND_YAW] =
stabilization_gains.p.z * att_err.psi +
stabilization_gains.d.z * rate_err.r +
stabilization_gains.i.z * stabilization_att_sum_err.psi / 1024.;
stabilization_cmd[COMMAND_ROLL] =
(stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
stabilization_cmd[COMMAND_PITCH] =
(stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
stabilization_cmd[COMMAND_YAW] =
(stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW])/16.;
}
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;
}
void ahrs_update_accel(void) {
/* last column of roation matrix = ltp z-axis in imu-frame */
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)
};
struct FloatVect3 imu_accel_float;
ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);
struct FloatVect3 residual;
if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu;
FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
struct FloatVect3 corrected_gravity;
VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu);
/* compute the residual of gravity vector in imu frame */
FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
} else {
FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2);
}
#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
/* heuristic on acceleration norm */
const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float);
const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
#else
const float weight = 1.;
#endif
/* compute correction */
const float gravity_rate_update_gain = -5e-2; // -5e-2
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
const float gravity_bias_update_gain = 1e-5; // -5e-6
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
/* FIXME: saturate bias */
}
inline static void h_ctl_yaw_loop(void)
{
#if H_CTL_YAW_TRIM_NY
// Actual Acceleration from IMU:
#if (!defined SITL || defined USE_NPS)
struct Int32Vect3 accel_meas_body, accel_ned;
struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
struct NedCoor_i *accel_tmp = stateGetAccelNed_i();
VECT3_COPY(accel_ned, (*accel_tmp));
accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f);
int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned);
float ny = -ACCEL_FLOAT_OF_BFP(accel_meas_body.y) / 9.81f; // Lateral load factor (in g)
#else
float ny = 0.f;
#endif
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_yaw_ny_sum_err = 0.;
} else {
if (h_ctl_yaw_ny_igain > 0.) {
// only update when: phi<60degrees and ny<2g
if (fabsf(stateGetNedToBodyEulers_f()->phi) < 1.05 && fabsf(ny) < 2.) {
h_ctl_yaw_ny_sum_err += ny * H_CTL_REF_DT;
// max half rudder deflection for trim
BoundAbs(h_ctl_yaw_ny_sum_err, MAX_PPRZ / (2. * h_ctl_yaw_ny_igain));
}
} else {
h_ctl_yaw_ny_sum_err = 0.;
}
}
#endif
#ifdef USE_AIRSPEED
float Vo = stateGetAirspeed_f();
Bound(Vo, STALL_AIRSPEED, RACE_AIRSPEED);
#else
float Vo = NOMINAL_AIRSPEED;
#endif
h_ctl_ref.yaw_rate = h_ctl_yaw_rate_setpoint // set by RC
+ 9.81f / Vo * sinf(h_ctl_roll_setpoint); // for turns
float d_err = h_ctl_ref.yaw_rate - stateGetBodyRates_f()->r;
float cmd = + h_ctl_yaw_dgain * d_err
#if H_CTL_YAW_TRIM_NY
+ h_ctl_yaw_ny_igain * h_ctl_yaw_ny_sum_err
#endif
;
cmd /= airspeed_ratio2;
h_ctl_rudder_setpoint = TRIM_PPRZ(cmd);
}
void mf_daq_send_state(void)
{
// Send aircraft state to DAQ board
DOWNLINK_SEND_MF_DAQ_STATE(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
&autopilot_flight_time,
&stateGetBodyRates_f()->p,
&stateGetBodyRates_f()->q,
&stateGetBodyRates_f()->r,
&stateGetNedToBodyEulers_f()->phi,
&stateGetNedToBodyEulers_f()->theta,
&stateGetNedToBodyEulers_f()->psi,
&stateGetAccelNed_f()->x,
&stateGetAccelNed_f()->y,
&stateGetAccelNed_f()->z,
&stateGetSpeedEnu_f()->x,
&stateGetSpeedEnu_f()->y,
&stateGetSpeedEnu_f()->z,
&stateGetPositionLla_f()->lat,
&stateGetPositionLla_f()->lon,
&stateGetPositionLla_f()->alt,
&stateGetHorizontalWindspeed_f()->y,
&stateGetHorizontalWindspeed_f()->x);
}
inline static void h_ctl_roll_loop( void ) {
float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint;
struct FloatRates* body_rate = stateGetBodyRates_f();
#ifdef SITL
static float last_err = 0;
body_rate->p = (err - last_err)/(1/60.);
last_err = err;
#endif
float cmd = h_ctl_roll_attitude_gain * err
+ h_ctl_roll_rate_gain * body_rate->p
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
static void send_att(void) {
struct FloatRates* body_rate = stateGetBodyRates_f();
struct FloatEulers* att = stateGetNedToBodyEulers_f();
DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err.phi,
&stabilization_att_sum_err.theta,
&stabilization_att_sum_err.psi,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW],
&body_rate_d.p, &body_rate_d.q, &body_rate_d.r);
}
static void send_att(struct transport_tx *trans, struct link_device *dev) {
struct FloatRates* body_rate = stateGetBodyRates_f();
struct FloatEulers* att = stateGetNedToBodyEulers_f();
float foo = 0.0;
pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err.phi,
&stabilization_att_sum_err.theta,
&stabilization_att_sum_err.psi,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW],
&foo, &foo, &foo);
}
void CN_potential_velocity(void)
{
float OF_Result_Vy = 0;
float OF_Result_Vx = 0;
//Constants
float new_heading;
float alpha_fil = 0.2;
//Initialize
float potential_obst = 0; //define potential field variables
float potential_obst_integrated = 0;
float Distance_est;
float fp_angle;
float diff_angle;
float total_vel;
float current_heading;
float angle_hor = - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2] /
2; //calculate position angle of stereo
//Tune
//float dt = 0.5; //define delta t for integration! check response under 0.1 and 1
int8_t min_disparity = 40;
//Current state of system;
float r_old = stateGetBodyRates_f()->r;
current_heading = stateGetNedToBodyEulers_f()->psi; //check maybe cause of errors
//current_heading = 0;
total_vel = pow((OF_Result_Vy * OF_Result_Vy + OF_Result_Vx * OF_Result_Vx), 0.5);
if (total_vel > vmin) {
fp_angle = atan2(OF_Result_Vx, OF_Result_Vy);
} else {
fp_angle = 0;
}
heading_goal_ref = (fp_angle + current_heading) - heading_goal_f;
FLOAT_ANGLE_NORMALIZE(heading_goal_ref);
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
/ 2; //Check if bodyframe is correct with current_heading correction
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
FLOAT_ANGLE_NORMALIZE(angle_hor);
for (int i2 = 0; i2 < size_matrix[1]; i2++) {
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
Distance_est = ((baseline * (float)focal / (float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
size_matrix[2] + i3] - 18.0)) / 1000;
} else {
Distance_est = 2000;
}
diff_angle = fp_angle - angle_hor;
FLOAT_ANGLE_NORMALIZE(diff_angle);
potential_obst = potential_obst + K_obst * (diff_angle) * exp(-c3_oa * fabs(diff_angle)) * exp(
-c4_oa * Distance_est); //(tan(obst_width[i]+c5)-tan(c5));
potential_obst_integrated = potential_obst_integrated + K_obst * c3_oa * (fabs(diff_angle) + 1) / (c3_oa * c3_oa) * exp(
-c3_oa * fabs(diff_angle)) * exp(-c4_oa * Distance_est); // (tan(obst_width[i]+c5)-tan(c5));
}
}
}
//calculate angular accelaration from potential field
r_dot_new = -b_damp * r_old - K_goal * (heading_goal_ref) * (exp(-c1_oa * sqrt(VECT2_NORM2(
pos_diff))) + c2_oa) + potential_obst;
// Calculate velocity from potential
speed_pot = vref_max * exp(-kv * potential_obst_integrated) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff))) - epsilon;
if (speed_pot <= 0) {
speed_pot = 0;
}
//calculate new_ref_speeds
new_heading = fp_angle + alpha_fil * r_dot_new;
FLOAT_ANGLE_NORMALIZE(new_heading);
ref_pitch = new_heading;
}
void CN_potential_heading(void)
{
//float OF_Result_Vy = 0;
//float OF_Result_Vx = 0
//Initialize
float potential_obst = 0; //define potential field variables
float potential_obst_temp = 0;
float potential_obst_integrated = 0;
float Distance_est;
float angle_hor = - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2] /
2; //calculate position angle of stereo
float angle_hor_abs;
float obst_count = 0.0;
//float total_vel;
//float current_heading;
//float fp_angle;
//Tune
//float dt = 0.5; //define delta t for integration! check response under 0.1 and 1
int8_t min_disparity = 40;
//Current state of system;
float r_old = stateGetBodyRates_f()->r;
//current_heading = stateGetNedToBodyEulers_f()->psi; //check maybe cause of errors
//current_heading = 0;
//check if side slip is zero
//total_vel = pow((OF_Result_Vy*OF_Result_Vy + OF_Result_Vx*OF_Result_Vx),0.5);
//if (total_vel>vmin){
// fp_angle = atan2(OF_Result_Vx,OF_Result_Vy);
//}
//else{
// fp_angle = 0;
//}
//fp_angle = 0;
heading_goal_ref = stateGetNedToBodyEulers_f()->psi - heading_goal_f;//
FLOAT_ANGLE_NORMALIZE(heading_goal_ref);
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
/ 2; //Check if bodyframe is correct with current_heading correction
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
FLOAT_ANGLE_NORMALIZE(angle_hor);
potential_obst_temp = 0.0;
obst_count = 0;
for (int i2 = 0; i2 < 4; i2++) {
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
Distance_est = ((baseline * (float)focal / (float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
size_matrix[2] + i3] - 18.0)) / 1000;
if (angle_hor < 0) {
angle_hor_abs = -1 * angle_hor;
} else {
angle_hor_abs = angle_hor;
}
potential_obst_temp = potential_obst_temp - K_obst * (angle_hor) * exp(-c3_oa * angle_hor_abs) * exp(
-c4_oa * Distance_est);//(tan(obst_width[i]+c5)-tan(c5));
potential_obst_integrated = potential_obst_integrated + K_obst * c3_oa * (fabs(angle_hor) + 1) / (c3_oa * c3_oa) * exp(
-c3_oa * fabs(angle_hor)) * exp(-c4_oa * Distance_est); // (tan(obst_width[i]+c5)-tan(c5));
//printf("angle_hor: %f, angle_hor_abs: %f, c3: %f, potential_obst: %f", angle_hor, angle_hor_abs,c3_oa,potential_obst);
obst_count++;
} else {
Distance_est = 2000;
}
//potential_obst = potential_obst + K_obst*(angle_hor);//*exp(-c3*abs(angle_hor))*exp(-c4*Distance_est);//(tan(obst_width[i]+c5)-tan(c5));
//potential_obst_write = potential_obst;
//potential_obst_integrated = potential_obst_integrated + K_obst*c3*(abs(angle_hor)+1)/(c3*c3)*exp(-c3*abs(angle_hor))*exp(-c4*Distance_est);// (tan(obst_width[i]+c5)-tan(c5));
}
if (obst_count != 0) {
potential_obst = potential_obst + potential_obst_temp / ((float)obst_count);
}
}
}
//printf("current_heading, %f heading_goal_f: %f",current_heading, heading_goal_ref);
//calculate angular accelaration from potential field
r_dot_new = -b_damp * r_old - K_goal * (heading_goal_ref) * (exp(-c1_oa * sqrt(VECT2_NORM2(
pos_diff))) + c2_oa) + potential_obst;
// Calculate velocity from potential
speed_pot = vref_max * exp(-kv * potential_obst_integrated) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff))) - epsilon;
if (speed_pot <= 0) {
speed_pot = 0;
}
//calculate new_heading(ref_pitch)
//ref_pitch = (current_state(3)+sideslip) + alpha_fil*r_dot_new;)
}
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_attitude_run(bool enable_integrator)
{
/*
* Update reference
*/
static const float dt = (1./PERIODIC_FREQUENCY);
attitude_ref_quat_float_update(&att_ref_quat_f, &stab_att_sp_quat, dt);
/*
* Compute errors for feedback
*/
/* attitude error */
struct FloatQuat att_err;
struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
float_quat_inv_comp(&att_err, att_quat, &att_ref_quat_f.quat);
/* wrap it in the shortest direction */
float_quat_wrap_shortest(&att_err);
/* rate error */
struct FloatRates rate_err;
struct FloatRates *body_rate = stateGetBodyRates_f();
RATES_DIFF(rate_err, att_ref_quat_f.rate, *body_rate);
/* rate_d error */
RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
RATES_COPY(last_body_rate, *body_rate);
#define INTEGRATOR_BOUND 1.0
/* integrated error */
if (enable_integrator) {
stabilization_att_sum_err_quat.qx += att_err.qx / IERROR_SCALE;
stabilization_att_sum_err_quat.qy += att_err.qy / IERROR_SCALE;
stabilization_att_sum_err_quat.qz += att_err.qz / IERROR_SCALE;
Bound(stabilization_att_sum_err_quat.qx, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qy, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qz, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
} else {
/* reset accumulator */
float_quat_identity(&stabilization_att_sum_err_quat);
}
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &att_ref_quat_f.accel);
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d,
&stabilization_att_sum_err_quat);
stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
#ifdef HAS_SURFACE_COMMANDS
stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] +
stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] +
stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] +
stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
#endif
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
static void send_tune_roll(void) {
DOWNLINK_SEND_TUNE_ROLL(DefaultChannel, DefaultDevice,
&(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
}
inline static void h_ctl_pitch_loop( void ) {
#if !USE_GYRO_PITCH_RATE
static float last_err;
#endif
/* sanity check */
if (h_ctl_pitch_of_roll <0.)
h_ctl_pitch_of_roll = 0.;
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi);
#if USE_PITCH_TRIM
loiter();
#endif
#if USE_ANGLE_REF
// Update reference setpoints for pitch
h_ctl_ref.pitch_angle += h_ctl_ref.pitch_rate * H_CTL_REF_DT;
h_ctl_ref.pitch_rate += h_ctl_ref.pitch_accel * H_CTL_REF_DT;
h_ctl_ref.pitch_accel = H_CTL_REF_W_Q*H_CTL_REF_W_Q * (h_ctl_pitch_loop_setpoint - h_ctl_ref.pitch_angle) - 2*H_CTL_REF_XI_Q*H_CTL_REF_W_Q * h_ctl_ref.pitch_rate;
// Saturation on references
BoundAbs(h_ctl_ref.pitch_accel, h_ctl_ref.max_q_dot);
if (h_ctl_ref.pitch_rate > h_ctl_ref.max_q) {
h_ctl_ref.pitch_rate = h_ctl_ref.max_q;
if (h_ctl_ref.pitch_accel > 0.) h_ctl_ref.pitch_accel = 0.;
}
else if (h_ctl_ref.pitch_rate < - h_ctl_ref.max_q) {
h_ctl_ref.pitch_rate = - h_ctl_ref.max_q;
if (h_ctl_ref.pitch_accel < 0.) h_ctl_ref.pitch_accel = 0.;
}
#else
h_ctl_ref.pitch_angle = h_ctl_pitch_loop_setpoint;
h_ctl_ref.pitch_rate = 0.;
h_ctl_ref.pitch_accel = 0.;
#endif
// Compute errors
float err = h_ctl_ref.pitch_angle - stateGetNedToBodyEulers_f()->theta;
#if USE_GYRO_PITCH_RATE
float d_err = h_ctl_ref.pitch_rate - stateGetBodyRates_f()->q;
#else // soft derivation
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref.pitch_rate;
last_err = err;
#endif
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_pitch_sum_err = 0.;
}
else {
if (h_ctl_pitch_igain > 0.) {
h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain);
} else {
h_ctl_pitch_sum_err = 0.;
}
}
float cmd = - h_ctl_pitch_Kffa * h_ctl_ref.pitch_accel
- h_ctl_pitch_Kffd * h_ctl_ref.pitch_rate
+ h_ctl_pitch_pgain * err
+ h_ctl_pitch_dgain * d_err
+ h_ctl_pitch_igain * h_ctl_pitch_sum_err;
cmd /= airspeed_ratio2;
h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}
inline static void h_ctl_roll_loop( void ) {
static float cmd_fb = 0.;
#if USE_ANGLE_REF
// Update reference setpoints for roll
h_ctl_ref.roll_angle += h_ctl_ref.roll_rate * H_CTL_REF_DT;
h_ctl_ref.roll_rate += h_ctl_ref.roll_accel * H_CTL_REF_DT;
h_ctl_ref.roll_accel = H_CTL_REF_W_P*H_CTL_REF_W_P * (h_ctl_roll_setpoint - h_ctl_ref.roll_angle) - 2*H_CTL_REF_XI_P*H_CTL_REF_W_P * h_ctl_ref.roll_rate;
// Saturation on references
BoundAbs(h_ctl_ref.roll_accel, h_ctl_ref.max_p_dot);
if (h_ctl_ref.roll_rate > h_ctl_ref.max_p) {
h_ctl_ref.roll_rate = h_ctl_ref.max_p;
if (h_ctl_ref.roll_accel > 0.) h_ctl_ref.roll_accel = 0.;
}
else if (h_ctl_ref.roll_rate < - h_ctl_ref.max_p) {
h_ctl_ref.roll_rate = - h_ctl_ref.max_p;
if (h_ctl_ref.roll_accel < 0.) h_ctl_ref.roll_accel = 0.;
}
#else
h_ctl_ref.roll_angle = h_ctl_roll_setpoint;
h_ctl_ref.roll_rate = 0.;
h_ctl_ref.roll_accel = 0.;
#endif
#ifdef USE_KFF_UPDATE
// update Kff gains
h_ctl_roll_Kffa += KFFA_UPDATE * h_ctl_ref.roll_accel * cmd_fb / (h_ctl_ref.max_p_dot*h_ctl_ref.max_p_dot);
h_ctl_roll_Kffd += KFFD_UPDATE * h_ctl_ref.roll_rate * cmd_fb / (h_ctl_ref.max_p*h_ctl_ref.max_p);
#ifdef SITL
printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
#endif
h_ctl_roll_Kffa = Max(h_ctl_roll_Kffa, 0);
h_ctl_roll_Kffd = Max(h_ctl_roll_Kffd, 0);
#endif
// Compute errors
float err = h_ctl_ref.roll_angle - stateGetNedToBodyEulers_f()->phi;
struct FloatRates* body_rate = stateGetBodyRates_f();
float d_err = h_ctl_ref.roll_rate - body_rate->p;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_roll_sum_err = 0.;
}
else {
if (h_ctl_roll_igain > 0.) {
h_ctl_roll_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain);
} else {
h_ctl_roll_sum_err = 0.;
}
}
cmd_fb = h_ctl_roll_attitude_gain * err;
float cmd = - h_ctl_roll_Kffa * h_ctl_ref.roll_accel
- h_ctl_roll_Kffd * h_ctl_ref.roll_rate
- cmd_fb
- h_ctl_roll_rate_gain * d_err
- h_ctl_roll_igain * h_ctl_roll_sum_err
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
cmd /= airspeed_ratio2;
// Set aileron commands
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
static void send_tune_roll(struct transport_tx *trans, struct link_device *dev) {
pprz_msg_send_TUNE_ROLL(trans, dev, AC_ID,
&(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
}
void stabilization_attitude_run(bool_t enable_integrator) {
/*
* Update reference
*/
stabilization_attitude_ref_update();
/*
* Compute errors for feedback
*/
/* attitude error */
struct FloatQuat att_err;
struct FloatQuat* att_quat = stateGetNedToBodyQuat_f();
FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
/* rate error */
struct FloatRates rate_err;
struct FloatRates* body_rate = stateGetBodyRates_f();
RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
/* rate_d error */
struct FloatRates body_rate_d;
RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
RATES_COPY(last_body_rate, *body_rate);
/* integrated error */
if (enable_integrator) {
struct FloatQuat new_sum_err, scaled_att_err;
/* update accumulator */
scaled_att_err.qi = att_err.qi;
scaled_att_err.qx = att_err.qx / IERROR_SCALE;
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_NORMALIZE(new_sum_err);
FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
} else {
/* reset accumulator */
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
}
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel);
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d, &stabilization_att_sum_err_quat);
stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
#ifdef HAS_SURFACE_COMMANDS
stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
#endif
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
void ahrs_update_accel(void) {
/* last column of roation matrix = ltp z-axis in imu-frame */
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)};
struct FloatVect3 imu_accel_float;
ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);
struct FloatVect3 residual;
struct FloatVect3 pseudo_gravity_measurement;
if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu;
FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
}
FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
/* FIR filtered pseudo_gravity_measurement */
#define FIR_FILTER_SIZE 8
static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1);
VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
if (ahrs_impl.gravity_heuristic_factor) {
/* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
* e.g. for gravity_heuristic_factor 30:
* <0.66G = 0, 1G = 1.0, >1.33G = 0
*/
const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81;
ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
Bound(ahrs_impl.weight, 0.15, 1.0);
}
else {
ahrs_impl.weight = 1.0;
}
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
*/
const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega *
ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
*/
const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega *
ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain);
/* FIXME: saturate bias */
}
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;
}