rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Merge branch 'andrecillo/OP-1317_IMU_wind_estimation' into next

Browse Source
This commit is contained in:
Corvus Corax 2014-08-09 10:44:33 +02:00
commit e3df461871
12 changed files with 543 additions and 186 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
flight/libraries/CoordinateConversions.c
View File

@ -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 ** // ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
void Quaternion2R(float q[4], float Rbe[3][3]) 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][0] = q0s + q1s - q2s - q3s;
Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); 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; 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 ******** // ****** Express LLA in a local NED Base Frame ********
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]) void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{ {

15
flight/libraries/inc/CoordinateConversions.h
View File

@ -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 ** // ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
void Quaternion2R(float q[4], float Rbe[3][3]); 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 ******** // ****** Express LLA in a local NED Base Frame ********
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]); void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]);

100
flight/libraries/math/butterworth.c Normal file
View File

@ -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;
}

46
flight/libraries/math/butterworth.h Normal file
View File

@ -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

14
flight/modules/Airspeed/airspeed.c
View File

@ -44,7 +44,7 @@
#include "baro_airspeed_ms4525do.h" #include "baro_airspeed_ms4525do.h"
#include "baro_airspeed_etasv3.h" #include "baro_airspeed_etasv3.h"
#include "baro_airspeed_mpxv.h" #include "baro_airspeed_mpxv.h"
#include "gps_airspeed.h" #include "imu_airspeed.h"
#include "airspeedalarm.h" #include "airspeedalarm.h"
#include "taskinfo.h" #include "taskinfo.h"
@ -99,7 +99,7 @@ int32_t AirspeedInitialize()
HwSettingsInitialize(); HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules); HwSettingsOptionalModulesArrayGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) { if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
@ -136,7 +136,7 @@ MODULE_INITCALL(AirspeedInitialize, AirspeedStart);
static void airspeedTask(__attribute__((unused)) void *parameters) static void airspeedTask(__attribute__((unused)) void *parameters)
{ {
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
bool gpsAirspeedInitialized = false; bool imuAirspeedInitialized = false;
AirspeedSensorData airspeedData; AirspeedSensorData airspeedData;
AirspeedSensorGet(&airspeedData); AirspeedSensorGet(&airspeedData);
@ -164,9 +164,9 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
AirspeedSensorSet(&airspeedData); AirspeedSensorSet(&airspeedData);
break; break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION: case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
if (!gpsAirspeedInitialized) { if (!imuAirspeedInitialized) {
gpsAirspeedInitialized = true; imuAirspeedInitialized = true;
gps_airspeedInitialize(); imu_airspeedInitialize(&airspeedSettings);
} }
break; break;
} }
@ -192,7 +192,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
break; break;
#endif #endif
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION: case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
gps_airspeedGet(&airspeedData, &airspeedSettings); imu_airspeedGet(&airspeedData, &airspeedSettings);
break; break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE: case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
// no need to check so often until a sensor is enabled // no need to check so often until a sensor is enabled

170
flight/modules/Airspeed/gps_airspeed.c
View File

@ -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 <pios_math.h>
// 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];
}
}
/**
* @}
* @}
*/

306
flight/modules/Airspeed/imu_airspeed.c Normal file
View File

@ -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 <pios_math.h>
// 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);
}
/**
* @}
* @}
*/

14
flight/modules/Airspeed/inc/gps_airspeed.h → flight/modules/Airspeed/inc/imu_airspeed.h
View File

@ -3,10 +3,10 @@
* @addtogroup OpenPilotModules OpenPilot Modules * @addtogroup OpenPilotModules OpenPilot Modules
* @{ * @{
* @addtogroup AirspeedModule Airspeed Module * @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. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Airspeed module, reads temperature and pressure from BMP085 * @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., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef GPS_AIRSPEED_H #ifndef IMU_AIRSPEED_H
#define GPS_AIRSPEED_H #define IMU_AIRSPEED_H
void gps_airspeedInitialize(); void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings);
void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings); void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsData *airspeedSettings);
#endif // GPS_AIRSPEED_H #endif // IMU_AIRSPEED_H
/** /**
* @} * @}

2
flight/pios/inc/pios_math.h
View File

@ -40,6 +40,7 @@
#define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */ #define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */
#define M_1_PI_F 0.31830988618379067153776752675f /* 1/pi */ #define M_1_PI_F 0.31830988618379067153776752675f /* 1/pi */
#define M_2_PI_F 0.63661977236758134307553505349f /* 2/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_LN10_F 2.30258509299404568401799145468f /* ln(10) */
#define M_LN2_F 0.69314718055994530941723212146f /* ln(2) */ #define M_LN2_F 0.69314718055994530941723212146f /* ln(2) */
#define M_LNPI_F 1.14472988584940017414342735135f /* ln(pi) */ #define M_LNPI_F 1.14472988584940017414342735135f /* ln(pi) */
@ -54,6 +55,7 @@
#define M_PI_D 3.14159265358979323846264338328d /* pi */ #define M_PI_D 3.14159265358979323846264338328d /* pi */
#define M_PI_2_D 1.57079632679489661923132169164d /* pi/2 */ #define M_PI_2_D 1.57079632679489661923132169164d /* pi/2 */
#define M_PI_4_D 0.78539816339744830961566084582d /* pi/4 */ #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_SQRTPI_D 1.77245385090551602729816748334d /* sqrt(pi) */
#define M_2_SQRTPI_D 1.12837916709551257389615890312d /* 2/sqrt(pi) */ #define M_2_SQRTPI_D 1.12837916709551257389615890312d /* 2/sqrt(pi) */
#define M_1_PI_D 0.31830988618379067153776752675d /* 1/pi */ #define M_1_PI_D 0.31830988618379067153776752675d /* 1/pi */

1
flight/targets/boards/simposix/firmware/Makefile
View File

@ -100,6 +100,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c
SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c SRC += $(MATHLIB)/pid.c
SRC += $(MATHLIB)/mathmisc.c SRC += $(MATHLIB)/mathmisc.c
SRC += $(MATHLIB)/butterworth.c
SRC += $(PIOSCORECOMMON)/pios_task_monitor.c SRC += $(PIOSCORECOMMON)/pios_task_monitor.c
SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c

1
make/apps-defs.mk
View File

@ -109,6 +109,7 @@ SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c SRC += $(MATHLIB)/pid.c
SRC += $(MATHLIB)/mathmisc.c SRC += $(MATHLIB)/mathmisc.c
SRC += $(MATHLIB)/butterworth.c
SRC += $(FLIGHTLIB)/printf-stdarg.c SRC += $(FLIGHTLIB)/printf-stdarg.c
## Modules ## Modules

3
shared/uavobjectdefinition/airspeedsettings.xml
View File

@ -5,7 +5,8 @@
<field name="ZeroPoint" units="raw" type="uint16" elements="1" defaultvalue="0"/> <field name="ZeroPoint" units="raw" type="uint16" elements="1" defaultvalue="0"/>
<field name="Scale" units="raw" type="float" elements="1" defaultvalue="1.0"/> <field name="Scale" units="raw" type="float" elements="1" defaultvalue="1.0"/>
<field name="AirspeedSensorType" units="" type="enum" elements="1" options="PixHawkAirspeedMS4525DO,EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002,GroundSpeedBasedWindEstimation,None" defaultvalue="None"/> <field name="AirspeedSensorType" units="" type="enum" elements="1" options="PixHawkAirspeedMS4525DO,EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002,GroundSpeedBasedWindEstimation,None" defaultvalue="None"/>
<field name="GroundSpeedBasedEstimationLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.08" /> <field name="IMUBasedEstimationLowPassPeriod1" units="s" type="float" elements="1" defaultvalue="0.5" />
<field name="IMUBasedEstimationLowPassPeriod2" units="s" type="float" elements="1" defaultvalue="10" />
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>