diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index fc4c66276..c3a4f2382 100644 --- a/flight/libraries/CoordinateConversions.c +++ b/flight/libraries/CoordinateConversions.c @@ -180,7 +180,7 @@ void RPY2Quaternion(const float rpy[3], float q[4]) // ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void Quaternion2R(float q[4], float Rbe[3][3]) { - float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; + const float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; Rbe[0][0] = q0s + q1s - q2s - q3s; Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); @@ -193,6 +193,58 @@ void Quaternion2R(float q[4], float Rbe[3][3]) Rbe[2][2] = q0s - q1s - q2s + q3s; } + +// ** Find x of body frame from quaternion ** +void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3]) +{ + const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3; + + x[0] = q0s + q1s - q2s - q3s; + x[1] = 2 * (q1 * q2 + q0 * q3); + x[2] = 2 * (q1 * q3 - q0 * q2); +} + + +void Quaternion2xB(const float q[4], float x[3]) +{ + QuaternionC2xB(q[0], q[1], q[2], q[3], x); +} + + +// ** Find y of body frame from quaternion ** +void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3]) +{ + const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3; + + y[0] = 2 * (q1 * q2 - q0 * q3); + y[1] = q0s - q1s + q2s - q3s; + y[2] = 2 * (q2 * q3 + q0 * q1); +} + + +void Quaternion2yB(const float q[4], float y[3]) +{ + QuaternionC2yB(q[0], q[1], q[2], q[3], y); +} + + +// ** Find z of body frame from quaternion ** +void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]) +{ + const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3; + + z[0] = 2 * (q1 * q3 + q0 * q2); + z[1] = 2 * (q2 * q3 - q0 * q1); + z[2] = q0s - q1s - q2s + q3s; +} + + +void Quaternion2zB(const float q[4], float z[3]) +{ + QuaternionC2zB(q[0], q[1], q[2], q[3], z); +} + + // ****** Express LLA in a local NED Base Frame ******** void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]) { diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index 8c5f55858..a2136c73d 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -50,6 +50,18 @@ void RPY2Quaternion(const float rpy[3], float q[4]); // ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void Quaternion2R(float q[4], float Rbe[3][3]); +// ** Find x of body frame from quaternion ** +void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3]); +void Quaternion2xB(const float q[4], float x[3]); + +// ** Find y of body frame from quaternion ** +void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3]); +void Quaternion2yB(const float q[4], float y[3]); + +// ** Find x of body frame from quaternion ** +void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]); +void Quaternion2zB(const float q[4], float z[3]); + // ****** Express LLA in a local NED Base Frame ******** void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]); diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index 8f368bef7..456131a90 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -44,7 +44,7 @@ #include "baro_airspeed_ms4525do.h" #include "baro_airspeed_etasv3.h" #include "baro_airspeed_mpxv.h" -#include "gps_airspeed.h" +#include "imu_airspeed.h" #include "taskinfo.h" // Private constants @@ -143,7 +143,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters) AirspeedSettingsUpdatedCb(NULL); - gps_airspeedInitialize(); + imu_airspeedInitialize(); airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; @@ -183,7 +183,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters) break; #endif case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION: - gps_airspeedGet(&airspeedData, &airspeedSettings); + imu_airspeedGet(&airspeedData, &airspeedSettings); break; case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE: default: diff --git a/flight/modules/Airspeed/gps_airspeed.c b/flight/modules/Airspeed/gps_airspeed.c deleted file mode 100644 index 5486f8161..000000000 --- a/flight/modules/Airspeed/gps_airspeed.c +++ /dev/null @@ -1,185 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup AirspeedModule Airspeed Module - * @brief Use GPS data to estimate airspeed - * @{ - * - * @file gps_airspeed.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Airspeed module, handles temperature and pressure readings from BMP085 - * - * @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 - */ - - -#include "openpilot.h" -#include "velocitystate.h" -#include "attitudestate.h" -#include "airspeedsensor.h" -#include "airspeedsettings.h" -#include "gps_airspeed.h" -#include "CoordinateConversions.h" -#include - - -// Private constants -#define GPS_AIRSPEED_BIAS_KP 0.1f // Needs to be settable in a UAVO -#define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO -#define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO -#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO -#define COSINE_OF_5_DEG 0.9961947f - -// Private types -struct GPSGlobals { - float RbeCol1_old[3]; - float gpsVelOld_N; - float gpsVelOld_E; - float gpsVelOld_D; - float oldAirspeed; -}; - -// Private variables -static struct GPSGlobals *gps; - -// Private functions -// a simple square inline function based on multiplication faster than powf(x,2.0f) -static inline float Sq(float x) -{ - return x * x; -} - - -/* - * Initialize function loads first data sets, and allocates memory for structure. - */ -void gps_airspeedInitialize() -{ - // This method saves memory in case we don't use the GPS module. - gps = (struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals)); - - // GPS airspeed calculation variables - VelocityStateInitialize(); - VelocityStateData gpsVelData; - VelocityStateGet(&gpsVelData); - - gps->gpsVelOld_N = gpsVelData.North; - gps->gpsVelOld_E = gpsVelData.East; - gps->gpsVelOld_D = gpsVelData.Down; - - gps->oldAirspeed = 0.0f; - - AttitudeStateData attData; - AttitudeStateGet(&attData); - - float Rbe[3][3]; - float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; - - // Calculate rotation matrix - Quaternion2R(q, Rbe); - - gps->RbeCol1_old[0] = Rbe[0][0]; - gps->RbeCol1_old[1] = Rbe[0][1]; - gps->RbeCol1_old[2] = Rbe[0][2]; -} - -/* - * Calculate airspeed as a function of GPS groundspeed and vehicle attitude. - * From "IMU Wind Estimation (Theory)", by William Premerlani. - * The idea is that V_gps=V_air+V_wind. If we assume wind constant, => - * V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1. - * If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1), - * where "f" is the fuselage vector in earth coordinates. - * We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|. - */ -/* Remark regarding "IMU Wind Estimation": The approach includes errors when |V| is - * not constant, i.e. when the change in V_gps does not solely come from a reorientation - * this error depends strongly on the time scale considered. Is the time between t1 and t2 too - * small, "spikes" absorving unconsidred acceleration will arise - */ -void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) -{ - float Rbe[3][3]; - - { // Scoping to save memory. We really just need Rbe. - AttitudeStateData attData; - AttitudeStateGet(&attData); - - float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; - - // Calculate rotation matrix - Quaternion2R(q, Rbe); - } - - // Calculate the cos(angle) between the two fuselage basis vectors - float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]); - - // If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue. - if (fabsf(cosDiff) < COSINE_OF_5_DEG) { - VelocityStateData gpsVelData; - VelocityStateGet(&gpsVelData); - - if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) { - airspeedData->CalibratedAirspeed = 0; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); - return; // do not calculate if gps velocity is insufficient... - } - - // Calculate the norm^2 of the difference between the two GPS vectors - float normDiffGPS2 = Sq(gpsVelData.North - gps->gpsVelOld_N) + Sq(gpsVelData.East - gps->gpsVelOld_E) + Sq(gpsVelData.Down - gps->gpsVelOld_D); - - // Calculate the norm^2 of the difference between the two fuselage vectors - float normDiffAttitude2 = Sq(Rbe[0][0] - gps->RbeCol1_old[0]) + Sq(Rbe[0][1] - gps->RbeCol1_old[1]) + Sq(Rbe[0][2] - gps->RbeCol1_old[2]); - - // Airspeed magnitude is the ratio between the two difference norms - float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2); - if (!IS_REAL(airspeedData->CalibratedAirspeed)) { - airspeedData->CalibratedAirspeed = 0; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); - } else if (!IS_REAL(airspeed)) { - airspeedData->CalibratedAirspeed = 0; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); - } else { - // need a low pass filter to filter out spikes in non coordinated maneuvers - airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed; - gps->oldAirspeed = airspeedData->CalibratedAirspeed; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); - } - - // Save old variables for next pass - gps->gpsVelOld_N = gpsVelData.North; - gps->gpsVelOld_E = gpsVelData.East; - gps->gpsVelOld_D = gpsVelData.Down; - - gps->RbeCol1_old[0] = Rbe[0][0]; - gps->RbeCol1_old[1] = Rbe[0][1]; - gps->RbeCol1_old[2] = Rbe[0][2]; - } -} - - -/** - * @} - * @} - */ diff --git a/flight/modules/Airspeed/inc/gps_airspeed.h b/flight/modules/Airspeed/inc/gps_airspeed.h deleted file mode 100644 index 4b5f08ad6..000000000 --- a/flight/modules/Airspeed/inc/gps_airspeed.h +++ /dev/null @@ -1,42 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup AirspeedModule Airspeed Module - * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements - * @{ - * - * @file gps_airspeed.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Airspeed module, reads temperature and pressure from BMP085 - * - * @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 GPS_AIRSPEED_H -#define GPS_AIRSPEED_H - -void gps_airspeedInitialize(); -void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings); - -#endif // GPS_AIRSPEED_H - -/** - * @} - * @} - */ diff --git a/shared/uavobjectdefinition/airspeedsensor.xml b/shared/uavobjectdefinition/airspeedsensor.xml index f61cb63fc..cc9341a2d 100644 --- a/shared/uavobjectdefinition/airspeedsensor.xml +++ b/shared/uavobjectdefinition/airspeedsensor.xml @@ -7,6 +7,12 @@ + + + + + +