mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
OP-1317 Added two file (actually renames)
This commit is contained in:
Andres
parent
02e62f91ed
commit
530c0aa967
217
flight/modules/Airspeed/imu_airspeed.c
Normal file
217
flight/modules/Airspeed/imu_airspeed.c
Normal file
@ -0,0 +1,217 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
* @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 <pios_math.h>
|
||||||
|
|
||||||
|
|
||||||
|
// Private constants
|
||||||
|
#define EPS_REORIENTATION 1e-8f
|
||||||
|
#define EPS_VELOCITY 1.f
|
||||||
|
|
||||||
|
// Private types
|
||||||
|
// structure with smoothed fuselage orientation, ground speed and their changes in time
|
||||||
|
struct IMUGlobals {
|
||||||
|
float xB[3];
|
||||||
|
float dxB[3];
|
||||||
|
float Vel[3];
|
||||||
|
float dVel[3];
|
||||||
|
};
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Initialize function loads first data sets, and allocates memory for structure.
|
||||||
|
*/
|
||||||
|
void imu_airspeedInitialize()
|
||||||
|
{
|
||||||
|
// This method saves memory in case we don't use the module.
|
||||||
|
imu = (struct IMUGlobals *)pvPortMalloc(sizeof(struct IMUGlobals));
|
||||||
|
|
||||||
|
// airspeed calculation variables
|
||||||
|
VelocityStateInitialize();
|
||||||
|
VelocityStateData velData;
|
||||||
|
VelocityStateGet(&velData);
|
||||||
|
|
||||||
|
AttitudeStateData attData;
|
||||||
|
AttitudeStateGet(&attData);
|
||||||
|
|
||||||
|
// for Holt-Winters double exponential smoothing (s smooth variable, b smooth trend)
|
||||||
|
// s1 = x1
|
||||||
|
// b1 = x1 - x0
|
||||||
|
// Calculate x of body frame
|
||||||
|
QuaternionC2xB(attData.q1, attData.q2, attData.q3, attData.q4, imu->xB);
|
||||||
|
|
||||||
|
// ground speed
|
||||||
|
imu->Vel[0] = velData.North;
|
||||||
|
imu->Vel[1] = velData.East;
|
||||||
|
imu->Vel[2] = velData.Down;
|
||||||
|
|
||||||
|
// trend assumed to be zero
|
||||||
|
imu->dxB[0] = imu->dxB[1] = imu->dxB[2] = 0.f;
|
||||||
|
imu->dVel[0] = imu->dVel[1] = imu->dVel[2] = 0.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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
|
||||||
|
* Note: filtering of xB and gpsV is more effective than of the airspeed itself. Reason: derivative of oscillating part is scaled
|
||||||
|
* by 1/period, i.e. fast oscillation => small period => large oscillation in derivative => large oscillation in airspeed
|
||||||
|
* Idea: treat gpsV and xB as noisy time series with trend and
|
||||||
|
* apply Holt-Winters double exponential smoothing to avoid smoothing out of trend (=derivative)
|
||||||
|
* s1 = x1
|
||||||
|
* b1 = x1 - x0
|
||||||
|
* s_{k+1} = alpha*x_{k+1} + (1-alpha)*(s_k + b_k)
|
||||||
|
* b_{k+1} = beta*(s_{k+1} - s_k) + (1-beta)b_k
|
||||||
|
*/
|
||||||
|
void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings)
|
||||||
|
{
|
||||||
|
const float alpha = airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha;
|
||||||
|
const float beta = airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha;
|
||||||
|
|
||||||
|
// get values and conduct smoothing of ground speed and orientation independently of the calculation of airspeed
|
||||||
|
{ // Scoping to save memory
|
||||||
|
float xB[3];
|
||||||
|
AttitudeStateData attData;
|
||||||
|
AttitudeStateGet(&attData);
|
||||||
|
VelocityStateData velData;
|
||||||
|
VelocityStateGet(&velData);
|
||||||
|
|
||||||
|
|
||||||
|
// Calculate rotation matrix
|
||||||
|
QuaternionC2xB(attData.q1, attData.q2, attData.q3, attData.q4, xB);
|
||||||
|
|
||||||
|
// Holt-Winters double exponential smoothing
|
||||||
|
// Orientation xB
|
||||||
|
float sk = imu->xB[0];
|
||||||
|
imu->xB[0] = alpha * xB[0] + (1.f - alpha) * (sk + imu->dxB[0]);
|
||||||
|
imu->dxB[0] = beta * (imu->xB[0] - sk) + (1.f - beta) * imu->dxB[0];
|
||||||
|
|
||||||
|
sk = imu->xB[1];
|
||||||
|
imu->xB[1] = alpha * xB[1] + (1.f - alpha) * (sk + imu->dxB[1]);
|
||||||
|
imu->dxB[1] = beta * (imu->xB[1] - sk) + (1.f - beta) * imu->dxB[1];
|
||||||
|
|
||||||
|
sk = imu->xB[2];
|
||||||
|
imu->xB[2] = alpha * xB[2] + (1.f - alpha) * (sk + imu->dxB[2]);
|
||||||
|
imu->dxB[2] = beta * (imu->xB[2] - sk) + (1.f - beta) * imu->dxB[2];
|
||||||
|
|
||||||
|
// Ground speed Vel
|
||||||
|
sk = imu->Vel[0];
|
||||||
|
imu->Vel[0] = alpha * velData.North + (1.f - alpha) * (sk + imu->dVel[0]);
|
||||||
|
imu->dVel[0] = beta * (imu->Vel[0] - sk) + (1.f - beta) * imu->dVel[0];
|
||||||
|
|
||||||
|
sk = imu->Vel[1];
|
||||||
|
imu->Vel[1] = alpha * velData.East + (1.f - alpha) * (sk + imu->dVel[1]);
|
||||||
|
imu->dVel[1] = beta * (imu->Vel[1] - sk) + (1.f - beta) * imu->dVel[1];
|
||||||
|
|
||||||
|
sk = imu->Vel[2];
|
||||||
|
imu->Vel[2] = alpha * velData.Down + (1.f - alpha) * (sk + imu->dVel[2]);
|
||||||
|
imu->dVel[2] = beta * (imu->Vel[2] - sk) + (1.f - beta) * imu->dVel[2];
|
||||||
|
|
||||||
|
/////// for debugging purposes only! ////////////
|
||||||
|
airspeedData->f[0] = imu->xB[0];
|
||||||
|
airspeedData->f[1] = imu->xB[1];
|
||||||
|
airspeedData->f[2] = imu->xB[2];
|
||||||
|
|
||||||
|
airspeedData->v[0] = imu->Vel[0];
|
||||||
|
airspeedData->v[1] = imu->Vel[1];
|
||||||
|
airspeedData->v[2] = imu->Vel[2];
|
||||||
|
|
||||||
|
airspeedData->df[0] = imu->dxB[0];
|
||||||
|
airspeedData->df[1] = imu->dxB[1];
|
||||||
|
airspeedData->df[2] = imu->dxB[2];
|
||||||
|
|
||||||
|
airspeedData->dv[0] = imu->dVel[0];
|
||||||
|
airspeedData->dv[1] = imu->dVel[1];
|
||||||
|
airspeedData->dv[2] = imu->dVel[2];
|
||||||
|
airspeedData->absdf = Sq(imu->dxB[0]) + Sq(imu->dxB[1]) + Sq(imu->dxB[2]);
|
||||||
|
airspeedData->dvdotdf = imu->dVel[0] * imu->dxB[0] + imu->dVel[1] * imu->dxB[1] + imu->dVel[2] * imu->dxB[2];
|
||||||
|
//////////////////////////////////////////////////
|
||||||
|
}
|
||||||
|
|
||||||
|
// Calculate the norm^2 of the difference between the two fuselage vectors
|
||||||
|
const float normDiffAttitude2 = Sq(imu->dxB[0]) + Sq(imu->dxB[1]) + Sq(imu->dxB[2]);
|
||||||
|
const float normVel2 = Sq(imu->Vel[0]) + Sq(imu->Vel[1]) + Sq(imu->Vel[2]);
|
||||||
|
|
||||||
|
// Some reorientation needed to be able to calculate airspeed and calculate only for sufficient velocity
|
||||||
|
if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY) {
|
||||||
|
// Calculate scalar product of difference vectors
|
||||||
|
const float dvdtDotdfdt = imu->dVel[0] * imu->dxB[0] + imu->dVel[1] * imu->dxB[1] + imu->dVel[2] * imu->dxB[2];
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
|
||||||
|
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
|
||||||
|
airspeedData->CalibratedAirspeed = 0;
|
||||||
|
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
|
||||||
|
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
|
||||||
|
} else {
|
||||||
|
airspeedData->CalibratedAirspeed = airspeed;
|
||||||
|
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
|
||||||
|
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
airspeedData->CalibratedAirspeed = 0;
|
||||||
|
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
|
||||||
|
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
42
flight/modules/Airspeed/inc/imu_airspeed.h
Normal file
42
flight/modules/Airspeed/inc/imu_airspeed.h
Normal file
@ -0,0 +1,42 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||||
|
* @{
|
||||||
|
* @addtogroup AirspeedModule Airspeed Module
|
||||||
|
* @brief Calculate airspeed as a function of the difference between sequential ground velocity and attitude measurements
|
||||||
|
* @{
|
||||||
|
*
|
||||||
|
* @file imu_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 IMU_AIRSPEED_H
|
||||||
|
#define IMU_AIRSPEED_H
|
||||||
|
|
||||||
|
void imu_airspeedInitialize();
|
||||||
|
void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings);
|
||||||
|
|
||||||
|
#endif // IMU_AIRSPEED_H
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
Loading…
x
Reference in New Issue
Block a user