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); }