/** * Locally cache some variables from the AtttitudeSettings object */ static void settingsUpdatedCb(UAVObjEvent * objEv) { RevoCalibrationGet(&cal); mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X]; mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y]; mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z]; mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X]; mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y]; mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z]; accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X]; accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y]; accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z]; accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X]; accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y]; accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z]; // Do not store gyros_bias here as that comes from the state estimator and should be // used from GyroBias directly // Zero out any adaptive tracking MagBiasData magBias; MagBiasGet(&magBias); magBias.x = 0; magBias.y = 0; magBias.z = 0; MagBiasSet(&magBias); AttitudeSettingsData attitudeSettings; AttitudeSettingsGet(&attitudeSettings); bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE); // Indicates not to expend cycles on rotation if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && attitudeSettings.BoardRotation[2] == 0) { rotate = 0; } else { float rotationQuat[4]; const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]}; RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R); rotate = 1; } }
static void settingsUpdatedCb(UAVObjEvent * ev) { if (ev == NULL || ev->obj == RevoCalibrationHandle()) { RevoCalibrationGet(&revoCalibration); /* When the revo calibration is updated, update the GyroBias object */ GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosBias.x = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X]; gyrosBias.y = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y]; gyrosBias.z = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z]; GyrosBiasSet(&gyrosBias); gyroBiasSettingsUpdated = true; // In case INS currently running INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); INSSetBaroVar(revoCalibration.baro_var); } if(ev == NULL || ev->obj == HomeLocationHandle()) { HomeLocationGet(&homeLocation); // Compute matrix to convert deltaLLA to NED float lat, alt; lat = homeLocation.Latitude / 10.0e6f * DEG2RAD; alt = homeLocation.Altitude; T[0] = alt+6.378137E6f; T[1] = cosf(lat)*(alt+6.378137E6f); T[2] = -1.0f; } if (ev == NULL || ev->obj == AttitudeSettingsHandle()) AttitudeSettingsGet(&attitudeSettings); if (ev == NULL || ev->obj == RevoSettingsHandle()) RevoSettingsGet(&revoSettings); }
/** * Perform an update of the @ref MagBias based on * Magnetometer Offset Cancellation: Theory and Implementation, * revisited William Premerlani, October 14, 2011 */ static void magOffsetEstimation(MagnetometerData *mag) { #if 0 RevoCalibrationData cal; RevoCalibrationGet(&cal); // Constants, to possibly go into a UAVO static const float MIN_NORM_DIFFERENCE = 50; static float B2[3] = {0, 0, 0}; MagBiasData magBias; MagBiasGet(&magBias); // Remove the current estimate of the bias mag->x -= magBias.x; mag->y -= magBias.y; mag->z -= magBias.z; // First call if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { B2[0] = mag->x; B2[1] = mag->y; B2[2] = mag->z; return; } float B1[3] = {mag->x, mag->y, mag->z}; float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2)); if (norm_diff > MIN_NORM_DIFFERENCE) { float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; magBias.x += b_error[0]; magBias.y += b_error[1]; magBias.z += b_error[2]; MagBiasSet(&magBias); // Store this value to compare against next update B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; } #else HomeLocationData homeLocation; HomeLocationGet(&homeLocation); AttitudeActualData attitude; AttitudeActualGet(&attitude); MagBiasData magBias; MagBiasGet(&magBias); // Remove the current estimate of the bias mag->x -= magBias.x; mag->y -= magBias.y; mag->z -= magBias.z; const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]); const float Rz = homeLocation.Be[2]; const float rate = 0.01; float R[3][3]; float B_e[3]; float xy[2]; float delta[3]; // Get the rotation matrix Quaternion2R(&attitude.q1, R); // Rotate the mag into the NED frame B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; float cy = cosf(attitude.Yaw * DEG2RAD); float sy = sinf(attitude.Yaw * DEG2RAD); xy[0] = cy * B_e[0] + sy * B_e[1]; xy[1] = -sy * B_e[0] + cy * B_e[1]; float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]); if (xy_norm > 0) { delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); delta[2] = -rate * (Rz - B_e[2]); magBias.x += delta[0]; magBias.y += delta[1]; magBias.z += delta[2]; MagBiasSet(&magBias); } #endif }