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