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 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);
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_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_impl.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 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);
}
/** update ins state from horizontal filter */
static void ins_update_from_hff(void)
{
ins_impl.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
ins_impl.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
ins_impl.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
ins_impl.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
ins_impl.ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
ins_impl.ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
}
void ins_update_baro() {
#ifdef USE_VFF
if (baro.status == BS_RUNNING) {
if (!ins_baro_initialised) {
ins_qfe = baro.absolute;
ins_baro_initialised = TRUE;
}
ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
if (ins_vf_realign) {
ins_vf_realign = FALSE;
ins_qfe = baro.absolute;
#ifdef BOOZ2_SONAR
ins_sonar_offset = sonar_meas;
#endif
vff_realign(0.);
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);
ins_enu_pos.z = -ins_ltp_pos.z;
ins_enu_speed.z = -ins_ltp_speed.z;
ins_enu_accel.z = -ins_ltp_accel.z;
}
vff_update(alt_float);
}
#endif
}
void ins_update_baro() {
#if USE_VFF
if (baro.status == BS_RUNNING) {
if (!ins_baro_initialised) {
ins_qfe = baro.absolute;
ins_baro_initialised = TRUE;
}
if (ins.vf_realign) {
ins.vf_realign = FALSE;
ins_qfe = baro.absolute;
#if USE_SONAR
ins_sonar_offset = sonar_meas;
#endif
vff_realign(0.);
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 { /* not realigning, so normal update with baro measurement */
ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
vff_update(alt_float);
}
}
INS_NED_TO_STATE();
#endif
}
void handle_ins_msg(void) {
#if USE_INS
update_fw_estimator();
#endif
#if USE_IMU
#ifdef XSENS_BACKWARDS
if (imu_xsens.gyro_available) {
RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(ins_p), -RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
}
if (imu_xsens.accel_available) {
VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(ins_ax), -ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
}
if (imu_xsens.mag_available) {
VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(ins_mx), -MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
}
#else
if (imu_xsens.gyro_available) {
RATES_ASSIGN(imu.gyro_unscaled, RATE_BFP_OF_REAL(ins_p), RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
}
if (imu_xsens.accel_available) {
VECT3_ASSIGN(imu.accel_unscaled, ACCEL_BFP_OF_REAL(ins_ax), ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
}
if (imu_xsens.mag_available) {
VECT3_ASSIGN(imu.mag_unscaled, MAG_BFP_OF_REAL(ins_mx), MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
}
#endif /* XSENS_BACKWARDS */
#endif /* USE_IMU */
#if USE_GPS_XSENS
#ifndef ALT_KALMAN
#warning NO_VZ
#endif
// Horizontal speed
float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
if (gps.fix != GPS_FIX_3D) {
fspeed = 0;
}
gps.gspeed = fspeed * 100.;
gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
float fcourse = atan2f((float)ins_vy, (float)ins_vx);
gps.course = fcourse * 1e7;
#endif // USE_GPS_XSENS
}
inline static void h_ctl_cl_loop(void)
{
#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
#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 nz = -ACCEL_FLOAT_OF_BFP(accel_meas_body.z) / 9.81f;
// max load factor to be taken into acount
// to prevent negative flap movement du to negative acceleration
Bound(nz, 1.f, 2.f);
#else
float nz = 1.f;
#endif
#endif
// Compute a corrected airspeed corresponding to the current load factor nz
// with Cz the lift coef at 1g, Czn the lift coef at n g, both at the same speed V,
// the corrected airspeed Vn is so that nz = Czn/Cz = V^2 / Vn^2,
// thus Vn = V / sqrt(nz)
#if H_CTL_CL_LOOP_USE_AIRSPEED_SETPOINT
float corrected_airspeed = v_ctl_auto_airspeed_setpoint;
#else
float corrected_airspeed = stateGetAirspeed_f();
#endif
#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
corrected_airspeed /= sqrtf(nz);
#endif
Bound(corrected_airspeed, STALL_AIRSPEED, RACE_AIRSPEED);
float cmd = 0.f;
// deadband around NOMINAL_AIRSPEED, rest linear
if (corrected_airspeed > NOMINAL_AIRSPEED + H_CTL_CL_DEADBAND) {
cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_RACE - H_CTL_CL_FLAPS_NOMINAL) / (RACE_AIRSPEED - NOMINAL_AIRSPEED);
}
else if (corrected_airspeed < NOMINAL_AIRSPEED - H_CTL_CL_DEADBAND) {
cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_STALL - H_CTL_CL_FLAPS_NOMINAL) / (STALL_AIRSPEED - NOMINAL_AIRSPEED);
}
// no control in manual mode
if (pprz_mode == PPRZ_MODE_MANUAL) {
cmd = 0.f;
}
// bound max flap angle
Bound(cmd, H_CTL_CL_FLAPS_RACE, H_CTL_CL_FLAPS_STALL);
// from percent to pprz
cmd = cmd * MAX_PPRZ;
h_ctl_flaps_setpoint = TRIM_PPRZ(cmd);
}
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 ins_propagate() {
#if CONTROL_USE_VFF
if (altimeter_system_status == STATUS_INITIALIZED && ins.baro_initialised) {
float accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.z);
b2_vff_propagate(accel_float);
ins.ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
ins.ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
ins.ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
ins.enu_pos.z = -ins.ltp_pos.z;
ins.enu_speed.z = -ins.ltp_speed.z;
ins.enu_accel.z = -ins.ltp_accel.z;
}
#endif
#ifdef USE_HFF
if (booz_ahrs.status == BOOZ_AHRS_RUNNING &&
gps_state.fix == BOOZ2_GPS_FIX_3D && ins.ltp_initialised )
b2ins_propagate();
#endif
}
void ins_update_baro() {
int32_t baro_pressure = update_median_filter(&baro_median, baro.absolute);
if (baro.status == BS_RUNNING) {
if (!ins_baro_initialised) {
ins_qfe = baro_pressure;
ins_baro_initialised = TRUE;
}
if (ins.vf_realign) {
ins.vf_realign = FALSE;
ins_qfe = baro_pressure;
vff_realign(0.);
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 { /* not realigning, so normal update with baro measurement */
ins_baro_alt = ((baro_pressure - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
vff_update_baro(alt_float);
}
}
}
/** Adaptation function.
* @param zdd_meas vert accel measurement in m/s^2 with #INT32_ACCEL_FRAC
* @param thrust_applied controller input [0 : MAX_PPRZ]
* @param zd_ref vertical speed reference in m/s with #INT32_SPEED_FRAC
*/
void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
{
static const int32_t gv_adapt_min_cmd = GUIDANCE_V_ADAPT_MIN_CMD * MAX_PPRZ;
static const int32_t gv_adapt_max_cmd = GUIDANCE_V_ADAPT_MAX_CMD * MAX_PPRZ;
static const int32_t gv_adapt_max_accel = ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL);
/* Update only if accel and commands are in a valid range */
/* This also ensures we don't divide by zero */
if (thrust_applied < gv_adapt_min_cmd || thrust_applied > gv_adapt_max_cmd
|| zdd_meas < -gv_adapt_max_accel || zdd_meas > gv_adapt_max_accel) {
return;
}
/* We don't propagate state, it's constant ! */
/* We propagate our covariance */
gv_adapt_P = gv_adapt_P + GV_ADAPT_SYS_NOISE;
/* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 30 bits */
const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81,
INT32_ACCEL_FRAC) - zdd_meas) << (GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
if (g_m_zdd > 0) {
gv_adapt_Xmeas = (g_m_zdd + (thrust_applied >> 1)) / thrust_applied;
} else {
/** update ins state from vertical filter */
static void ins_update_from_vff(void)
{
ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff.zdotdot);
ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff.zdot);
ins_impl.ltp_pos.z = POS_BFP_OF_REAL(vff.z);
}
void ins_reset_altitude_ref(void)
{
#if USE_GPS
struct LlaCoor_i lla = {
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
ltp_def_from_lla_i(&ins_impl.ltp_def, &lla);
ins_impl.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
#endif
ins_impl.vf_reset = TRUE;
}
void ins_propagate(float dt)
{
/* 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, dt);
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();
}
static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure)
{
if (!ins_impl.baro_initialized && *pressure > 1e-7) {
// wait for a first positive value
ins_impl.qfe = *pressure;
ins_impl.baro_initialized = TRUE;
}
if (ins_impl.baro_initialized) {
if (ins_impl.vf_reset) {
ins_impl.vf_reset = FALSE;
ins_impl.qfe = *pressure;
vff_realign(0.);
ins_update_from_vff();
} else {
ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe);
#if USE_VFF_EXTENDED
vff_update_baro(ins_impl.baro_z);
#else
vff_update(ins_impl.baro_z);
#endif
}
ins_ned_to_state();
}
}
#if USE_GPS
void ins_update_gps(void)
{
if (gps.fix == GPS_FIX_3D) {
if (!ins_impl.ltp_initialized) {
ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
ins_impl.ltp_def.hmsl = gps.hmsl;
ins_impl.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
}
struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
/// @todo maybe use gps.ned_vel directly??
struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel);
#if INS_USE_GPS_ALT
vff_update_z_conf((float)gps_pos_cm_ned.z / 100.0, INS_VFF_R_GPS);
#endif
#if USE_HFF
/* horizontal gps transformed to NED in meters as float */
struct FloatVect2 gps_pos_m_ned;
VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
struct FloatVect2 gps_speed_m_s_ned;
VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);
if (ins_impl.hf_realign) {
ins_impl.hf_realign = FALSE;
const struct FloatVect2 zero = {0.0f, 0.0f};
b2_hff_realign(gps_pos_m_ned, zero);
}
// run horizontal filter
b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
// convert and copy result to ins_impl
ins_update_from_hff();
#else /* hff not used */
/* simply copy horizontal pos/speed from gps */
INT32_VECT2_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif /* USE_HFF */
ins_ned_to_state();
}
}
#endif /* USE_GPS */
#if USE_SONAR
static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance)
{
static float last_offset = 0.;
/* update filter assuming a flat ground */
if (*distance < INS_SONAR_MAX_RANGE && *distance > INS_SONAR_MIN_RANGE
#ifdef INS_SONAR_THROTTLE_THRESHOLD
&& stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#endif
#ifdef INS_SONAR_BARO_THRESHOLD
&& ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
#endif
&& ins_impl.update_on_agl
&& ins_impl.baro_initialized) {
vff_update_z_conf(-(*distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(*distance));
last_offset = vff.offset;
} else {
/* update offset with last value to avoid divergence */
vff_update_offset(last_offset);
}
}
#endif // USE_SONAR
/** initialize the local origin (ltp_def) from flight plan position */
static void ins_init_origin_from_flightplan(void)
{
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = NAV_LAT0;
llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
struct EcefCoor_i ecef_nav0;
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
ins_impl.ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
}
/** copy position and speed to state interface */
static void ins_ned_to_state(void)
{
stateSetPositionNed_i(&ins_impl.ltp_pos);
stateSetSpeedNed_i(&ins_impl.ltp_speed);
stateSetAccelNed_i(&ins_impl.ltp_accel);
#if defined SITL && USE_NPS
if (nps_bypass_ins) {
sim_overwrite_ins();
}
#endif
}
void ins_reset_altitude_ref(void)
{
#if USE_GPS
struct LlaCoor_i lla = {
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
ltp_def_from_lla_i(&ins_int.ltp_def, &lla);
ins_int.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_int.ltp_def);
#endif
ins_int.vf_reset = TRUE;
}
void ins_int_propagate(struct Int32Vect3 *accel, float dt)
{
/* 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, 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);
/* Propagate only if we got any measurement during the last INS_MAX_PROPAGATION_STEPS.
* Otherwise halt the propagation to not diverge and only set the acceleration.
* This should only be relevant in the startup phase when the baro is not yet initialized
* and there is no gps fix yet...
*/
if (ins_int.propagation_cnt < INS_MAX_PROPAGATION_STEPS) {
vff_propagate(z_accel_meas_float, dt);
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_int.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_int */
ins_update_from_hff();
#else
ins_int.ltp_accel.x = accel_meas_ltp.x;
ins_int.ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */
ins_ned_to_state();
/* increment the propagation counter, while making sure it doesn't overflow */
if (ins_int.propagation_cnt < 100 * INS_MAX_PROPAGATION_STEPS) {
ins_int.propagation_cnt++;
}
}
static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
{
if (!ins_int.baro_initialized && pressure > 1e-7) {
// wait for a first positive value
ins_int.qfe = pressure;
ins_int.baro_initialized = TRUE;
}
if (ins_int.baro_initialized) {
if (ins_int.vf_reset) {
ins_int.vf_reset = FALSE;
ins_int.qfe = pressure;
vff_realign(0.);
ins_update_from_vff();
} else {
ins_int.baro_z = -pprz_isa_height_of_pressure(pressure, ins_int.qfe);
#if USE_VFF_EXTENDED
vff_update_baro(ins_int.baro_z);
#else
vff_update(ins_int.baro_z);
#endif
}
ins_ned_to_state();
/* reset the counter to indicate we just had a measurement update */
ins_int.propagation_cnt = 0;
}
}
#if USE_GPS
void ins_int_update_gps(struct GpsState *gps_s)
{
if (gps_s->fix < GPS_FIX_3D) {
return;
}
if (!ins_int.ltp_initialized) {
ins_reset_local_origin();
}
struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps_s->ecef_pos);
/* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */
#ifdef INS_BODY_TO_GPS_X
/* body2gps translation in body frame */
struct Int32Vect3 b2g_b = {
.x = INS_BODY_TO_GPS_X,
.y = INS_BODY_TO_GPS_Y,
.z = INS_BODY_TO_GPS_Z
};
/* rotate offset given in body frame to navigation/ltp frame using current attitude */
struct Int32Quat q_b2n = *stateGetNedToBodyQuat_i();
QUAT_INVERT(q_b2n, q_b2n);
struct Int32Vect3 b2g_n;
int32_quat_vmult(&b2g_n, &q_b2n, &b2g_b);
/* subtract body2gps translation in ltp from gps position */
VECT3_SUB(gps_pos_cm_ned, b2g_n);
#endif
/// @todo maybe use gps_s->ned_vel directly??
struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps_s->ecef_vel);
#if INS_USE_GPS_ALT
vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
#endif
#if INS_USE_GPS_ALT_SPEED
vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
#endif
#if USE_HFF
/* horizontal gps transformed to NED in meters as float */
struct FloatVect2 gps_pos_m_ned;
VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
struct FloatVect2 gps_speed_m_s_ned;
VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);
if (ins_int.hf_realign) {
ins_int.hf_realign = FALSE;
const struct FloatVect2 zero = {0.0f, 0.0f};
b2_hff_realign(gps_pos_m_ned, zero);
}
// run horizontal filter
b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
// convert and copy result to ins_int
ins_update_from_hff();
#else /* hff not used */
/* simply copy horizontal pos/speed from gps */
INT32_VECT2_SCALE_2(ins_int.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif /* USE_HFF */
ins_ned_to_state();
/* reset the counter to indicate we just had a measurement update */
ins_int.propagation_cnt = 0;
}
#else
void ins_int_update_gps(struct GpsState *gps_s __attribute__((unused))) {}
#endif /* USE_GPS */
#if USE_SONAR
static void sonar_cb(uint8_t __attribute__((unused)) sender_id, float distance)
{
static float last_offset = 0.;
/* update filter assuming a flat ground */
if (distance < INS_SONAR_MAX_RANGE && distance > INS_SONAR_MIN_RANGE
#ifdef INS_SONAR_THROTTLE_THRESHOLD
&& stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#endif
#ifdef INS_SONAR_BARO_THRESHOLD
&& ins_int.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
#endif
&& ins_int.update_on_agl
&& ins_int.baro_initialized) {
vff_update_z_conf(-(distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance));
last_offset = vff.offset;
} else {
/* update offset with last value to avoid divergence */
vff_update_offset(last_offset);
}
/* reset the counter to indicate we just had a measurement update */
ins_int.propagation_cnt = 0;
}
#endif // USE_SONAR
/** initialize the local origin (ltp_def) from flight plan position */
static void ins_init_origin_from_flightplan(void)
{
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = NAV_LAT0;
llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
struct EcefCoor_i ecef_nav0;
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_i(&ins_int.ltp_def, &ecef_nav0);
ins_int.ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_int.ltp_def);
}
/** copy position and speed to state interface */
static void ins_ned_to_state(void)
{
stateSetPositionNed_i(&ins_int.ltp_pos);
stateSetSpeedNed_i(&ins_int.ltp_speed);
stateSetAccelNed_i(&ins_int.ltp_accel);
#if defined SITL && USE_NPS
if (nps_bypass_ins) {
sim_overwrite_ins();
}
#endif
}
void b2_hff_update_gps(void) {
b2_hff_lost_counter = 0;
#if USE_GPS_ACC4R
Rgps_pos = (float) gps.pacc / 100.;
if (Rgps_pos < HFF_R_POS_MIN)
Rgps_pos = HFF_R_POS_MIN;
Rgps_vel = (float) gps.sacc / 100.;
if (Rgps_vel < HFF_R_SPEED_MIN)
Rgps_vel = HFF_R_SPEED_MIN;
#endif
#ifdef GPS_LAG
if (GPS_LAG_N == 0) {
#endif
/* update filter state with measurement */
b2_hff_update_x(&b2_hff_state, ins_gps_pos_m_ned.x, Rgps_pos);
b2_hff_update_y(&b2_hff_state, ins_gps_pos_m_ned.y, Rgps_pos);
#ifdef HFF_UPDATE_SPEED
b2_hff_update_xdot(&b2_hff_state, ins_gps_speed_m_s_ned.x, Rgps_vel);
b2_hff_update_ydot(&b2_hff_state, ins_gps_speed_m_s_ned.y, Rgps_vel);
#endif
/* update ins state */
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
} else if (b2_hff_rb_n > 0) {
/* roll back if state was saved approx when GPS was valid */
lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err));
if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
b2_hff_rb_last->rollback = TRUE;
b2_hff_update_x(b2_hff_rb_last, ins_gps_pos_m_ned.x, Rgps_pos);
b2_hff_update_y(b2_hff_rb_last, ins_gps_pos_m_ned.y, Rgps_pos);
#ifdef HFF_UPDATE_SPEED
b2_hff_update_xdot(b2_hff_rb_last, ins_gps_speed_m_s_ned.x, Rgps_vel);
b2_hff_update_ydot(b2_hff_rb_last, ins_gps_speed_m_s_ned.y, Rgps_vel);
#endif
past_save_counter = GPS_DT_N-1;// + lag_counter_err;
PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter));
b2_hff_propagate_past(b2_hff_rb_last);
} else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N+1)) {
/* apparently missed a GPS update, try next saved state */
PRINT_DBG(2, ("try next saved state\n"));
b2_hff_rb_drop_last();
b2_hff_update_gps();
}
} else if (save_counter < 0) {
/* ringbuffer empty -> save output filter state at next GPS validity point in time */
save_counter = GPS_DT_N-1 - (GPS_LAG_N % GPS_DT_N);
PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
}
#endif /* GPS_LAG */
}
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++;
}
}
