void ins_propagate(void) {
/* untilt accels */
struct Int32Vect3 accel_meas_body;
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel);
struct Int32Vect3 accel_meas_ltp;
INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body);
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
if (ins_impl.baro_initialized) {
vff_propagate(z_accel_meas_float);
ins_update_from_vff();
}
else { // feed accel from the sensors
// subtract -9.81m/s2 (acceleration measured due to gravity,
// but vehicle not accelerating in ltp)
ins_impl.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
}
#if USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
/* convert and copy result to ins_impl */
ins_update_from_hff();
#else
ins_impl.ltp_accel.x = accel_meas_ltp.x;
ins_impl.ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */
ins_ned_to_state();
}
void ins_propagate() {
/* untilt accels */
struct Int32Vect3 accel_body;
INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel);
struct Int32Vect3 accel_ltp;
INT32_RMAT_TRANSP_VMULT(accel_ltp, ahrs.ltp_to_body_rmat, accel_body);
float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
#ifdef USE_VFF
if (baro.status == BS_RUNNING && ins_baro_initialised) {
vff_propagate(z_accel_float);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
}
else { // feed accel from the sensors
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
}
#else
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
#endif /* USE_VFF */
#ifdef USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
#else
ins_ltp_accel.x = accel_ltp.x;
ins_ltp_accel.y = accel_ltp.y;
#endif /* USE_HFF */
INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
}
void ins_propagate() {
/* untilt accels */
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
struct Int32Vect3 accel_meas_ltp;
INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body);
#if USE_VFF
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
if (baro.status == BS_RUNNING && ins_baro_initialised) {
vff_propagate(z_accel_meas_float);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
}
else { // feed accel from the sensors
// subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
}
#else
ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
#endif /* USE_VFF */
#if USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
#else
ins_ltp_accel.x = accel_meas_ltp.x;
ins_ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */
INS_NED_TO_STATE();
}
void b2_hff_store_accel_body(void) {
INT32_RMAT_TRANSP_VMULT(acc_body.buf[acc_body.w], imu.body_to_imu_rmat, imu.accel);
acc_body.w = (acc_body.w + 1) < acc_body.size ? (acc_body.w + 1) : 0;
/* once the buffer is full it always has the last acc_body.size accel measurements */
if (acc_body.n < acc_body.size) {
acc_body.n++;
} else {
acc_body.r = (acc_body.r + 1) < acc_body.size ? (acc_body.r + 1) : 0;
}
}
static inline void ahrs_update_mag_2d(void) {
const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y)};
struct Int32Vect3 measured_ltp;
INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
struct Int32Vect3 residual_ltp =
{ 0,
0,
(measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)};
struct Int32Vect3 residual_imu;
INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
// residual_ltp FRAC = 2 * MAG_FRAC = 22
// rate_correction FRAC = RATE_FRAC = 12
// 2^12 / 2^22 * 2.5 = 1/410
// ahrs_impl.rate_correction.p += residual_imu.x*(1<<5)/410;
// ahrs_impl.rate_correction.q += residual_imu.y*(1<<5)/410;
// ahrs_impl.rate_correction.r += residual_imu.z*(1<<5)/410;
ahrs_impl.rate_correction.p += residual_imu.x/16;
ahrs_impl.rate_correction.q += residual_imu.y/16;
ahrs_impl.rate_correction.r += residual_imu.z/16;
// residual_ltp FRAC = 2 * MAG_FRAC = 22
// high_rez_bias = RATE_FRAC+28 = 40
// 2^40 / 2^22 * 2.5e-3 = 655
// ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<5)*655;
// ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<5)*655;
// ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<5)*655;
ahrs_impl.high_rez_bias.p -= residual_imu.x*1024;
ahrs_impl.high_rez_bias.q -= residual_imu.y*1024;
ahrs_impl.high_rez_bias.r -= residual_imu.z*1024;
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
void nav_catapult_highrate_module(void)
{
// Only run when
if (nav_catapult_armed)
{
if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
nav_catapult_launch++;
}
// Launch detection Filter
if (nav_catapult_launch < 5)
{
// Five consecutive measurements > 1.5
#ifndef SITL
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
if (ACCEL_FLOAT_OF_BFP(accel_meas_body.x) < (nav_catapult_acceleration_threshold * 9.81))
#else
if (launch != 1)
#endif
{
nav_catapult_launch = 0;
}
}
// Launch was detected: Motor Delay Counter
else if (nav_catapult_launch >= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
{
// Turn on Motor
NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle));
launch = 1;
}
}
else
{
nav_catapult_launch = 0;
}
}
void ahrs_propagate(void) {
struct NedCoor_f accel;
struct FloatRates body_rates;
struct FloatEulers eulers;
// realign all the filter if needed
// a complete init cycle is required
if (ins_impl.reset) {
ins_impl.reset = FALSE;
ins.status = INS_UNINIT;
ahrs.status = AHRS_UNINIT;
init_invariant_state();
}
// fill command vector
struct Int32Rates gyro_meas_body;
INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.body_to_imu_rmat, imu.gyro);
RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);
// update correction gains
error_output(&ins_impl);
// propagate model
struct inv_state new_state;
runge_kutta_4_float((float*)&new_state,
(float*)&ins_impl.state, INV_STATE_DIM,
(float*)&ins_impl.cmd, INV_COMMAND_DIM,
invariant_model, dt);
ins_impl.state = new_state;
// normalize quaternion
FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);
// set global state
FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
#if INS_UPDATE_FW_ESTIMATOR
// Some stupid lines of code for neutrals
eulers.phi -= ins_roll_neutral;
eulers.theta -= ins_pitch_neutral;
stateSetNedToBodyEulers_f(&eulers);
#else
stateSetNedToBodyQuat_f(&ins_impl.state.quat);
#endif
RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
stateSetBodyRates_f(&body_rates);
stateSetPositionNed_f(&ins_impl.state.pos);
stateSetSpeedNed_f(&ins_impl.state.speed);
// untilt accel and remove gravity
FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel);
FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
FLOAT_VECT3_ADD(accel, A);
stateSetAccelNed_f(&accel);
//------------------------------------------------------------//
RunOnceEvery(3,{
DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice,
&ins_impl.state.quat.qi,
&eulers.phi,
&eulers.theta,
&eulers.psi,
&ins_impl.state.speed.x,
&ins_impl.state.speed.y,
&ins_impl.state.speed.z,
&ins_impl.state.pos.x,
&ins_impl.state.pos.y,
&ins_impl.state.pos.z,
&ins_impl.state.bias.p,
&ins_impl.state.bias.q,
&ins_impl.state.bias.r,
&ins_impl.state.as,
&ins_impl.state.hb,
&ins_impl.meas.baro_alt,
&ins_impl.meas.pos_gps.z)
});
#if LOG_INVARIANT_FILTER
if (pprzLogFile.fs != NULL) {
if (!log_started) {
// log file header
sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
log_started = TRUE;
}
else {
sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
ins_impl.cmd.rates.p,
ins_impl.cmd.rates.q,
ins_impl.cmd.rates.r,
ins_impl.cmd.accel.x,
ins_impl.cmd.accel.y,
ins_impl.cmd.accel.z,
ins_impl.meas.pos_gps.x,
ins_impl.meas.pos_gps.y,
ins_impl.meas.pos_gps.z,
ins_impl.meas.speed_gps.x,
ins_impl.meas.speed_gps.y,
ins_impl.meas.speed_gps.z,
ins_impl.meas.mag.x,
ins_impl.meas.mag.y,
ins_impl.meas.mag.z,
ins_impl.meas.baro_alt,
ins_impl.state.quat.qi,
ins_impl.state.quat.qx,
ins_impl.state.quat.qy,
ins_impl.state.quat.qz,
ins_impl.state.bias.p,
ins_impl.state.bias.q,
ins_impl.state.bias.r,
ins_impl.state.speed.x,
ins_impl.state.speed.y,
ins_impl.state.speed.z,
ins_impl.state.pos.x,
ins_impl.state.pos.y,
ins_impl.state.pos.z,
ins_impl.state.hb,
ins_impl.state.as);
}
}
#endif
}
void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu);//得到ecef的向量坐标
}
/* check if resolution of INT32_TRIG_FRAC (14) is enough here */
void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu);//将enu坐标系转换成ecef坐标系,乘的是转置矩阵
INT32_VECT3_ADD(*ecef, def->ecef);//将def里面存储的ecef加入到由enu转换后的ecef
}
void b2_hff_propagate(void) {
if (b2_hff_lost_counter < b2_hff_lost_limit)
b2_hff_lost_counter++;
#ifdef GPS_LAG
/* continue re-propagating to catch up with the present */
if (b2_hff_rb_last->rollback) {
b2_hff_propagate_past(b2_hff_rb_last);
}
#endif
/* store body accelerations for mean computation */
b2_hff_store_accel_body();
/* propagate current state if it is time */
if (b2_hff_ps_counter == HFF_PRESCALER) {
b2_hff_ps_counter = 1;
if (b2_hff_lost_counter < b2_hff_lost_limit) {
/* compute float ltp mean acceleration */
b2_hff_compute_accel_body_mean(HFF_PRESCALER);
struct Int32Vect3 mean_accel_ltp;
INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, ahrs.ltp_to_body_rmat, acc_body_mean);
b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
#ifdef GPS_LAG
b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
#endif
/*
* propagate current state
*/
b2_hff_propagate_x(&b2_hff_state);
b2_hff_propagate_y(&b2_hff_state);
/* update ins state from horizontal filter */
ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
#ifdef GPS_LAG
/* increase lag counter on last saved state */
if (b2_hff_rb_n > 0)
b2_hff_rb_last->lag_counter++;
/* save filter state if needed */
if (save_counter == 0) {
PRINT_DBG(1, ("save current state\n"));
b2_hff_rb_put_state(&b2_hff_state);
save_counter = -1;
} else if (save_counter > 0) {
save_counter--;
}
#endif
}
} else {
b2_hff_ps_counter++;
}
}
/* check if resolution of INT32_TRIG_FRAC (14) is enough here */
void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu);
INT32_VECT3_ADD(*ecef, def->ecef);
}
/**
* Auto-throttle inner loop
* \brief
*/
void v_ctl_climb_loop( void )
{
// Airspeed setpoint rate limiter:
// AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1
float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew;
BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude);
v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
// Ground speed control loop (input: groundspeed error, output: airspeed controlled)
float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - (*stateGetHorizontalSpeedNorm_f()));
v_ctl_auto_groundspeed_sum_err += err_groundspeed;
BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain;
// Do not allow controlled airspeed below the setpoint
if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint_slew) {
v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew;
// reset integrator of ground speed loop
v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled/(v_ctl_auto_groundspeed_pgain*v_ctl_auto_groundspeed_igain);
}
#else
v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew;
#endif
// Airspeed outerloop: positive means we need to accelerate
float speed_error = v_ctl_auto_airspeed_controlled - (*stateGetAirspeed_f());
// Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration);
// Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
#ifndef SITL
struct Int32Vect3 accel_meas_body;
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel);
float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
#else
float vdot = 0;
#endif
// Acceleration Error: positive means UAV needs to accelerate: needs extra energy
float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot );
// Flight Path Outerloop: positive means needs to climb more: needs extra energy
float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_controlled;
// Total Energy Error: positive means energy should be added
float en_tot_err = gamma_err + vdot_err;
// Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up
float en_dis_err = gamma_err - vdot_err;
// Auto Cruise Throttle
if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB))
{
v_ctl_auto_throttle_nominal_cruise_throttle +=
v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude
+ en_tot_err * v_ctl_energy_total_igain * dt_attidude;
Bound(v_ctl_auto_throttle_nominal_cruise_throttle,0.0f,1.0f);
}
// Total Controller
float controlled_throttle = v_ctl_auto_throttle_nominal_cruise_throttle
+ v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
+ v_ctl_auto_throttle_of_airspeed_pgain * speed_error
+ v_ctl_energy_total_pgain * en_tot_err;
if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle==1))
{
// If your energy supply is not sufficient, then neglect the climb requirement
en_dis_err = -vdot_err;
// adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb
if(v_ctl_climb_setpoint>0) v_ctl_climb_setpoint += - 30. * dt_attidude;
if(v_ctl_climb_setpoint<0) v_ctl_climb_setpoint += 30. * dt_attidude;
}
/* pitch pre-command */
if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB))
{
v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt_attidude
+ v_ctl_energy_diff_igain * en_dis_err * dt_attidude;
Bound(v_ctl_auto_throttle_nominal_cruise_pitch,H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
}
float v_ctl_pitch_of_vz =
+ (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
- v_ctl_auto_pitch_of_airspeed_pgain * speed_error
+ v_ctl_auto_pitch_of_airspeed_dgain * vdot
+ v_ctl_energy_diff_pgain * en_dis_err
+ v_ctl_auto_throttle_nominal_cruise_pitch;
if(kill_throttle) v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1/V_CTL_GLIDE_RATIO;
v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;
Bound(v_ctl_pitch_setpoint,H_CTL_PITCH_MIN_SETPOINT,H_CTL_PITCH_MAX_SETPOINT)
ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);
v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
}
