/* Parse the InterMCU message */ static inline void mag_pitot_parse_msg(void) { uint32_t now_ts = get_sys_time_usec(); /* Parse the mag-pitot message */ uint8_t msg_id = pprzlink_get_msg_id(mp_msg_buf); #if PPRZLINK_DEFAULT_VER == 2 // Check that the message is really a intermcu message if (pprzlink_get_msg_class_id(mp_msg_buf) == DL_intermcu_CLASS_ID) { #endif switch (msg_id) { /* Got a magneto message */ case DL_IMCU_REMOTE_MAG: { struct Int32Vect3 raw_mag; // Read the raw magneto information raw_mag.x = DL_IMCU_REMOTE_MAG_mag_x(mp_msg_buf); raw_mag.y = DL_IMCU_REMOTE_MAG_mag_y(mp_msg_buf); raw_mag.z = DL_IMCU_REMOTE_MAG_mag_z(mp_msg_buf); // Rotate the magneto struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&mag_pitot.imu_to_mag); int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &raw_mag); // Send and set the magneto IMU data imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_MAG_PITOT_ID, now_ts, &imu.mag); break; } /* Got a barometer message */ case DL_IMCU_REMOTE_BARO: { float pitot_stat = DL_IMCU_REMOTE_BARO_pitot_stat(mp_msg_buf); float pitot_temp = DL_IMCU_REMOTE_BARO_pitot_temp(mp_msg_buf); AbiSendMsgBARO_ABS(IMU_MAG_PITOT_ID, pitot_stat); AbiSendMsgTEMPERATURE(IMU_MAG_PITOT_ID, pitot_temp); break; } /* Got an airspeed message */ case DL_IMCU_REMOTE_AIRSPEED: { // Should be updated to differential pressure float pitot_ias = DL_IMCU_REMOTE_AIRSPEED_pitot_IAS(mp_msg_buf); AbiSendMsgAIRSPEED(IMU_MAG_PITOT_ID, pitot_ias); break; } default: break; } #if PPRZLINK_DEFAULT_VER == 2 } #endif }
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 ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt) { // check if we had at least one propagation since last update if (ahrs_icq.accel_cnt == 0) { return; } // c2 = ltp z-axis in imu-frame struct Int32RMat ltp_to_imu_rmat; int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat); struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0, 2), RMAT_ELMT(ltp_to_imu_rmat, 1, 2), RMAT_ELMT(ltp_to_imu_rmat, 2, 2) }; struct Int32Vect3 residual; struct Int32Vect3 pseudo_gravity_measurement; if (ahrs_icq.correct_gravity && ahrs_icq.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame * a_c_body = omega x (omega x r) * (omega x r) = tangential velocity in body frame * a_c_body = omega x vel_tangential_body * assumption: tangential velocity only along body x-axis */ // FIXME: check overflows ! #define COMPUTATION_FRAC 16 #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC const struct Int32Vect3 vel_tangential_body = {ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); struct Int32Rates body_rate; int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate); struct Int32Vect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body); INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC); /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement * of the gravity vector */ VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_imu); } else {
/** * Handle all the events of the Navstik IMU components. * When there is data available convert it to the correct axis and save it in the imu structure. */ void imu_bebop_event(void) { uint32_t now_ts = get_sys_time_usec(); /* MPU-60x0 event taks */ mpu60x0_i2c_event(&imu_bebop.mpu); if (imu_bebop.mpu.data_available) { /* default orientation of the MPU is upside down sor corrigate this here */ RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q, -imu_bebop.mpu.data_rates.rates.r); VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y, -imu_bebop.mpu.data_accel.vect.z); imu_bebop.mpu.data_available = false; imu_scale_gyro(&imu); imu_scale_accel(&imu); AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); } /* AKM8963 event task */ ak8963_event(&imu_bebop.ak); if (imu_bebop.ak.data_available) { #if BEBOP_VERSION2 struct Int32Vect3 mag_temp; // In the second bebop version the magneto is turned 90 degrees VECT3_ASSIGN(mag_temp, -imu_bebop.ak.data.vect.x, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z); // Rotate the magneto struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&imu_to_mag_bebop); int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &mag_temp); #else //BEBOP regular first verion VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z); #endif imu_bebop.ak.data_available = false; imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); } }