/** 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 stateCalcHorizontalWindspeed_i(void) { if (bit_is_set(state.wind_air_status, WINDSPEED_I)) return; if (bit_is_set(state.wind_air_status, WINDSPEED_F)) { state.h_windspeed_i.x = SPEED_BFP_OF_REAL(state.h_windspeed_f.x); state.h_windspeed_i.y = SPEED_BFP_OF_REAL(state.h_windspeed_f.y); } /* set bit to indicate this representation is computed */ SetBit(state.rate_status, WINDSPEED_I); }
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_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 stateCalcHorizontalSpeedDir_i(void) { if (bit_is_set(state.speed_status, SPEED_HDIR_I)) return; if (bit_is_set(state.speed_status, SPEED_HDIR_F)){ state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f); } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); SetBit(state.speed_status, SPEED_NED_I); INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); SetBit(state.speed_status, SPEED_ENU_I); INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_HDIR_I); }
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_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 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 stateCalcHorizontalSpeedNorm_i(void) { if (bit_is_set(state.speed_status, SPEED_HNORM_I)) return; if (bit_is_set(state.speed_status, SPEED_HNORM_F)){ state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { /// @todo consider INT32_SPEED_FRAC INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i); } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); SetBit(state.speed_status, SPEED_HNORM_F); state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { /// @todo consider INT32_SPEED_FRAC INT32_VECT2_NORM(state.h_speed_norm_i, state.enu_speed_i); } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f); SetBit(state.speed_status, SPEED_HNORM_F); state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { /* transform ecef speed to ned, set status bit, then compute norm */ //foo /// @todo consider INT32_SPEED_FRAC //INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i); } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f); SetBit(state.speed_status, SPEED_NED_F); FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); SetBit(state.speed_status, SPEED_HNORM_F); state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else { //int32_t _norm_zero = 0; //return _norm_zero; } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_HNORM_I); }
void stateCalcAirspeed_i(void) { if (bit_is_set(state.wind_air_status, AIRSPEED_I)) return; if (bit_is_set(state.wind_air_status, AIRSPEED_F)) { state.airspeed_i = SPEED_BFP_OF_REAL(state.airspeed_f); } /* set bit to indicate this representation is computed */ SetBit(state.wind_air_status, AIRSPEED_I); }
void guidance_v_read_rc(void) { /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */ guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE]; /* used in RC_CLIMB */ guidance_v_rc_zd_sp = (MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]; DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND); static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) / (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) / (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); if(guidance_v_rc_zd_sp > 0) guidance_v_rc_zd_sp *= descent_scale; else guidance_v_rc_zd_sp *= climb_scale; }
void stateCalcHorizontalSpeedNorm_i(void) { if (bit_is_set(state.speed_status, SPEED_HNORM_I)) return; if (bit_is_set(state.speed_status, SPEED_HNORM_F)){ state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { int32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC; INT32_SQRT(state.h_speed_norm_i, n2); }
void aos_compute_sensors(void) { struct FloatRates gyro; RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias); // printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r)); float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise); RATES_BFP_OF_REAL(imu.gyro, gyro); RATES_BFP_OF_REAL(imu.gyro_prev, gyro); struct FloatVect3 g_ltp = {0., 0., 9.81}; struct FloatVect3 accelero_ltp; VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp); struct FloatVect3 accelero_imu; float_quat_vmult(&accelero_imu, &aos.ltp_to_imu_quat, &accelero_ltp); float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise); ACCELS_BFP_OF_REAL(imu.accel, accelero_imu); #ifndef DISABLE_MAG_UPDATE struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; struct FloatVect3 h_imu; float_quat_vmult(&h_imu, &aos.ltp_to_imu_quat, &h_earth); MAGS_BFP_OF_REAL(imu.mag, h_imu); #endif aos.heading_meas = aos.ltp_to_imu_euler.psi + get_gaussian_noise() * aos.heading_noise; #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN #if AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel); ahrs_impl.ltp_vel_norm_valid = true; #endif #if AHRS_TYPE == AHRS_TYPE_FCR2 ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel); ahrs_impl.ltp_vel_norm_valid = true; #endif #if AHRS_TYPE == AHRS_TYPE_FCR ahrs_impl.gps_speed = float_vect3_norm(&aos.ltp_vel); ahrs_impl.gps_age = 0; ahrs_update_gps(); //RunOnceEvery(100,printf("# gps accel: %f\n", ahrs_impl.gps_acceleration)); #endif #if AHRS_TYPE == AHRS_TYPE_ICQ ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(float_vect3_norm(&aos.ltp_vel)); ahrs_impl.ltp_vel_norm_valid = true; #endif #endif }
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); } } }
void aos_compute_sensors(void) { struct FloatRates gyro; RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias); // printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r)); float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise); RATES_BFP_OF_REAL(imu.gyro, gyro); RATES_BFP_OF_REAL(imu.gyro_prev, gyro); struct FloatVect3 g_ltp = {0., 0., 9.81}; struct FloatVect3 accelero_ltp; VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp); struct FloatVect3 accelero_imu; FLOAT_QUAT_VMULT(accelero_imu, aos.ltp_to_imu_quat, accelero_ltp); float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise); ACCELS_BFP_OF_REAL(imu.accel, accelero_imu); struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; struct FloatVect3 h_imu; FLOAT_QUAT_VMULT(h_imu, aos.ltp_to_imu_quat, h_earth); MAGS_BFP_OF_REAL(imu.mag, h_imu); #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN #if AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ VECT3_COPY(ahrs_impl.est_ltp_speed, aos.ltp_vel); #endif #if AHRS_TYPE == AHRS_TYPE_ICQ ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel)); #endif #endif }
void autopilot_periodic(void) { RunOnceEvery(NAV_PRESCALER, nav_periodic_task()); #if FAILSAFE_GROUND_DETECT INFO("Using FAILSAFE_GROUND_DETECT")//使用模式FAILSAFE_GROUND_DETECT失效保护_ if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) { autopilot_set_mode(AP_MODE_KILL); autopilot_detect_ground = FALSE; } #endif /* set failsafe commands, if in FAILSAFE or KILL mode */ #if !FAILSAFE_GROUND_DETECT if (autopilot_mode == AP_MODE_KILL || autopilot_mode == AP_MODE_FAILSAFE) { #else if (autopilot_mode == AP_MODE_KILL) { #endif SetCommands(commands_failsafe); } else { /* 计算向导模式下的两个方向水平和垂直方向的姿态信息*/ guidance_v_run( autopilot_in_flight ); guidance_h_run( autopilot_in_flight ); /*设置旋翼的命令:稳定模式配置,飞行模式,电机打开状态*/ SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on); } } /*飞控模式设置函数*/ void autopilot_set_mode(uint8_t new_autopilot_mode) { /* force kill mode as long as AHRS is not aligned 强制杀死模式只要ahrs不是均衡的 */ if (!ahrs_is_aligned()) new_autopilot_mode = AP_MODE_KILL; /* 新的飞行模式*/ if (new_autopilot_mode != autopilot_mode) { /* horizontal mode 水平模式 */ switch (new_autopilot_mode) { case AP_MODE_FAILSAFE://失效保护模式 #ifndef KILL_AS_FAILSAFE stab_att_sp_euler.phi = 0; stab_att_sp_euler.theta = 0; guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; #endif case AP_MODE_KILL://kill模式 autopilot_set_motors_on(FALSE); autopilot_in_flight = FALSE; autopilot_in_flight_counter = 0; guidance_h_mode_changed(GUIDANCE_H_MODE_KILL); break; case AP_MODE_RC_DIRECT://RC指挥模式 guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT); break; case AP_MODE_RATE_DIRECT://速度指挥模式 case AP_MODE_RATE_Z_HOLD://Z轴(高度)速度指挥模式 guidance_h_mode_changed(GUIDANCE_H_MODE_RATE); break; case AP_MODE_ATTITUDE_RC_CLIMB://RC 姿态爬升模式 case AP_MODE_ATTITUDE_DIRECT://姿态向导模式 case AP_MODE_ATTITUDE_CLIMB://姿态爬升模式 case AP_MODE_ATTITUDE_Z_HOLD://高度保持模式 guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; case AP_MODE_FORWARD://前进模式 guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD); break; case AP_MODE_CARE_FREE_DIRECT://自由模式 guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE); break; case AP_MODE_HOVER_DIRECT://盘旋向导模式 case AP_MODE_HOVER_CLIMB://盘旋爬升模式 case AP_MODE_HOVER_Z_HOLD://盘旋高度保持模式 guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER); break; case AP_MODE_NAV://导航模式 guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); break; default: break; } /* vertical mode 垂直模式*/ switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5); guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); break; #endif case AP_MODE_KILL: guidance_v_mode_changed(GUIDANCE_V_MODE_KILL); break; case AP_MODE_RC_DIRECT: case AP_MODE_RATE_DIRECT: case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_HOVER_DIRECT: case AP_MODE_CARE_FREE_DIRECT: case AP_MODE_FORWARD: guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT); break; case AP_MODE_RATE_RC_CLIMB: case AP_MODE_ATTITUDE_RC_CLIMB: guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB); break; case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_HOVER_CLIMB: guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); break; case AP_MODE_RATE_Z_HOLD: case AP_MODE_ATTITUDE_Z_HOLD: case AP_MODE_HOVER_Z_HOLD: guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER); break; case AP_MODE_NAV: guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); break; default: break; } autopilot_mode = new_autopilot_mode; } } static inline void autopilot_check_in_flight( bool_t motors_on ) { if (autopilot_in_flight) { if (autopilot_in_flight_counter > 0) { if (THROTTLE_STICK_DOWN()) { autopilot_in_flight_counter--; if (autopilot_in_flight_counter == 0) { autopilot_in_flight = FALSE; } } else { /* !THROTTLE_STICK_DOWN */ autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME; } } } else { /* not in flight */ if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME && motors_on) { if (!THROTTLE_STICK_DOWN()) { autopilot_in_flight_counter++; if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) autopilot_in_flight = TRUE; } else { /* THROTTLE_STICK_DOWN */ autopilot_in_flight_counter = 0; } } } } void autopilot_set_motors_on(bool_t motors_on) { if (ahrs_is_aligned() && motors_on) autopilot_motors_on = TRUE; else autopilot_motors_on = FALSE; kill_throttle = ! autopilot_motors_on; autopilot_arming_set(autopilot_motors_on); } void autopilot_on_rc_frame(void) { //是否关闭开关 if (kill_switch_is_on()) autopilot_set_mode(AP_MODE_KILL);//关闭自驾模式 else { uint8_t new_autopilot_mode = 0; AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode); /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */ if (!(new_autopilot_mode == AP_MODE_NAV #if USE_GPS && GpsIsLost()//GPS丢失时不要使用NAV导航模式 #endif )) autopilot_set_mode(new_autopilot_mode); } /* if not in FAILSAFE mode check motor and in_flight status, read RC */ //飞行模式不是:失效保护时,检查电机和飞行状态,读RC if (autopilot_mode > AP_MODE_FAILSAFE) { /* if there are some commands that should always be set from RC, do it */ //如果有来自于RC的命令,则去执行命令 #ifdef SetAutoCommandsFromRC SetAutoCommandsFromRC(commands, radio_control.values); #endif /* if not in NAV_MODE set commands from the rc */ //如果不是导航模式,设置来自RC的命令 #ifdef SetCommandsFromRC if (autopilot_mode != AP_MODE_NAV) { SetCommandsFromRC(commands, radio_control.values); } #endif /* an arming sequence is used to start/stop motors */ // 一个。。。序列用来启动和关闭电机 autopilot_arming_check_motors_on(); kill_throttle = ! autopilot_motors_on;//关闭电机 autopilot_check_in_flight(autopilot_motors_on); guidance_v_read_rc();//的垂直方向的信息 guidance_h_read_rc(autopilot_in_flight);//读EC } }
void autopilot_set_mode(uint8_t new_autopilot_mode) { if (new_autopilot_mode != autopilot_mode) { /* horizontal mode */ switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE stab_att_sp_euler.phi = 0; stab_att_sp_euler.theta = 0; guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; #endif case AP_MODE_KILL: autopilot_motors_on = FALSE; guidance_h_mode_changed(GUIDANCE_H_MODE_KILL); break; case AP_MODE_RATE_DIRECT: case AP_MODE_RATE_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_RATE); break; case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_ATTITUDE_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; case AP_MODE_HOVER_DIRECT: case AP_MODE_HOVER_CLIMB: case AP_MODE_HOVER_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER); break; case AP_MODE_NAV: guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); break; default: break; } /* vertical mode */ switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5); guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); break; #endif case AP_MODE_KILL: guidance_v_mode_changed(GUIDANCE_V_MODE_KILL); break; case AP_MODE_RATE_DIRECT: case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_HOVER_DIRECT: guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT); break; case AP_MODE_RATE_RC_CLIMB: case AP_MODE_ATTITUDE_RC_CLIMB: guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB); break; case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_HOVER_CLIMB: guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); break; case AP_MODE_RATE_Z_HOLD: case AP_MODE_ATTITUDE_Z_HOLD: case AP_MODE_HOVER_Z_HOLD: guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER); break; case AP_MODE_NAV: guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); break; default: break; } autopilot_mode = new_autopilot_mode; } }
void autopilot_periodic(void) { RunOnceEvery(NAV_PRESCALER, nav_periodic_task()); #if FAILSAFE_GROUND_DETECT INFO("Using FAILSAFE_GROUND_DETECT") if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) { autopilot_set_mode(AP_MODE_KILL); autopilot_detect_ground = FALSE; } #endif /* set failsafe commands, if in FAILSAFE or KILL mode */ #if !FAILSAFE_GROUND_DETECT if (autopilot_mode == AP_MODE_KILL || autopilot_mode == AP_MODE_FAILSAFE) { #else if (autopilot_mode == AP_MODE_KILL) { #endif SetCommands(commands_failsafe); } else { guidance_v_run( autopilot_in_flight ); guidance_h_run( autopilot_in_flight ); SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on); } } void autopilot_set_mode(uint8_t new_autopilot_mode) { /* force kill mode as long as AHRS is not aligned */ if (!ahrs_is_aligned()) new_autopilot_mode = AP_MODE_KILL; if (new_autopilot_mode != autopilot_mode) { /* horizontal mode */ switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE stab_att_sp_euler.phi = 0; stab_att_sp_euler.theta = 0; guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; #endif case AP_MODE_KILL: autopilot_set_motors_on(FALSE); autopilot_in_flight = FALSE; autopilot_in_flight_counter = 0; guidance_h_mode_changed(GUIDANCE_H_MODE_KILL); break; case AP_MODE_RC_DIRECT: guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT); break; case AP_MODE_RATE_DIRECT: case AP_MODE_RATE_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_RATE); break; case AP_MODE_ATTITUDE_RC_CLIMB: case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_ATTITUDE_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; case AP_MODE_FORWARD: guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD); break; case AP_MODE_CARE_FREE_DIRECT: guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE); break; case AP_MODE_HOVER_DIRECT: case AP_MODE_HOVER_CLIMB: case AP_MODE_HOVER_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER); break; case AP_MODE_NAV: guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); break; default: break; } /* vertical mode */ switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5); guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); break; #endif case AP_MODE_KILL: guidance_v_mode_changed(GUIDANCE_V_MODE_KILL); break; case AP_MODE_RC_DIRECT: case AP_MODE_RATE_DIRECT: case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_HOVER_DIRECT: case AP_MODE_CARE_FREE_DIRECT: case AP_MODE_FORWARD: guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT); break; case AP_MODE_RATE_RC_CLIMB: case AP_MODE_ATTITUDE_RC_CLIMB: guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB); break; case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_HOVER_CLIMB: guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); break; case AP_MODE_RATE_Z_HOLD: case AP_MODE_ATTITUDE_Z_HOLD: case AP_MODE_HOVER_Z_HOLD: guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER); break; case AP_MODE_NAV: guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); break; default: break; } autopilot_mode = new_autopilot_mode; } } static inline void autopilot_check_in_flight( bool_t motors_on ) { if (autopilot_in_flight) { if (autopilot_in_flight_counter > 0) { if (THROTTLE_STICK_DOWN()) { autopilot_in_flight_counter--; if (autopilot_in_flight_counter == 0) { autopilot_in_flight = FALSE; } } else { /* !THROTTLE_STICK_DOWN */ autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME; } } } else { /* not in flight */ if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME && motors_on) { if (!THROTTLE_STICK_DOWN()) { autopilot_in_flight_counter++; if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) autopilot_in_flight = TRUE; } else { /* THROTTLE_STICK_DOWN */ autopilot_in_flight_counter = 0; } } } } void autopilot_set_motors_on(bool_t motors_on) { if (ahrs_is_aligned() && motors_on) autopilot_motors_on = TRUE; else autopilot_motors_on = FALSE; kill_throttle = ! autopilot_motors_on; autopilot_arming_set(autopilot_motors_on); } void autopilot_on_rc_frame(void) { if (kill_switch_is_on()) autopilot_set_mode(AP_MODE_KILL); else { uint8_t new_autopilot_mode = 0; AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode); /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */ if (!(new_autopilot_mode == AP_MODE_NAV #if USE_GPS && GpsIsLost() #endif )) autopilot_set_mode(new_autopilot_mode); } /* if not in FAILSAFE mode check motor and in_flight status, read RC */ if (autopilot_mode > AP_MODE_FAILSAFE) { /* if there are some commands that should always be set from RC, do it */ #ifdef SetAutoCommandsFromRC SetAutoCommandsFromRC(commands, radio_control.values); #endif /* if not in NAV_MODE set commands from the rc */ #ifdef SetCommandsFromRC if (autopilot_mode != AP_MODE_NAV) { SetCommandsFromRC(commands, radio_control.values); } #endif /* an arming sequence is used to start/stop motors */ autopilot_arming_check_motors_on(); kill_throttle = ! autopilot_motors_on; autopilot_check_in_flight(autopilot_motors_on); guidance_v_read_rc(); guidance_h_read_rc(autopilot_in_flight); } }
/** 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 autopilot_set_mode(uint8_t new_autopilot_mode) { /* force startup mode (default is kill) as long as AHRS is not aligned */ if (!ahrs_is_aligned()) { new_autopilot_mode = MODE_STARTUP; } if (new_autopilot_mode != autopilot_mode) { /* horizontal mode */ switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE stabilization_attitude_set_failsafe_setpoint(); guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; #endif case AP_MODE_KILL: autopilot_in_flight = false; autopilot_in_flight_counter = 0; guidance_h_mode_changed(GUIDANCE_H_MODE_KILL); break; case AP_MODE_RC_DIRECT: guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT); break; case AP_MODE_RATE_RC_CLIMB: case AP_MODE_RATE_DIRECT: case AP_MODE_RATE_Z_HOLD: #if USE_STABILIZATION_RATE guidance_h_mode_changed(GUIDANCE_H_MODE_RATE); #else return; #endif break; case AP_MODE_ATTITUDE_RC_CLIMB: case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_ATTITUDE_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; case AP_MODE_FORWARD: guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD); break; case AP_MODE_CARE_FREE_DIRECT: guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE); break; case AP_MODE_HOVER_DIRECT: case AP_MODE_HOVER_CLIMB: case AP_MODE_HOVER_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER); break; case AP_MODE_HOME: case AP_MODE_NAV: guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); break; case AP_MODE_MODULE: #ifdef GUIDANCE_H_MODE_MODULE_SETTING guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING); #endif break; case AP_MODE_FLIP: guidance_h_mode_changed(GUIDANCE_H_MODE_FLIP); break; case AP_MODE_GUIDED: guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED); break; default: break; } /* vertical mode */ switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED); break; #endif case AP_MODE_KILL: autopilot_set_motors_on(FALSE); stabilization_cmd[COMMAND_THRUST] = 0; guidance_v_mode_changed(GUIDANCE_V_MODE_KILL); break; case AP_MODE_RC_DIRECT: case AP_MODE_RATE_DIRECT: case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_HOVER_DIRECT: case AP_MODE_CARE_FREE_DIRECT: case AP_MODE_FORWARD: guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT); break; case AP_MODE_RATE_RC_CLIMB: case AP_MODE_ATTITUDE_RC_CLIMB: guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB); break; case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_HOVER_CLIMB: guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); break; case AP_MODE_RATE_Z_HOLD: case AP_MODE_ATTITUDE_Z_HOLD: case AP_MODE_HOVER_Z_HOLD: guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER); break; case AP_MODE_HOME: case AP_MODE_NAV: guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); break; case AP_MODE_MODULE: #ifdef GUIDANCE_V_MODE_MODULE_SETTING guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING); #endif break; case AP_MODE_FLIP: guidance_v_mode_changed(GUIDANCE_V_MODE_FLIP); break; case AP_MODE_GUIDED: guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED); break; default: break; } //if switching to rate mode but rate mode is not defined, the function returned autopilot_mode = new_autopilot_mode; } }
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++; } }
void autopilot_set_mode(uint8_t new_autopilot_mode) { /* force kill mode as long as AHRS is not aligned */ if (!ahrs_is_aligned()) new_autopilot_mode = AP_MODE_KILL; if (new_autopilot_mode != autopilot_mode) { /* horizontal mode */ switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE stabilization_attitude_set_failsafe_setpoint(); guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; #endif case AP_MODE_KILL: autopilot_in_flight = FALSE; autopilot_in_flight_counter = 0; guidance_h_mode_changed(GUIDANCE_H_MODE_KILL); break; case AP_MODE_RC_DIRECT: guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT); break; case AP_MODE_RATE_DIRECT: case AP_MODE_RATE_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_RATE); break; case AP_MODE_ATTITUDE_RC_CLIMB: case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_ATTITUDE_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; case AP_MODE_FORWARD: guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD); break; case AP_MODE_CARE_FREE_DIRECT: guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE); break; case AP_MODE_HOVER_DIRECT: case AP_MODE_HOVER_CLIMB: case AP_MODE_HOVER_Z_HOLD: guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER); break; case AP_MODE_NAV: guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); break; default: break; } /* vertical mode */ switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5); break; #endif case AP_MODE_KILL: autopilot_set_motors_on(FALSE); stabilization_cmd[COMMAND_THRUST] = 0; guidance_v_mode_changed(GUIDANCE_V_MODE_KILL); break; case AP_MODE_RC_DIRECT: case AP_MODE_RATE_DIRECT: case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_HOVER_DIRECT: case AP_MODE_CARE_FREE_DIRECT: case AP_MODE_FORWARD: guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT); break; case AP_MODE_RATE_RC_CLIMB: case AP_MODE_ATTITUDE_RC_CLIMB: guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB); break; case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_HOVER_CLIMB: guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); break; case AP_MODE_RATE_Z_HOLD: case AP_MODE_ATTITUDE_Z_HOLD: case AP_MODE_HOVER_Z_HOLD: guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER); break; case AP_MODE_NAV: guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); break; default: break; } autopilot_mode = new_autopilot_mode; } }
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 */ }