From 016ba6940db8a9533d8c35e1e889d28656cbcc52 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 19 Jul 2014 18:43:57 +0200 Subject: [PATCH] OP-1403 - drop attitude/revolution as it has been supersede by StateEstimation module --- flight/modules/Attitude/revolution/attitude.c | 1348 ----------------- .../Attitude/revolution/inc/attitude.h | 37 - 2 files changed, 1385 deletions(-) delete mode 100644 flight/modules/Attitude/revolution/attitude.c delete mode 100644 flight/modules/Attitude/revolution/inc/attitude.h diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c deleted file mode 100644 index 2ee8901e0..000000000 --- a/flight/modules/Attitude/revolution/attitude.c +++ /dev/null @@ -1,1348 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Attitude Copter Control Attitude Estimation - * @brief Acquires sensor data and computes attitude estimate - * Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects - * @{ - * - * @file attitude.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Module to handle all comms to the AHRS on a periodic basis. - * - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -/** - * Input objects: None, takes sensor data via pios - * Output objects: @ref AttitudeRaw @ref AttitudeState - * - * This module computes an attitude estimate from the sensor data - * - * The module executes in its own thread. - * - * UAVObjects are automatically generated by the UAVObjectGenerator from - * the object definition XML file. - * - * Modules have no API, all communication to other modules is done through UAVObjects. - * However modules may use the API exposed by shared libraries. - * See the OpenPilot wiki for more details. - * http://www.openpilot.org/OpenPilot_Application_Architecture - * - */ - -#include -#include -#include "attitude.h" -#include "accelsensor.h" -#include "accelstate.h" -#include "airspeedsensor.h" -#include "airspeedstate.h" -#include "attitudestate.h" -#include "attitudesettings.h" -#include "barosensor.h" -#include "flightstatus.h" -#include "gpspositionsensor.h" -#include "gpsvelocitysensor.h" -#include "gyrostate.h" -#include "gyrosensor.h" -#include "homelocation.h" -#include "magsensor.h" -#include "magstate.h" -#include "positionstate.h" -#include "ekfconfiguration.h" -#include "ekfstatevariance.h" -#include "revocalibration.h" -#include "revosettings.h" -#include "velocitystate.h" -#include "taskinfo.h" - -#include "CoordinateConversions.h" - -// Private constants -#define STACK_SIZE_BYTES 2048 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -#define FAILSAFE_TIMEOUT_MS 10 - -#define CALIBRATION_DELAY 4000 -#define CALIBRATION_DURATION 6000 -// low pass filter configuration to calculate offset -// of barometric altitude sensor -// reasoning: updates at: 10 Hz, tau= 300 s settle time -// exp(-(1/f) / tau ) ~=~ 0.9997 -#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f - -// simple IAS to TAS aproximation - 2% increase per 1000ft -// since we do not have flowing air temperature information -#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f)) - -// Private types - -// Private variables -static xTaskHandle attitudeTaskHandle; - -static xQueueHandle gyroQueue; -static xQueueHandle accelQueue; -static xQueueHandle magQueue; -static xQueueHandle airspeedQueue; -static xQueueHandle baroQueue; -static xQueueHandle gpsQueue; -static xQueueHandle gpsVelQueue; - -static AttitudeSettingsData attitudeSettings; -static HomeLocationData homeLocation; -static RevoCalibrationData revoCalibration; -static EKFConfigurationData ekfConfiguration; -static RevoSettingsData revoSettings; -static FlightStatusData flightStatus; -const uint32_t SENSOR_QUEUE_SIZE = 10; - -static bool volatile variance_error = true; -static bool volatile initialization_required = true; -static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running -static float rollPitchBiasRate = 0; - -// Accel filtering -static float accel_alpha = 0; -static bool accel_filter_enabled = false; -static float accels_filtered[3]; -static float grot_filtered[3]; - -// Private functions -static void AttitudeTask(void *parameters); - -static int32_t updateAttitudeComplementary(bool first_run); -static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode); -static void settingsUpdatedCb(UAVObjEvent *objEv); - -static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED); - -static void magOffsetEstimation(MagSensorData *mag); - -// check for invalid values -static inline bool invalid(float data) -{ - if (isnan(data) || isinf(data)) { - return true; - } - return false; -} - -// check for invalid variance values -static inline bool invalid_var(float data) -{ - if (invalid(data)) { - return true; - } - if (data < 1e-15f) { // var should not be close to zero. And not negative either. - return true; - } - return false; -} - -/** - * API for sensor fusion algorithms: - * Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro) - * Stores all the queues the algorithm will pull data from - * FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias) - * Update() -- queries queues and updates the attitude estiamte - */ - - -/** - * Initialise the module. Called before the start function - * \returns 0 on success or -1 if initialisation failed - */ -int32_t AttitudeInitialize(void) -{ - GyroSensorInitialize(); - GyroStateInitialize(); - AccelSensorInitialize(); - AccelStateInitialize(); - MagSensorInitialize(); - MagStateInitialize(); - AirspeedSensorInitialize(); - AirspeedStateInitialize(); - BaroSensorInitialize(); - GPSPositionSensorInitialize(); - GPSVelocitySensorInitialize(); - AttitudeSettingsInitialize(); - AttitudeStateInitialize(); - PositionStateInitialize(); - VelocityStateInitialize(); - RevoSettingsInitialize(); - RevoCalibrationInitialize(); - EKFConfigurationInitialize(); - EKFStateVarianceInitialize(); - FlightStatusInitialize(); - - // Initialize this here while we aren't setting the homelocation in GPS - HomeLocationInitialize(); - - // Initialize quaternion - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - attitude.q1 = 1.0f; - attitude.q2 = 0.0f; - attitude.q3 = 0.0f; - attitude.q4 = 0.0f; - AttitudeStateSet(&attitude); - - AttitudeSettingsConnectCallback(&settingsUpdatedCb); - RevoSettingsConnectCallback(&settingsUpdatedCb); - RevoCalibrationConnectCallback(&settingsUpdatedCb); - HomeLocationConnectCallback(&settingsUpdatedCb); - EKFConfigurationConnectCallback(&settingsUpdatedCb); - FlightStatusConnectCallback(&settingsUpdatedCb); - - return 0; -} - -/** - * Start the task. Expects all objects to be initialized by this point. - * \returns 0 on success or -1 if initialisation failed - */ -int32_t AttitudeStart(void) -{ - // Create the queues for the sensors - gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - accelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - - // Start main task - xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &attitudeTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); -#endif - - GyroSensorConnectQueue(gyroQueue); - AccelSensorConnectQueue(accelQueue); - MagSensorConnectQueue(magQueue); - AirspeedSensorConnectQueue(airspeedQueue); - BaroSensorConnectQueue(baroQueue); - GPSPositionSensorConnectQueue(gpsQueue); - GPSVelocitySensorConnectQueue(gpsVelQueue); - - return 0; -} - -MODULE_INITCALL(AttitudeInitialize, AttitudeStart); - -/** - * Module thread, should not return. - */ -static void AttitudeTask(__attribute__((unused)) void *parameters) -{ - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - - // Force settings update to make sure rotation loaded - settingsUpdatedCb(NULL); - - // Wait for all the sensors be to read - vTaskDelay(100); - - // Main task loop - TODO: make it run as delayed callback - while (1) { - int32_t ret_val = -1; - - bool first_run = false; - if (initialization_required) { - initialization_required = false; - first_run = true; - } - - // This function blocks on data queue - switch (running_algorithm) { - case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: - ret_val = updateAttitudeComplementary(first_run); - break; - case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR: - ret_val = updateAttitudeINSGPS(first_run, true); - break; - case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: - ret_val = updateAttitudeINSGPS(first_run, false); - break; - default: - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - break; - } - - if (ret_val != 0) { - initialization_required = true; - } - -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); -#endif - } -} - -static inline void apply_accel_filter(const float *raw, float *filtered) -{ - if (accel_filter_enabled) { - filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); - filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); - filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); - } else { - filtered[0] = raw[0]; - filtered[1] = raw[1]; - filtered[2] = raw[2]; - } -} - -float accel_mag; -float qmag; -float attitudeDt; -float mag_err[3]; - -static int32_t updateAttitudeComplementary(bool first_run) -{ - UAVObjEvent ev; - GyroSensorData gyroSensorData; - GyroStateData gyroStateData; - AccelSensorData accelSensorData; - static int32_t timeval; - float dT; - static uint8_t init = 0; - static float gyro_bias[3] = { 0, 0, 0 }; - static bool magCalibrated = true; - static uint32_t initStartupTime = 0; - - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe - if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || - xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) { - // When one of these is updated so should the other - // Do not set attitude timeout warnings in simulation mode - if (!AttitudeStateReadOnly()) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - return -1; - } - } - - AccelSensorGet(&accelSensorData); - -// TODO: put in separate filter - AccelStateData accelState; - accelState.x = accelSensorData.x; - accelState.y = accelSensorData.y; - accelState.z = accelSensorData.z; - AccelStateSet(&accelState); - - // During initialization and - if (first_run) { -#if defined(PIOS_INCLUDE_HMC5X83) - // To initialize we need a valid mag reading - if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) { - return -1; - } - MagSensorData magData; - MagSensorGet(&magData); -#else - MagSensorData magData; - magData.x = 100.0f; - magData.y = 0.0f; - magData.z = 0.0f; -#endif - float magBias[3]; - RevoCalibrationmag_biasArrayGet(magBias); - // don't trust Mag for initial orientation if it has not been calibrated - if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) { - magCalibrated = false; - magData.x = 100.0f; - magData.y = 0.0f; - magData.z = 0.0f; - } - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - init = 0; - - // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, - // so pseudo "north" vector can be estimated even if the board is not level - attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z); - float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y; - float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z; - - // rotate accels z vector according to roll - float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y; - attitudeState.Pitch = atan2f(accelSensorData.x, -azn); - - float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn; - - attitudeState.Yaw = atan2f(-yn, xn); - // TODO: This is still a hack - // Put this in a proper generic function in CoordinateConversion.c - // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) - // should calculate the rotation in 3d space using proper cross product math - // SUBTODO: formulate the math required - - attitudeState.Roll = RAD2DEG(attitudeState.Roll); - attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); - attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - - RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); - AttitudeStateSet(&attitudeState); - - timeval = PIOS_DELAY_GetRaw(); - - // wait calibration_delay only at powerup - if (xTaskGetTickCount() < 3000) { - initStartupTime = 0; - } else { - initStartupTime = xTaskGetTickCount() - CALIBRATION_DELAY; - } - - // Zero gyro bias - // This is really needed after updating calibration settings. - gyro_bias[0] = 0.0f; - gyro_bias[1] = 0.0f; - gyro_bias[2] = 0.0f; - return 0; - } - - if ((xTaskGetTickCount() - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY) && - (xTaskGetTickCount() - initStartupTime > CALIBRATION_DELAY)) { - // For first CALIBRATION_DURATION seconds after CALIBRATION_DELAY from startup - // Zero gyro bias assuming it is steady, smoothing the gyro input value applying rollPitchBiasRate. - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.0f; - attitudeSettings.YawBiasRate = 0.23f; - accel_filter_enabled = false; - rollPitchBiasRate = 0.01f; - attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f; - init = 0; - } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.0f; - attitudeSettings.YawBiasRate = 0.23f; - accel_filter_enabled = false; - rollPitchBiasRate = 0.01f; - attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f; - init = 0; - } else if (init == 0) { - // Reload settings (all the rates) - AttitudeSettingsGet(&attitudeSettings); - rollPitchBiasRate = 0.0f; - if (accel_alpha > 0.0f) { - accel_filter_enabled = true; - } - init = 1; - } - - GyroSensorGet(&gyroSensorData); - gyroStateData.x = gyroSensorData.x; - gyroStateData.y = gyroSensorData.y; - gyroStateData.z = gyroSensorData.z; - - // Compute the dT using the cpu clock - dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; - timeval = PIOS_DELAY_GetRaw(); - - float q[4]; - - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - - float grot[3]; - float accel_err[3]; - - // Get the current attitude estimate - quat_copy(&attitudeState.q1, q); - - // Apply smoothing to accel values, to reduce vibration noise before main calculations. - apply_accel_filter((const float *)&accelSensorData.x, accels_filtered); - - // Rotate gravity to body frame and cross with accels - grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); - grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - - apply_accel_filter(grot, grot_filtered); - - CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); - - // Account for accel magnitude - accel_mag = accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]; - accel_mag = sqrtf(accel_mag); - - float grot_mag; - if (accel_filter_enabled) { - grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); - } else { - grot_mag = 1.0f; - } - - // TODO! check grot_mag & accel vector magnitude values for correctness. - - accel_err[0] /= (accel_mag * grot_mag); - accel_err[1] /= (accel_mag * grot_mag); - accel_err[2] /= (accel_mag * grot_mag); - - - if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) { - // Rotate gravity to body frame and cross with accels - float brot[3]; - float Rbe[3][3]; - MagSensorData mag; - - Quaternion2R(q, Rbe); - MagSensorGet(&mag); - -// TODO: separate filter! - if (revoCalibration.MagBiasNullingRate > 0) { - magOffsetEstimation(&mag); - } - MagStateData mags; - mags.x = mag.x; - mags.y = mag.y; - mags.z = mag.z; - MagStateSet(&mags); - - // If the mag is producing bad data don't use it (normally bad calibration) - if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) { - rot_mult(Rbe, homeLocation.Be, brot); - - float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); - mag.x /= mag_len; - mag.y /= mag_len; - mag.z /= mag_len; - - float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); - brot[0] /= bmag; - brot[1] /= bmag; - brot[2] /= bmag; - - // Only compute if neither vector is null - if (bmag < 1.0f || mag_len < 1.0f) { - mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; - } else { - CrossProduct((const float *)&mag.x, (const float *)brot, mag_err); - } - } - } else { - mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; - } - - // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - // Correct rates based on integral coefficient - gyroStateData.x -= gyro_bias[0]; - gyroStateData.y -= gyro_bias[1]; - gyroStateData.z -= gyro_bias[2]; - - gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi - (gyroStateData.x) * rollPitchBiasRate; - gyro_bias[1] -= accel_err[1] * attitudeSettings.AccelKi - (gyroStateData.y) * rollPitchBiasRate; - gyro_bias[2] -= -mag_err[2] * attitudeSettings.MagKi - (gyroStateData.z) * rollPitchBiasRate; - - // save gyroscope state - GyroStateSet(&gyroStateData); - - // Correct rates based on proportional coefficient - gyroStateData.x += accel_err[0] * attitudeSettings.AccelKp / dT; - gyroStateData.y += accel_err[1] * attitudeSettings.AccelKp / dT; - gyroStateData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * attitudeSettings.MagKp / dT; - - // Work out time derivative from INSAlgo writeup - // Also accounts for the fact that gyros are in deg/s - float qdot[4]; - qdot[0] = DEG2RAD(-q[1] * gyroStateData.x - q[2] * gyroStateData.y - q[3] * gyroStateData.z) * dT / 2; - qdot[1] = DEG2RAD(q[0] * gyroStateData.x - q[3] * gyroStateData.y + q[2] * gyroStateData.z) * dT / 2; - qdot[2] = DEG2RAD(q[3] * gyroStateData.x + q[0] * gyroStateData.y - q[1] * gyroStateData.z) * dT / 2; - qdot[3] = DEG2RAD(-q[2] * gyroStateData.x + q[1] * gyroStateData.y + q[0] * gyroStateData.z) * dT / 2; - - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; - - if (q[0] < 0.0f) { - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } - - // Renomalize - qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; - - // If quaternion has become inappropriately short or is nan reinit. - // THIS SHOULD NEVER ACTUALLY HAPPEN - if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { - q[0] = 1.0f; - q[1] = 0.0f; - q[2] = 0.0f; - q[3] = 0.0f; - } - - quat_copy(q, &attitudeState.q1); - - // Convert into eueler degrees (makes assumptions about RPY order) - Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll); - - AttitudeStateSet(&attitudeState); - - // Flush these queues for avoid errors - xQueueReceive(baroQueue, &ev, 0); - if (xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE) { - float NED[3]; - // Transform the GPS position into NED coordinates - GPSPositionSensorData gpsPosition; - GPSPositionSensorGet(&gpsPosition); - getNED(&gpsPosition, NED); - - PositionStateData positionState; - PositionStateGet(&positionState); - positionState.North = NED[0]; - positionState.East = NED[1]; - positionState.Down = NED[2]; - PositionStateSet(&positionState); - } - - if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) { - // Transform the GPS position into NED coordinates - GPSVelocitySensorData gpsVelocity; - GPSVelocitySensorGet(&gpsVelocity); - - VelocityStateData velocityState; - VelocityStateGet(&velocityState); - velocityState.North = gpsVelocity.North; - velocityState.East = gpsVelocity.East; - velocityState.Down = gpsVelocity.Down; - VelocityStateSet(&velocityState); - } - - if (xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE) { - // Calculate true airspeed from indicated airspeed - AirspeedSensorData airspeedSensor; - AirspeedSensorGet(&airspeedSensor); - - AirspeedStateData airspeed; - AirspeedStateGet(&airspeed); - - PositionStateData positionState; - PositionStateGet(&positionState); - - if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { - // we have airspeed available - airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; - airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed; - AirspeedStateSet(&airspeed); - } - } - - if (!init && flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else if (variance_error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - - - return 0; -} - -#include "insgps.h" -int32_t ins_failed = 0; -extern struct NavStruct Nav; -int32_t init_stage = 0; - -/** - * @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS) - * @params[in] first_run This is the first run so trigger reinitialization - * @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0) - * @return 0 for success, -1 for failure - */ -static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) -{ - UAVObjEvent ev; - GyroSensorData gyroSensorData; - AccelSensorData accelSensorData; - MagStateData magData; - AirspeedSensorData airspeedData; - BaroSensorData baroData; - GPSPositionSensorData gpsData; - GPSVelocitySensorData gpsVelData; - - static bool mag_updated = false; - static bool baro_updated; - static bool airspeed_updated; - static bool gps_updated; - static bool gps_vel_updated; - - static bool value_error = false; - - static float baroOffset = 0.0f; - - static uint32_t ins_last_time = 0; - static bool inited; - - float NED[3] = { 0.0f, 0.0f, 0.0f }; - float vel[3] = { 0.0f, 0.0f, 0.0f }; - float zeros[3] = { 0.0f, 0.0f, 0.0f }; - - // Perform the update - uint16_t sensors = 0; - float dT; - - // Wait until the gyro and accel object is updated, if a timeout then go to failsafe - if ((xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || - (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE)) { - // Do not set attitude timeout warnings in simulation mode - if (!AttitudeStateReadOnly()) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - return -1; - } - } - - if (inited) { - mag_updated = 0; - baro_updated = 0; - airspeed_updated = 0; - gps_updated = 0; - gps_vel_updated = 0; - } - - if (first_run) { - inited = false; - init_stage = 0; - - mag_updated = 0; - baro_updated = 0; - airspeed_updated = 0; - gps_updated = 0; - gps_vel_updated = 0; - - ins_last_time = PIOS_DELAY_GetRaw(); - - return 0; - } - - mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); - baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; - airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; - - // Check if we are running simulation - if (!GPSPositionSensorReadOnly()) { - gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; - } else { - gps_updated |= pdTRUE && outdoor_mode; - } - - if (!GPSVelocitySensorReadOnly()) { - gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; - } else { - gps_vel_updated |= pdTRUE && outdoor_mode; - } - - // Get most recent data - GyroSensorGet(&gyroSensorData); - AccelSensorGet(&accelSensorData); -// TODO: separate filter! - if (mag_updated) { - MagSensorData mags; - MagSensorGet(&mags); - if (revoCalibration.MagBiasNullingRate > 0) { - magOffsetEstimation(&mags); - } - magData.x = mags.x; - magData.y = mags.y; - magData.z = mags.z; - MagStateSet(&magData); - } else { - MagStateGet(&magData); - } - - BaroSensorGet(&baroData); - AirspeedSensorGet(&airspeedData); - GPSPositionSensorGet(&gpsData); - GPSVelocitySensorGet(&gpsVelData); - -// TODO: put in separate filter - AccelStateData accelState; - accelState.x = accelSensorData.x; - accelState.y = accelSensorData.y; - accelState.z = accelSensorData.z; - AccelStateSet(&accelState); - - - value_error = false; - // safety checks - if (invalid(gyroSensorData.x) || - invalid(gyroSensorData.y) || - invalid(gyroSensorData.z) || - invalid(accelSensorData.x) || - invalid(accelSensorData.y) || - invalid(accelSensorData.z)) { - // cannot run process update, raise error! - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - return 0; - } - - if (invalid(magData.x) || - invalid(magData.y) || - invalid(magData.z)) { - // magnetometers can be ignored for a while - mag_updated = false; - value_error = true; - } - - // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily - // switching between indoor and outdoor mode with Set = false) - if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) { - mag_updated = false; - value_error = true; - } - - if (invalid(baroData.Altitude)) { - baro_updated = false; - value_error = true; - } - - if (invalid(airspeedData.CalibratedAirspeed)) { - airspeed_updated = false; - value_error = true; - } - - if (invalid(gpsData.Altitude)) { - gps_updated = false; - value_error = true; - } - - if (invalid_var(ekfConfiguration.R.GPSPosNorth) || - invalid_var(ekfConfiguration.R.GPSPosEast) || - invalid_var(ekfConfiguration.R.GPSPosDown) || - invalid_var(ekfConfiguration.R.GPSVelNorth) || - invalid_var(ekfConfiguration.R.GPSVelEast) || - invalid_var(ekfConfiguration.R.GPSVelDown)) { - gps_updated = false; - value_error = true; - } - - if (invalid(gpsVelData.North) || - invalid(gpsVelData.East) || - invalid(gpsVelData.Down)) { - gps_vel_updated = false; - value_error = true; - } - - // Discard airspeed if sensor not connected - if (airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { - airspeed_updated = false; - } - - // Have a minimum requirement for gps usage - if ((gpsData.Satellites < 7) || - (gpsData.PDOP > 4.0f) || - (gpsData.Latitude == 0 && gpsData.Longitude == 0) || - (homeLocation.Set != HOMELOCATION_SET_TRUE)) { - gps_updated = false; - gps_vel_updated = false; - } - - if (!inited) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else if (value_error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else if (variance_error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else if (outdoor_mode && gpsData.Satellites < 7) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - - dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; - ins_last_time = PIOS_DELAY_GetRaw(); - - // This should only happen at start up or at mode switches - if (dT > 0.01f) { - dT = 0.01f; - } else if (dT <= 0.001f) { - dT = 0.001f; - } - - if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) { - // Don't initialize until all sensors are read - if (init_stage == 0) { - // Reset the INS algorithm - INSGPSInit(); - INSSetMagVar((float[3]) { ekfConfiguration.R.MagX, - ekfConfiguration.R.MagY, - ekfConfiguration.R.MagZ } - ); - INSSetAccelVar((float[3]) { ekfConfiguration.Q.AccelX, - ekfConfiguration.Q.AccelY, - ekfConfiguration.Q.AccelZ } - ); - INSSetGyroVar((float[3]) { ekfConfiguration.Q.GyroX, - ekfConfiguration.Q.GyroY, - ekfConfiguration.Q.GyroZ } - ); - INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.GyroDriftX, - ekfConfiguration.Q.GyroDriftY, - ekfConfiguration.Q.GyroDriftZ } - ); - INSSetBaroVar(ekfConfiguration.R.BaroZ); - - // Initialize the gyro bias - float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; - INSSetGyroBias(gyro_bias); - - float pos[3] = { 0.0f, 0.0f, 0.0f }; - - if (outdoor_mode) { - GPSPositionSensorData gpsPosition; - GPSPositionSensorGet(&gpsPosition); - - // Transform the GPS position into NED coordinates - getNED(&gpsPosition, pos); - - // Initialize barometric offset to current GPS NED coordinate - baroOffset = -pos[2] - baroData.Altitude; - } else { - // Initialize barometric offset to homelocation altitude - baroOffset = -baroData.Altitude; - pos[2] = -(baroData.Altitude + baroOffset); - } - - // xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); - // MagSensorGet(&magData); - - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - - // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, - // so pseudo "north" vector can be estimated even if the board is not level - attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z); - float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y; - float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z; - - // rotate accels z vector according to roll - float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y; - attitudeState.Pitch = atan2f(accelSensorData.x, -azn); - - float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn; - - attitudeState.Yaw = atan2f(-yn, xn); - // TODO: This is still a hack - // Put this in a proper generic function in CoordinateConversion.c - // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) - // should calculate the rotation in 3d space using proper cross product math - // SUBTODO: formulate the math required - - attitudeState.Roll = RAD2DEG(attitudeState.Roll); - attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); - attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - - RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); - AttitudeStateSet(&attitudeState); - - float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 }; - INSSetState(pos, zeros, q, zeros, zeros); - - INSResetP(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)); - } else { - // Run prediction a bit before any corrections - - // Because the sensor module remove the bias we need to add it - // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) }; - INSStatePrediction(gyros, &accelSensorData.x, dT); - - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - attitude.q1 = Nav.q[0]; - attitude.q2 = Nav.q[1]; - attitude.q3 = Nav.q[2]; - attitude.q4 = Nav.q[3]; - Quaternion2RPY(&attitude.q1, &attitude.Roll); - AttitudeStateSet(&attitude); - } - - init_stage++; - if (init_stage > 10) { - inited = true; - } - - return 0; - } - - if (!inited) { - return 0; - } - - // Because the sensor module remove the bias we need to add it - // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) }; - - // Advance the state estimate - INSStatePrediction(gyros, &accelSensorData.x, dT); - - // Copy the attitude into the UAVO - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - attitude.q1 = Nav.q[0]; - attitude.q2 = Nav.q[1]; - attitude.q3 = Nav.q[2]; - attitude.q4 = Nav.q[3]; - Quaternion2RPY(&attitude.q1, &attitude.Roll); - AttitudeStateSet(&attitude); - - // Advance the covariance estimate - INSCovariancePrediction(dT); - - if (mag_updated) { - sensors |= MAG_SENSORS; - } - - if (baro_updated) { - sensors |= BARO_SENSOR; - } - - INSSetMagNorth(homeLocation.Be); - - if (gps_updated && outdoor_mode) { - INSSetPosVelVar((float[3]) { ekfConfiguration.R.GPSPosNorth, - ekfConfiguration.R.GPSPosEast, - ekfConfiguration.R.GPSPosDown }, - (float[3]) { ekfConfiguration.R.GPSVelNorth, - ekfConfiguration.R.GPSVelEast, - ekfConfiguration.R.GPSVelDown } - ); - sensors |= POS_SENSORS; - - if (0) { // Old code to take horizontal velocity from GPS Position update - sensors |= HORIZ_SENSORS; - vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading)); - vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading)); - vel[2] = 0.0f; - } - // Transform the GPS position into NED coordinates - getNED(&gpsData, NED); - - // Track barometric altitude offset with a low pass filter - baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + - (1.0f - BARO_OFFSET_LOWPASS_ALPHA) - * (-NED[2] - baroData.Altitude); - } else if (!outdoor_mode) { - INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor }, - (float[3]) { ekfConfiguration.FakeR.FakeGPSVelIndoor, - ekfConfiguration.FakeR.FakeGPSVelIndoor, - ekfConfiguration.FakeR.FakeGPSVelIndoor } - ); - vel[0] = vel[1] = vel[2] = 0.0f; - NED[0] = NED[1] = 0.0f; - NED[2] = -(baroData.Altitude + baroOffset); - sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; - sensors |= POS_SENSORS | VERT_SENSORS; - } - - if (gps_vel_updated && outdoor_mode) { - sensors |= HORIZ_SENSORS | VERT_SENSORS; - vel[0] = gpsVelData.North; - vel[1] = gpsVelData.East; - vel[2] = gpsVelData.Down; - } - - // Copy the position into the UAVO - PositionStateData positionState; - PositionStateGet(&positionState); - positionState.North = Nav.Pos[0]; - positionState.East = Nav.Pos[1]; - positionState.Down = Nav.Pos[2]; - PositionStateSet(&positionState); - - // airspeed correction needs current positionState - if (airspeed_updated) { - // we have airspeed available - AirspeedStateData airspeed; - AirspeedStateGet(&airspeed); - - airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; - airspeed.TrueAirspeed = (airspeedData.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedData.TrueAirspeed; - - AirspeedStateSet(&airspeed); - - if (!gps_vel_updated && !gps_updated) { - // feed airspeed into EKF, treat wind as 1e2 variance - sensors |= HORIZ_SENSORS | VERT_SENSORS; - INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor }, - (float[3]) { ekfConfiguration.FakeR.FakeGPSVelAirspeed, - ekfConfiguration.FakeR.FakeGPSVelAirspeed, - ekfConfiguration.FakeR.FakeGPSVelAirspeed } - ); - // rotate airspeed vector into NED frame - airspeed is measured in X axis only - float R[3][3]; - Quaternion2R(Nav.q, R); - float vtas[3] = { airspeed.TrueAirspeed, 0.0f, 0.0f }; - rot_mult(R, vtas, vel); - } - } - - /* - * TODO: Need to add a general sanity check for all the inputs to make sure their kosher - * although probably should occur within INS itself - */ - if (sensors) { - INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors); - } - - // Copy the velocity into the UAVO - VelocityStateData velocityState; - VelocityStateGet(&velocityState); - velocityState.North = Nav.Vel[0]; - velocityState.East = Nav.Vel[1]; - velocityState.Down = Nav.Vel[2]; - VelocityStateSet(&velocityState); - - GyroStateData gyroState; - gyroState.x = RAD2DEG(gyros[0] - RAD2DEG(Nav.gyro_bias[0])); - gyroState.y = RAD2DEG(gyros[1] - RAD2DEG(Nav.gyro_bias[1])); - gyroState.z = RAD2DEG(gyros[2] - RAD2DEG(Nav.gyro_bias[2])); - GyroStateSet(&gyroState); - - EKFStateVarianceData vardata; - EKFStateVarianceGet(&vardata); - INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)); - EKFStateVarianceSet(&vardata); - - return 0; -} - -/** - * @brief Convert the GPS LLA position into NED coordinates - * @note this method uses a taylor expansion around the home coordinates - * to convert to NED which allows it to be done with all floating - * calculations - * @param[in] Current GPS coordinates - * @param[out] NED frame coordinates - * @returns 0 for success, -1 for failure - */ -float T[3]; -static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED) -{ - float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), - DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), - (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) }; - - NED[0] = T[0] * dL[0]; - NED[1] = T[1] * dL[1]; - NED[2] = T[2] * dL[2]; - - return 0; -} - -static void settingsUpdatedCb(UAVObjEvent *ev) -{ - if (ev == NULL || ev->obj == FlightStatusHandle()) { - FlightStatusGet(&flightStatus); - } - if (ev == NULL || ev->obj == RevoCalibrationHandle()) { - RevoCalibrationGet(&revoCalibration); - } - // change of these settings require reinitialization of the EKF - // when an error flag has been risen, we also listen to flightStatus updates, - // since we are waiting for the system to get disarmed so we can reinitialize safely. - if (ev == NULL || - ev->obj == EKFConfigurationHandle() || - ev->obj == RevoSettingsHandle() || - (variance_error == true && ev->obj == FlightStatusHandle()) - ) { - bool error = false; - - EKFConfigurationGet(&ekfConfiguration); - int t; - for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { - if (invalid_var(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)[t])) { - error = true; - } - } - for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { - if (invalid_var(cast_struct_to_array(ekfConfiguration.Q, ekfConfiguration.Q.AccelX)[t])) { - error = true; - } - } - for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { - if (invalid_var(cast_struct_to_array(ekfConfiguration.R, ekfConfiguration.R.BaroZ)[t])) { - error = true; - } - } - - RevoSettingsGet(&revoSettings); - - // Reinitialization of the EKF is not desired during flight. - // It will be delayed until the board is disarmed by raising the error flag. - // We will not prevent the initial initialization though, since the board could be in always armed mode. - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required) { - error = true; - } - - if (error) { - variance_error = true; - } else { - // trigger reinitialization - possibly with new algorithm - running_algorithm = revoSettings.FusionAlgorithm; - variance_error = false; - initialization_required = true; - } - } - if (ev == NULL || ev->obj == HomeLocationHandle()) { - HomeLocationGet(&homeLocation); - // Compute matrix to convert deltaLLA to NED - float lat, alt; - lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); - alt = homeLocation.Altitude; - - T[0] = alt + 6.378137E6f; - T[1] = cosf(lat) * (alt + 6.378137E6f); - T[2] = -1.0f; - - // TODO: convert positionState to new reference frame and gracefully update EKF state! - // needed for long range flights where the reference coordinate is adjusted in flight - } - if (ev == NULL || ev->obj == AttitudeSettingsHandle()) { - AttitudeSettingsGet(&attitudeSettings); - - // Calculate accel filter alpha, in the same way as for gyro data in stabilization module. - const float fakeDt = 0.0015f; - if (attitudeSettings.AccelTau < 0.0001f) { - accel_alpha = 0; // not trusting this to resolve to 0 - accel_filter_enabled = false; - } else { - accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); - accel_filter_enabled = true; - } - } -} - -/** - * Perform an update of the @ref MagBias based on - * Magmeter Offset Cancellation: Theory and Implementation, - * revisited William Premerlani, October 14, 2011 - */ -static void magOffsetEstimation(MagSensorData *mag) -{ - #if 0 - // 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 // if 0 - static float magBias[3] = { 0 }; - - // Remove the current estimate of the bias - mag->x -= magBias[0]; - mag->y -= magBias[1]; - mag->z -= magBias[2]; - - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - - 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 = revoCalibration.MagBiasNullingRate; - float Rot[3][3]; - float B_e[3]; - float xy[2]; - float delta[3]; - - // Get the rotation matrix - Quaternion2R(&attitude.q1, Rot); - - // Rotate the mag into the NED frame - B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z; - B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z; - B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z; - - float cy = cosf(DEG2RAD(attitude.Yaw)); - float sy = sinf(DEG2RAD(attitude.Yaw)); - - 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]); - - 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]); - - if (!isnan(delta[0]) && !isinf(delta[0]) && - !isnan(delta[1]) && !isinf(delta[1]) && - !isnan(delta[2]) && !isinf(delta[2])) { - magBias[0] += delta[0]; - magBias[1] += delta[1]; - magBias[2] += delta[2]; - } - #endif // if 0 -} - - -/** - * @} - * @} - */ diff --git a/flight/modules/Attitude/revolution/inc/attitude.h b/flight/modules/Attitude/revolution/inc/attitude.h deleted file mode 100644 index 64c556a8e..000000000 --- a/flight/modules/Attitude/revolution/inc/attitude.h +++ /dev/null @@ -1,37 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Attitude Attitude Module - * @{ - * - * @file attitude.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. - * @brief Acquires sensor data and fuses it into attitude estimate for CC - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ -#ifndef ATTITUDE_H -#define ATTITUDE_H - -#include "openpilot.h" - -int32_t AttitudeInitialize(void); - -#endif // ATTITUDE_H