static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up) { if (msg_up->valid.vane && vane_callback) vane_callback(0, msg_up->vane_angle1, msg_up->vane_angle2); // Fill pressure data if (msg_up->valid.pressure_absolute && pressure_absolute_callback) pressure_absolute_callback(0, msg_up->pressure_absolute); if (msg_up->valid.pressure_differential && pressure_differential_callback) pressure_differential_callback(0, (32768 + msg_up->pressure_differential)); if (msg_up->valid.adc) { if(adc_callback) { adc_callback(msg_up->adc.channels); } } // Fill radio data if (msg_up->valid.rc && radio_control_callback) { radio_control.values[RADIO_ROLL] = msg_up->rc_roll; radio_control.values[RADIO_PITCH] = msg_up->rc_pitch; radio_control.values[RADIO_YAW] = msg_up->rc_yaw; radio_control.values[RADIO_THROTTLE] = msg_up->rc_thrust; radio_control.values[RADIO_MODE] = msg_up->rc_mode; radio_control.values[RADIO_KILL] = msg_up->rc_kill; radio_control.values[RADIO_GEAR] = msg_up->rc_gear; radio_control.values[RADIO_AUX2] = msg_up->rc_aux2; radio_control.values[RADIO_AUX3] = msg_up->rc_aux3; radio_control_callback(); } // always fill status, it may change even when in the case when there is no new data radio_control.status = msg_up->rc_status; // Fill IMU data imuFloat.gyro.p = RATE_FLOAT_OF_BFP(msg_up->gyro.p); imuFloat.gyro.q = RATE_FLOAT_OF_BFP(msg_up->gyro.q); imuFloat.gyro.r = RATE_FLOAT_OF_BFP(msg_up->gyro.r); imuFloat.accel.x = ACCEL_FLOAT_OF_BFP(msg_up->accel.x); imuFloat.accel.y = ACCEL_FLOAT_OF_BFP(msg_up->accel.y); imuFloat.accel.z = ACCEL_FLOAT_OF_BFP(msg_up->accel.z); imuFloat.mag.x = MAG_FLOAT_OF_BFP(msg_up->mag.x); imuFloat.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y); imuFloat.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z); imuFloat.sample_count = msg_up->imu_tick; if (msg_up->valid.imu) rdyb_booz_imu_update(&imuFloat); }
void ArduIMU_event(void) { // Handle INS I2C event if (ardu_ins_trans.status == I2CTransSuccess) { // received data from I2C transaction recievedData[0] = (ardu_ins_trans.buf[1] << 8) | ardu_ins_trans.buf[0]; recievedData[1] = (ardu_ins_trans.buf[3] << 8) | ardu_ins_trans.buf[2]; recievedData[2] = (ardu_ins_trans.buf[5] << 8) | ardu_ins_trans.buf[4]; recievedData[3] = (ardu_ins_trans.buf[7] << 8) | ardu_ins_trans.buf[6]; recievedData[4] = (ardu_ins_trans.buf[9] << 8) | ardu_ins_trans.buf[8]; recievedData[5] = (ardu_ins_trans.buf[11] << 8) | ardu_ins_trans.buf[10]; recievedData[6] = (ardu_ins_trans.buf[13] << 8) | ardu_ins_trans.buf[12]; recievedData[7] = (ardu_ins_trans.buf[15] << 8) | ardu_ins_trans.buf[14]; recievedData[8] = (ardu_ins_trans.buf[17] << 8) | ardu_ins_trans.buf[16]; // Update ArduIMU data arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]) - ins_roll_neutral; arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]) - ins_pitch_neutral; arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]); arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]); arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]); arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]); arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]); arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]); arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]); // Update estimator stateSetNedToBodyEulers_f(&arduimu_eulers); stateSetBodyRates_f(&arduimu_rates); stateSetAccelNed_f(&((struct NedCoor_f)arduimu_accel)); ardu_ins_trans.status = I2CTransDone; #ifdef ARDUIMU_SYNC_SEND // uint8_t arduimu_id = 102; //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi, &arduimu_id)); RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r)); RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z)); #endif } else if (ardu_ins_trans.status == I2CTransFailed) { ardu_ins_trans.status = I2CTransDone; } // Handle GPS I2C event if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) { ardu_gps_trans.status = I2CTransDone; } }
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 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 ArduIMU_event( void ) { // Handle INS I2C event if (ardu_ins_trans.status == I2CTransSuccess) { // received data from I2C transaction recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0]; recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2]; recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4]; recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6]; recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8]; recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10]; recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12]; recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14]; recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16]; // Update ArduIMU data arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]); arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]); arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]); arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]); arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]); arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]); arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]); arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]); arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]); // Update estimator estimator_phi = arduimu_eulers.phi - ins_roll_neutral; estimator_theta = arduimu_eulers.theta - ins_pitch_neutral; estimator_p = arduimu_rates.p; ardu_ins_trans.status = I2CTransDone; //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi)); RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r)); RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z)); } else if (ardu_ins_trans.status == I2CTransFailed) { ardu_ins_trans.status = I2CTransDone; } // Handle GPS I2C event if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) { ardu_gps_trans.status = I2CTransDone; } }
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 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; 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); 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 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 五个连续的测量会大于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 nav_catapult_highrate_module(void) { // Only run when if (nav_catapult_armed) { if (nav_catapult_launch < nav_catapult_heading_delay) 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) { // Turn on Motor NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle)); launch = 1; } } else { nav_catapult_launch = 0; } }
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 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++; } }
/** * 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); }
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_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 /* rotate imu accel measurement to body frame and filter */ struct Int32Vect3 acc_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); int32_rmat_transp_vmult(&acc_meas_body, body_to_imu_rmat, &imu.accel); struct Int32Vect3 acc_body_filtered; acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, acc_meas_body.x); acc_body_filtered.y = update_butterworth_2_low_pass_int(&filter_y, acc_meas_body.y); acc_body_filtered.z = update_butterworth_2_low_pass_int(&filter_z, acc_meas_body.z); /* 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) { struct Int32Vect3 filtered_accel_ltp; struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i(); int32_rmat_transp_vmult(&filtered_accel_ltp, ltp_to_body_rmat, &acc_body_filtered); b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x); b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_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, DT_HFILTER); b2_hff_propagate_y(&b2_hff_state, DT_HFILTER); #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++; } }