diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index fc4c66276..61bf9191b 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,61 @@ void Quaternion2R(float q[4], float Rbe[3][3]) Rbe[2][2] = q0s - q1s - q2s + q3s; } + +// ** Find first row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the fuselage/roll vector xB ** +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 second row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the spanwise/pitch vector yB ** +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 third row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the vertical/yaw vector zB ** +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 cc32cf442..f64823a43 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -50,6 +50,21 @@ 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 first row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the fuselage/roll vector xB ** +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 second row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the spanwise/pitch vector yB ** +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 third row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the vertical/yaw vector zB ** +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/libraries/math/butterworth.c b/flight/libraries/math/butterworth.c new file mode 100644 index 000000000..4f5a5c57b --- /dev/null +++ b/flight/libraries/math/butterworth.c @@ -0,0 +1,100 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Butterworth low pass filter + * @{ + * + * @file butterworth.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Direct form two of a second order Butterworth low pass filter + * + * @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 "math.h" +#include "butterworth.h" + +/** + * Initialization function for coefficients of a second order Butterworth biquadratic filter in direct from 2. + * Note that b1 = 2 * b0 and b2 = b0 is use here and in the sequel. + * @param[in] ff Cut-off frequency ratio + * @param[out] filterPtr Pointer to filter coefficients + * @returns Nothing + */ +void InitButterWorthDF2Filter(const float ff, struct ButterWorthDF2Filter *filterPtr) +{ + const float ita = 1.0f / tanf(M_PI_F * ff); + const float b0 = 1.0f / (1.0f + M_SQRT2_F * ita + ita * ita); + const float a1 = 2.0f * b0 * (ita * ita - 1.0f); + const float a2 = -b0 * (1.0f - M_SQRT2_F * ita + ita * ita); + + filterPtr->b0 = b0; + filterPtr->a1 = a1; + filterPtr->a2 = a2; +} + + +/** + * Initialization function for intermediate values of a second order Butterworth biquadratic filter in direct from 2. + * Obtained by solving a linear equation system. + * @param[in] x0 Prescribed value + * @param[in] filterPtr Pointer to filter coefficients + * @param[out] wn1Ptr Pointer to first intermediate value + * @param[out] wn2Ptr Pointer to second intermediate value + * @returns Nothing + */ +void InitButterWorthDF2Values(const float x0, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr) +{ + const float b0 = filterPtr->b0; + const float a1 = filterPtr->a1; + const float a2 = filterPtr->a2; + + const float a11 = 2.0f + a1; + const float a12 = 1.0f + a2; + const float a21 = 2.0f + a1 * a1 + a2; + const float a22 = 1.0f + a1 * a2; + const float det = a11 * a22 - a12 * a21; + const float rhs1 = x0 / b0 - x0; + const float rhs2 = x0 / b0 - x0 + a1 * x0; + + *wn1Ptr = (a22 * rhs1 - a12 * rhs2) / det; + *wn2Ptr = (-a21 * rhs1 + a11 * rhs2) / det; +} + + +/** + * Second order Butterworth biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored. + * Function takes care of updating the values wn1 and wn2. + * @param[in] xn New raw value + * @param[in] filterPtr Pointer to filter coefficients + * @param[out] wn1Ptr Pointer to first intermediate value + * @param[out] wn2Ptr Pointer to second intermediate value + * @returns Filtered value + */ +float FilterButterWorthDF2(const float xn, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr) +{ + const float wn = xn + filterPtr->a1 * (*wn1Ptr) + filterPtr->a2 * (*wn2Ptr); + const float val = filterPtr->b0 * (wn + 2.0f * (*wn1Ptr) + (*wn2Ptr)); + + *wn2Ptr = *wn1Ptr; + *wn1Ptr = wn; + return val; +} diff --git a/flight/libraries/math/butterworth.h b/flight/libraries/math/butterworth.h new file mode 100644 index 000000000..126cd5ca4 --- /dev/null +++ b/flight/libraries/math/butterworth.h @@ -0,0 +1,46 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Butterworth low pass filter + * @{ + * + * @file butterworth.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Direct form two of a second order Butterworth low pass filter + * + * @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 BUTTERWORTH_H +#define BUTTERWORTH_H + +// Coefficients of second order Butterworth biquadratic filter in direct from 2 +struct ButterWorthDF2Filter { + float b0; + float a1; + float a2; +}; + +// Function declarations +void InitButterWorthDF2Filter(const float ff, struct ButterWorthDF2Filter *filterPtr); +void InitButterWorthDF2Values(const float x0, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr); +float FilterButterWorthDF2(const float xn, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr); + +#endif diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index f859b950f..d44682c98 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 "airspeedalarm.h" #include "taskinfo.h" @@ -99,7 +99,7 @@ int32_t AirspeedInitialize() HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesArrayGet(optionalModules); if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) { @@ -136,7 +136,7 @@ MODULE_INITCALL(AirspeedInitialize, AirspeedStart); static void airspeedTask(__attribute__((unused)) void *parameters) { AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); - bool gpsAirspeedInitialized = false; + bool imuAirspeedInitialized = false; AirspeedSensorData airspeedData; AirspeedSensorGet(&airspeedData); @@ -164,9 +164,9 @@ static void airspeedTask(__attribute__((unused)) void *parameters) AirspeedSensorSet(&airspeedData); break; case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION: - if (!gpsAirspeedInitialized) { - gpsAirspeedInitialized = true; - gps_airspeedInitialize(); + if (!imuAirspeedInitialized) { + imuAirspeedInitialized = true; + imu_airspeedInitialize(&airspeedSettings); } break; } @@ -192,7 +192,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: // no need to check so often until a sensor is enabled diff --git a/flight/modules/Airspeed/gps_airspeed.c b/flight/modules/Airspeed/gps_airspeed.c deleted file mode 100644 index 1a56f353d..000000000 --- a/flight/modules/Airspeed/gps_airspeed.c +++ /dev/null @@ -1,170 +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 "airspeedalarm.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 - -// 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 - -/* - * 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 *)pios_malloc(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|. - */ -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) < cosf(DEG2RAD(5.0f))) { - 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; - AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); - return; // do not calculate if gps velocity is insufficient... - } - - // Calculate the norm^2 of the difference between the two GPS vectors - float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f); - - // Calculate the norm^2 of the difference between the two fuselage vectors - float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f); - - // 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; - AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); - } 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; - AirspeedAlarm(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/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c new file mode 100644 index 000000000..9c749221a --- /dev/null +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -0,0 +1,306 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Use attitude and velocity data to estimate airspeed + * @{ + * + * @file imu_airspeed.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief IMU based airspeed calculation + * + * @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 "imu_airspeed.h" +#include "CoordinateConversions.h" +#include "butterworth.h" +#include + + +// Private constants +#define EPS 1e-6f +#define EPS_REORIENTATION 1e-10f +#define EPS_VELOCITY 1.f + +// Private types +// structure with smoothed fuselage orientation, ground speed, wind vector and their changes in time +struct IMUGlobals { + // Butterworth filters + struct ButterWorthDF2Filter filter; + struct ButterWorthDF2Filter prefilter; + float ff, ffV; + + // storage variables for Butterworth filter + float pn1, pn2; + float yn1, yn2; + float v1n1, v1n2; + float v2n1, v2n2; + float v3n1, v3n2; + float Vw1n1, Vw1n2; + float Vw2n1, Vw2n2; + float Vw3n1, Vw3n2; + float Vw1, Vw2, Vw3; + + // storage variables for derivative calculation + float pOld, yOld; + float v1Old, v2Old, v3Old; +}; + + +// Private variables +static struct IMUGlobals *imu; + +// 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; +} + +// ****** find pitch, yaw from quaternion ******** +static void Quaternion2PY(const float q0, const float q1, const float q2, const float q3, float *pPtr, float *yPtr, bool principalArg) +{ + float R13, R11, R12; + const float q0s = q0 * q0; + const float q1s = q1 * q1; + const float q2s = q2 * q2; + const float q3s = q3 * q3; + + R13 = 2.0f * (q1 * q3 - q0 * q2); + R11 = q0s + q1s - q2s - q3s; + R12 = 2.0f * (q1 * q2 + q0 * q3); + + *pPtr = asinf(-R13); // pitch always between -pi/2 to pi/2 + + const float y_ = atan2f(R12, R11); + // use old yaw contained in y to add multiples of 2pi to have a continuous yaw if user does not want the principal argument + // else simply copy atan2 result into result + if (principalArg) { + *yPtr = y_; + } else { + // calculate needed mutliples of 2pi to avoid jumps + // number of cycles accumulated in old yaw + const int32_t cycles = (int32_t)(*yPtr / M_2PI_F); + // look for a jump by substracting the modulus, i.e. there is maximally one jump. + // take slightly less than 2pi, because the jump will always be lower than 2pi + const int32_t mod = (int32_t)((y_ - (*yPtr - cycles * M_2PI_F)) / (M_2PI_F * 0.8f)); + *yPtr = y_ + M_2PI_F * (cycles - mod); + } +} + +static void PY2xB(const float p, const float y, float x[3]) +{ + const float cosp = cosf(p); + + x[0] = cosp * cosf(y); + x[1] = cosp * sinf(y); + x[2] = -sinf(p); +} + + +static void PY2DeltaxB(const float p, const float y, const float xB[3], float x[3]) +{ + const float cosp = cosf(p); + + x[0] = xB[0] - cosp * cosf(y); + x[1] = xB[1] - cosp * sinf(y); + x[2] = xB[2] - -sinf(p); +} + + +/* + * Initialize function loads first data sets, and allocates memory for structure. + */ +void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings) +{ + // pre-filter frequency rate + const float ff = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1; + // filter frequency rate + const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2; + + // This method saves memory in case we don't use the module. + imu = (struct IMUGlobals *)pios_malloc(sizeof(struct IMUGlobals)); + + // airspeed calculation variables + VelocityStateInitialize(); + VelocityStateData velData; + VelocityStateGet(&velData); + + AttitudeStateData attData; + AttitudeStateGet(&attData); + + // initialize filters for given ff and ffV + InitButterWorthDF2Filter(ffV, &(imu->filter)); + InitButterWorthDF2Filter(ff, &(imu->prefilter)); + imu->ffV = ffV; + imu->ff = ff; + + // get pitch and yaw from quarternion; principal argument for yaw + Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld), &(imu->yOld), true); + InitButterWorthDF2Values(imu->pOld, &(imu->prefilter), &(imu->pn1), &(imu->pn2)); + InitButterWorthDF2Values(imu->yOld, &(imu->prefilter), &(imu->yn1), &(imu->yn2)); + + // use current NED speed as vOld vector and as initial value for filter + imu->v1Old = velData.North; + imu->v2Old = velData.East; + imu->v3Old = velData.Down; + InitButterWorthDF2Values(imu->v1Old, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2)); + InitButterWorthDF2Values(imu->v2Old, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2)); + InitButterWorthDF2Values(imu->v3Old, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2)); + + // initial guess for windspeed is zero + imu->Vw3 = imu->Vw2 = imu->Vw1 = 0.0f; + InitButterWorthDF2Values(0.0f, &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2)); + imu->Vw3n1 = imu->Vw2n1 = imu->Vw1n1; + imu->Vw3n2 = imu->Vw2n2 = imu->Vw1n2; +} + +/* + * Calculate airspeed as a function of groundspeed and vehicle attitude. + * Adapted 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|. + * Adapted to: |V| = (V_gps_2-V_gps_1) dot (f2_-f_1) / |f_2 - f1|^2. + * + * See OP-1317 imu_wind_estimation.pdf for details on the adaptation + * Need a low pass filter to filter out spikes in non coordinated maneuvers + * A two step Butterworth second order filter is used. In the first step fuselage vector xB + * and ground speed vector Vel are filtered. The fuselage vector is filtered through its pitch + * and yaw to keep a unit length. After building the differenced dxB and dVel are produced and + * the airspeed calculated. The calculated airspeed is filtered again with a Butterworth filter + */ +void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsData *airspeedSettings) +{ + // pre-filter frequency rate + const float ff = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1; + // filter frequency rate + const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2; + + // check for a change in filter frequency rate. if yes, then actualize filter constants and intermediate values + if (fabsf(ffV - imu->ffV) > EPS) { + InitButterWorthDF2Filter(ffV, &(imu->filter)); + InitButterWorthDF2Values(imu->Vw1, &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2)); + InitButterWorthDF2Values(imu->Vw2, &(imu->filter), &(imu->Vw2n1), &(imu->Vw2n2)); + InitButterWorthDF2Values(imu->Vw3, &(imu->filter), &(imu->Vw3n1), &(imu->Vw3n2)); + } + if (fabsf(ff - imu->ff) > EPS) { + InitButterWorthDF2Filter(ff, &(imu->prefilter)); + InitButterWorthDF2Values(imu->pOld, &(imu->prefilter), &(imu->pn1), &(imu->pn2)); + InitButterWorthDF2Values(imu->yOld, &(imu->prefilter), &(imu->yn1), &(imu->yn2)); + InitButterWorthDF2Values(imu->v1Old, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2)); + InitButterWorthDF2Values(imu->v2Old, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2)); + InitButterWorthDF2Values(imu->v3Old, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2)); + } + + float normVel2; + float normDiffAttitude2; + float dvdtDotdfdt; + + float xB[3]; + // get values and conduct smoothing of ground speed and orientation independently of the calculation of airspeed + { // Scoping to save memory + AttitudeStateData attData; + AttitudeStateGet(&attData); + VelocityStateData velData; + VelocityStateGet(&velData); + float p = imu->pOld, y = imu->yOld; + float dxB[3]; + + // get pitch and roll Euler angles from quaternion + // do not calculate the principlal argument of yaw, i.e. use old yaw to add multiples of 2pi to have a continuous yaw + Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &p, &y, false); + + // filter pitch and roll Euler angles instead of fuselage vector to guarantee a unit length at all times + p = FilterButterWorthDF2(p, &(imu->prefilter), &(imu->pn1), &(imu->pn2)); + y = FilterButterWorthDF2(y, &(imu->prefilter), &(imu->yn1), &(imu->yn2)); + // transform pitch and yaw into fuselage vector xB and xBold + PY2xB(p, y, xB); + // calculate change in fuselage vector by substraction of old value + PY2DeltaxB(imu->pOld, imu->yOld, xB, dxB); + + // filter ground speed from VelocityState + const float fv1n = FilterButterWorthDF2(velData.North, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2)); + const float fv2n = FilterButterWorthDF2(velData.East, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2)); + const float fv3n = FilterButterWorthDF2(velData.Down, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2)); + + // calculate norm of ground speed + normVel2 = Sq(fv1n) + Sq(fv2n) + Sq(fv3n); + // calculate norm of orientation change + normDiffAttitude2 = Sq(dxB[0]) + Sq(dxB[1]) + Sq(dxB[2]); + // cauclate scalar product between groundspeed change and orientation change + dvdtDotdfdt = (fv1n - imu->v1Old) * dxB[0] + (fv2n - imu->v2Old) * dxB[1] + (fv3n - imu->v3Old) * dxB[2]; + + // actualise old values + imu->pOld = p; + imu->yOld = y; + imu->v1Old = fv1n; + imu->v2Old = fv2n; + imu->v3Old = fv3n; + } + + // Some reorientation needed to be able to calculate airspeed, calculate only for sufficient velocity + // a negative scalar product is a clear sign that we are not really able to calculate the airspeed + // NOTE: normVel2 check against EPS_VELOCITY might make problems during hovering maneuvers in fixed wings + if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY && dvdtDotdfdt > 0.f) { + // Airspeed modulus: |v| = dv/dt * dxB/dt / |dxB/dt|^2 + // airspeed is always REAL because normDiffAttitude2 > EPS_REORIENTATION > 0 and REAL dvdtDotdfdt + const float airspeed = dvdtDotdfdt / normDiffAttitude2; + + // groundspeed = airspeed + wind ---> wind = groundspeed - airspeed + const float wind[3] = { imu->v1Old - xB[0] * airspeed, + imu->v2Old - xB[1] * airspeed, + imu->v3Old - xB[2] * airspeed }; + // filter raw wind + imu->Vw1 = FilterButterWorthDF2(wind[0], &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2)); + imu->Vw2 = FilterButterWorthDF2(wind[1], &(imu->filter), &(imu->Vw2n1), &(imu->Vw2n2)); + imu->Vw3 = FilterButterWorthDF2(wind[2], &(imu->filter), &(imu->Vw3n1), &(imu->Vw3n2)); + } // else leave wind estimation unchanged + + { // Scoping to save memory + // airspeed = groundspeed - wind + const float Vair[3] = { + imu->v1Old - imu->Vw1, + imu->v2Old - imu->Vw2, + imu->v3Old - imu->Vw3 + }; + + // project airspeed into fuselage vector + airspeedData->CalibratedAirspeed = Vair[0] * xB[0] + Vair[1] * xB[1] + Vair[2] * xB[2]; + } + + airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED); +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/Airspeed/inc/gps_airspeed.h b/flight/modules/Airspeed/inc/imu_airspeed.h similarity index 79% rename from flight/modules/Airspeed/inc/gps_airspeed.h rename to flight/modules/Airspeed/inc/imu_airspeed.h index 4b5f08ad6..47544d74c 100644 --- a/flight/modules/Airspeed/inc/gps_airspeed.h +++ b/flight/modules/Airspeed/inc/imu_airspeed.h @@ -3,10 +3,10 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup AirspeedModule Airspeed Module - * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements + * @brief Calculate airspeed as a function of the difference between sequential ground velocity and attitude measurements * @{ * - * @file gps_airspeed.h + * @file imu_airspeed.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Airspeed module, reads temperature and pressure from BMP085 * @@ -28,13 +28,13 @@ * 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 +#ifndef IMU_AIRSPEED_H +#define IMU_AIRSPEED_H -void gps_airspeedInitialize(); -void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings); +void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings); +void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsData *airspeedSettings); -#endif // GPS_AIRSPEED_H +#endif // IMU_AIRSPEED_H /** * @} diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index dc843999d..781e852c5 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -40,6 +40,7 @@ #define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */ #define M_1_PI_F 0.31830988618379067153776752675f /* 1/pi */ #define M_2_PI_F 0.63661977236758134307553505349f /* 2/pi */ +#define M_2PI_F 6.28318530717958647692528676656f /* 2pi */ #define M_LN10_F 2.30258509299404568401799145468f /* ln(10) */ #define M_LN2_F 0.69314718055994530941723212146f /* ln(2) */ #define M_LNPI_F 1.14472988584940017414342735135f /* ln(pi) */ @@ -54,6 +55,7 @@ #define M_PI_D 3.14159265358979323846264338328d /* pi */ #define M_PI_2_D 1.57079632679489661923132169164d /* pi/2 */ #define M_PI_4_D 0.78539816339744830961566084582d /* pi/4 */ +#define M_2PI_D 6.28318530717958647692528676656d /* 2pi */ #define M_SQRTPI_D 1.77245385090551602729816748334d /* sqrt(pi) */ #define M_2_SQRTPI_D 1.12837916709551257389615890312d /* 2/sqrt(pi) */ #define M_1_PI_D 0.31830988618379067153776752675d /* 1/pi */ diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 2c89a81ef..72b2acb47 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -100,6 +100,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/pid.c SRC += $(MATHLIB)/mathmisc.c +SRC += $(MATHLIB)/butterworth.c SRC += $(PIOSCORECOMMON)/pios_task_monitor.c SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c diff --git a/make/apps-defs.mk b/make/apps-defs.mk index 72381423e..1544e4c10 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -109,6 +109,7 @@ SRC += $(FLIGHTLIB)/CoordinateConversions.c SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/pid.c SRC += $(MATHLIB)/mathmisc.c +SRC += $(MATHLIB)/butterworth.c SRC += $(FLIGHTLIB)/printf-stdarg.c ## Modules diff --git a/shared/uavobjectdefinition/airspeedsettings.xml b/shared/uavobjectdefinition/airspeedsettings.xml index 0c438afa5..8f52ac9fa 100644 --- a/shared/uavobjectdefinition/airspeedsettings.xml +++ b/shared/uavobjectdefinition/airspeedsettings.xml @@ -5,7 +5,8 @@ - + +