void ins_propagate() {
/* untilt accels and speeds */
FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel);
FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed);
//Add g to the accelerations
ins_impl.ltp_accel.z += 9.81;
//Save the accelerations and speeds
stateSetAccelNed_f(&ins_impl.ltp_accel);
stateSetSpeedNed_f(&ins_impl.ltp_speed);
//Don't set the height if we use the one from the gps
#if !USE_GPS_HEIGHT
//Set the height and save the position
ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
stateSetPositionNed_i(&ins_impl.ltp_pos);
#endif
}
void nav_catapult_highrate_module(void)
{
if (nav_catapult.status == NAV_CATAPULT_UNINIT || nav_catapult.status == NAV_CATAPULT_ARMED) {
nav_catapult.timer = 0;
// nothing more to do
return;
}
// increase timer
nav_catapult.timer++;
// wait for acceleration
if (nav_catapult.status == NAV_CATAPULT_WAIT_ACCEL) {
// launch detection filter
if (nav_catapult.timer < NAV_CATAPULT_ACCELERATION_DETECTION) {
// several consecutive measurements above threshold
#ifndef SITL
struct FloatVect3 *accel_ned = (struct FloatVect3 *)stateGetAccelNed_f();
struct FloatRMat *ned_to_body = stateGetNedToBodyRMat_f();
struct FloatVect3 accel_body;
float_rmat_transp_vmult(&accel_body, ned_to_body, accel_ned);
if (accel_body.x < nav_catapult.accel_threshold * 9.81) {
// accel is low, reset timer
nav_catapult.timer = 0;
return;
}
#else
if (launch != 1) {
// wait for simulated launch
nav_catapult.timer = 0;
return;
}
#endif
}
// launch was detected: Motor Delay Counter
else if (nav_catapult.timer >= nav_catapult.motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
// turn on motor
NavVerticalThrottleMode(MAX_PPRZ * nav_catapult.initial_throttle);
launch = 1;
// go to next stage
nav_catapult.status = NAV_CATAPULT_MOTOR_ON;
}
}
// reaching timeout and function still running
// shuting it down
if (nav_catapult.timer > NAV_CATAPULT_TIMEOUT * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
nav_catapult.status = NAV_CATAPULT_UNINIT;
nav_catapult_nav_catapult_highrate_module_status = MODULES_STOP;
}
}
void ahrs_update_heading(float heading) {
FLOAT_ANGLE_NORMALIZE(heading);
struct FloatRMat* ltp_to_body_rmat = stateGetNedToBodyRMat_f();
// row 0 of ltp_to_body_rmat = body x-axis in ltp frame
// we only consider x and y
struct FloatVect2 expected_ltp =
{ RMAT_ELMT(*ltp_to_body_rmat, 0, 0),
RMAT_ELMT(*ltp_to_body_rmat, 0, 1)
};
// expected_heading cross measured_heading
struct FloatVect3 residual_ltp =
{ 0,
0,
expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)
};
struct FloatVect3 residual_imu;
FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);
const float heading_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain);
float heading_bias_update_gain;
/* crude attempt to only update bias if deviation is small
* e.g. needed when you only have gps providing heading
* and the inital heading is totally different from
* the gps course information you get once you have a gps fix.
* Otherwise the bias will be falsely "corrected".
*/
if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.)))
heading_bias_update_gain = -2.5e-4;
else
heading_bias_update_gain = 0.0;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_bias_update_gain);
}
void ins_reset_altitude_ref( void ) {
#if USE_GPS
struct LlaCoor_i lla = {
state.ned_origin_i.lla.lon,
state.ned_origin_i.lla.lat,
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
}
void ins_propagate(float __attribute__((unused)) dt) {
/* untilt accels and speeds */
float_rmat_transp_vmult(&ins_impl.ltp_accel, stateGetNedToBodyRMat_f(), &ahrs_impl.accel);
float_rmat_transp_vmult(&ins_impl.ltp_speed, stateGetNedToBodyRMat_f(), &ahrs_impl.speed);
//Add g to the accelerations
ins_impl.ltp_accel.z += 9.81;
//Save the accelerations and speeds
stateSetAccelNed_f(&ins_impl.ltp_accel);
stateSetSpeedNed_f(&ins_impl.ltp_speed);
//Don't set the height if we use the one from the gps
#if !USE_GPS_HEIGHT
//Set the height and save the position
ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
stateSetPositionNed_i(&ins_impl.ltp_pos);
#endif
