void ahrs_dcm_update_mag(struct Int32Vect3 *mag) { #if USE_MAGNETOMETER #warning MAGNETOMETER FEEDBACK NOT TESTED YET MAG_FLOAT_OF_BFP(mag_float, *mag); float cos_roll; float sin_roll; float cos_pitch; float sin_pitch; cos_roll = cosf(ahrs_dcm.ltp_to_imu_euler.phi); sin_roll = sinf(ahrs_dcm.ltp_to_imu_euler.phi); cos_pitch = cosf(ahrs_dcm.ltp_to_imu_euler.theta); sin_pitch = sinf(ahrs_dcm.ltp_to_imu_euler.theta); // Pitch&Roll Compensation: MAG_Heading_X = mag->x * cos_pitch + mag->y * sin_roll * sin_pitch + mag->z * cos_roll * sin_pitch; MAG_Heading_Y = mag->y * cos_roll - mag->z * sin_roll; /* * // Magnetic Heading Heading = atan2(-Head_Y,Head_X); // Declination correction (if supplied) if( declination != 0.0 ) { Heading = Heading + declination; if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) Heading -= (2.0 * M_PI); else if (Heading < -M_PI) Heading += (2.0 * M_PI); } // Optimization for external DCM use. Calculate normalized components Heading_X = cos(Heading); Heading_Y = sin(Heading); */ struct FloatVect3 ltp_mag; ltp_mag.x = MAG_Heading_X; ltp_mag.y = MAG_Heading_Y; #if FLOAT_DCM_SEND_DEBUG // Downlink RunOnceEvery(10, DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, <p_mag.x, <p_mag.y, <p_mag.z)); #endif // Magnetic Heading // MAG_Heading = atan2(mag->y, -mag->x); #else // !USE_MAGNETOMETER // get rid of unused param warning... mag = mag; #endif }
void ahrs_update_mag(void) { #ifdef USE_MAGNETOMETER #warning MAGNETOMETER FEEDBACK NOT TESTED YET float cos_roll; float sin_roll; float cos_pitch; float sin_pitch; cos_roll = cos(ahrs_float.ltp_to_imu_euler.phi); sin_roll = sin(ahrs_float.ltp_to_imu_euler.phi); cos_pitch = cos(ahrs_float.ltp_to_imu_euler.theta); sin_pitch = sin(ahrs_float.ltp_to_imu_euler.theta); // Pitch&Roll Compensation: MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch; MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll; /* * // Magnetic Heading Heading = atan2(-Head_Y,Head_X); // Declination correction (if supplied) if( declination != 0.0 ) { Heading = Heading + declination; if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) Heading -= (2.0 * M_PI); else if (Heading < -M_PI) Heading += (2.0 * M_PI); } // Optimization for external DCM use. Calculate normalized components Heading_X = cos(Heading); Heading_Y = sin(Heading); */ struct FloatVect3 ltp_mag; ltp_mag.x = MAG_Heading_X; ltp_mag.y = MAG_Heading_Y; // Downlink RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, <p_mag.x, <p_mag.y, <p_mag.z)); // Magnetic Heading // MAG_Heading = atan2(imu.mag.y, -imu.mag.x); #endif }
static void send_mag(void) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &mag_float.x, &mag_float.y, &mag_float.z); }
Heading += (2.0 * M_PI); } // Optimization for external DCM use. Calculate normalized components Heading_X = cos(Heading); Heading_Y = sin(Heading); */ #if FLOAT_DCM_SEND_DEBUG struct FloatVect3 ltp_mag; ltp_mag.x = MAG_Heading_X; ltp_mag.y = MAG_Heading_Y; // Downlink RunOnceEvery(10, DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, <p_mag.x, <p_mag.y, <p_mag.z)); #endif // Magnetic Heading // MAG_Heading = atan2(mag->y, -mag->x); #else // !USE_MAGNETOMETER // get rid of unused param warning... mag = mag; #endif } void Normalize(void) { float error = 0; float temporary[3][3];