void ahrs_fc_update_mag_2d(struct Int32Vect3 *mag, float dt) { struct FloatVect2 expected_ltp; VECT2_COPY(expected_ltp, ahrs_fc.mag_h); // normalize expected ltp in 2D (x,y) float_vect2_normalize(&expected_ltp); struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, *mag); struct FloatVect3 measured_ltp; float_rmat_transp_vmult(&measured_ltp, &ahrs_fc.ltp_to_imu_rmat, &measured_imu); struct FloatVect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y}; // normalize measured ltp in 2D (x,y) float_vect2_normalize(&measured_ltp_2d); struct FloatVect3 residual_ltp = { 0, 0, measured_ltp_2d.x *expected_ltp.y - measured_ltp_2d.y * expected_ltp.x }; // printf("res : %f\n", residual_ltp.z); struct FloatVect3 residual_imu; float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp); /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * weight * ahrs_fc.mag_cnt * with ahrs_fc.mag_cnt beeing the number of propagations since last update */ const float mag_rate_update_gain = 2 * ahrs_fc.mag_zeta * ahrs_fc.mag_omega * ahrs_fc.mag_cnt; RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, mag_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2 * dt */ const float mag_bias_update_gain = -(ahrs_fc.mag_omega * ahrs_fc.mag_omega) * dt; RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, mag_bias_update_gain); }
static void compute_points_from_bungee(void) { // Store init point (current position, where the plane will be released) VECT2_ASSIGN(init_point, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y); // Compute unitary 2D vector (bungee_point - init_point) = takeoff direction VECT2_DIFF(takeoff_dir, bungee_point, init_point); float_vect2_normalize(&takeoff_dir); // Find throttle point (the point where the throttle line and launch line intersect) // If TakeOff_Distance is positive, throttle point is after bungee point, before otherwise VECT2_SMUL(throttle_point, takeoff_dir, BUNGEE_TAKEOFF_DISTANCE); VECT2_SUM(throttle_point, bungee_point, throttle_point); }