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