mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-22 14:19:42 +01:00
ongoing dev work, incomplete
This commit is contained in:
Corvus Corax
parent
d97934cf86
commit
2ab9f938c9
319
flight/modules/StateEstimation/stateestimation.c
Normal file
319
flight/modules/StateEstimation/stateestimation.c
Normal file
@ -0,0 +1,319 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup State Estimation
|
||||
* @brief Acquires sensor data and computes state estimate
|
||||
* @{
|
||||
*
|
||||
* @file stateestimation.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
|
||||
* @brief Module to handle all comms to the AHRS on a periodic basis.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
******************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
// TODO goes in header
|
||||
|
||||
|
||||
typedef enum {
|
||||
gyr_UPDATED = 1 << 0,
|
||||
acc_UPDATED = 1 << 1,
|
||||
mag_UPDATED = 1 << 2,
|
||||
pos_UPDATED = 1 << 3,
|
||||
vel_UPDATED = 1 << 4,
|
||||
bar_UPDATED = 1 << 5,
|
||||
ias_UPDATED = 1 << 6
|
||||
} sensorUpdates;
|
||||
|
||||
typedef struct {
|
||||
float gyr[3];
|
||||
float acc[3];
|
||||
float mag[3];
|
||||
float pos[3];
|
||||
float vel[3];
|
||||
float bar[1];
|
||||
float ias[1];
|
||||
sensorUpdates updated;
|
||||
} stateEstimation;
|
||||
|
||||
#define ISSET(bitfield, bit) ((bitfield) & (bit) ? 1 : 0)
|
||||
#define UNSET(bitfield, bit) (bitfield) &= ~(bit)
|
||||
|
||||
|
||||
typedef struct stateFilterStruct {
|
||||
int32_t (*init)(void);
|
||||
int32_t (*update)(stateEstimation *state);
|
||||
} stateFilter;
|
||||
|
||||
struct filterQueueStruct;
|
||||
typedef struct filterQueueStruct {
|
||||
stateFilter filter;
|
||||
struct filterQueueStruct *next;
|
||||
} filtereQueue;
|
||||
|
||||
//
|
||||
|
||||
#include <openpilot.h>
|
||||
#include <gyrosensor.h>
|
||||
#include <accelsensor.h>
|
||||
#include <magnetosensor.h>
|
||||
#include <barosensor.h>
|
||||
#include <airspeedsensor.h>
|
||||
#include <gpsposition.h>
|
||||
#include <gpsvelocity.h>
|
||||
|
||||
#include "revosettings.h"
|
||||
#include "homelocation.h"
|
||||
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 2048
|
||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_HIGH
|
||||
#define TASK_PRIORITY CALLBACK_TASKPRIORITY_FLIGHTCONTROL
|
||||
#define TIMEOUT_MS 100
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle attitudeTaskHandle;
|
||||
static DelayedCallbackInfo *stateEstimationCallback;
|
||||
|
||||
static RevoSettingsData revoSettings;
|
||||
static HomeLocationData homeLocation;
|
||||
static float LLA2NEDM[3];
|
||||
static volatile sensorUpdates updatedSensors;
|
||||
|
||||
// Private functions
|
||||
|
||||
static void settingsUpdatedCb(UAVObjEvent *objEv);
|
||||
static void sensorUpdatedCb(UAVObjEvent *objEv);
|
||||
static void StateEstimationCb(void);
|
||||
static int32_t getNED(GPSPositionData *gpsPosition, float *NED);
|
||||
|
||||
|
||||
/**
|
||||
* Initialise the module. Called before the start function
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t StateEstimationInitialize(void)
|
||||
{
|
||||
RevoSettingsInitialize();
|
||||
|
||||
RevoSettingsConnectCallback(&settingsUpdatedCb);
|
||||
HomeLocationConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
GyroSensorConnectCallback(&sensorUpdatedCb);
|
||||
AccelSensorConnectCallback(&sensorUpdatedCb);
|
||||
MagnetoSensorConnectCallback(&sensorUpdatedCb);
|
||||
BaroSensorConnectCallback(&sensorUpdatedCb);
|
||||
AirspeedSensorConnectCallback(&sensorUpdatedCb);
|
||||
GPSPositionConnectCallback(&sensorUpdatedCb);
|
||||
GPSVelocityConnectCallback(&sensorUpdatedCb);
|
||||
|
||||
stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE_BYTES);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Start the task. Expects all objects to be initialized by this point.
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t StateEstimationStart(void)
|
||||
{
|
||||
RevoSettingsConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
// Force settings update to make sure rotation loaded
|
||||
settingsUpdatedCb(NULL);
|
||||
|
||||
// Initialize Filters
|
||||
stateFilter magFilter = filterMagInitialize();
|
||||
stateFilter baroFilter = filterBaroInitialize();
|
||||
stateFilter stationaryFilter = filterStationaryInitialize();
|
||||
stateFilter cfFilter = filterCFInitialize();
|
||||
stateFilter cfmFilter = filterCFMInitialize();
|
||||
stateFilter ekf13Filter = filterEKF13Initialize();
|
||||
stateFilter ekf16Filter = filterEKF16Initialize();
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
|
||||
|
||||
/**
|
||||
* Module callback
|
||||
*/
|
||||
static void StateEstimationCb(void)
|
||||
{
|
||||
// alarms flag
|
||||
uint8_t alarm = 0;
|
||||
|
||||
// set alarm to warning if called through timeout
|
||||
if (updatedSensor == 0) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
|
||||
alarm = 1;
|
||||
}
|
||||
|
||||
// read updated sensor UAVObjects and set initial state
|
||||
stateEstimation sensors;
|
||||
sensors.updated = updatedSensors;
|
||||
updatedSensors ^= sensors.updated;
|
||||
|
||||
// most sensors get only rudimentary sanity checks
|
||||
#define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \
|
||||
if (ISSET(sensors.updated, shortname##_UPDATED)) { \
|
||||
sensorname##Data s; \
|
||||
sensorname##GET(&s); \
|
||||
if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \
|
||||
sensors.shortname[0] = s.a1; \
|
||||
sensors.shortname[1] = s.a2; \
|
||||
sensors.shortname[2] = s.a3; \
|
||||
} \
|
||||
else { \
|
||||
UNSET(sensors.updated, shortname##_UPDATED); \
|
||||
} \
|
||||
}
|
||||
SANITYCHECK3(GyroSensor, gyr, x, y, z);
|
||||
SANITYCHECK3(AccelSensor, acc, x, y, z);
|
||||
SANITYCHECK3(MagnetoSensor, mag, x, y, z);
|
||||
SANITYCHECK3(GPSVelocity, vel, North, East, Down);
|
||||
#define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \
|
||||
if (ISSET(sensors.updated, shortname##_UPDATED)) { \
|
||||
sensorname##Data s; \
|
||||
sensorname##GET(&s); \
|
||||
if (sane(s.a1) && EXTRACHECK) { \
|
||||
sensors.shortname[0] = s.a1; \
|
||||
} \
|
||||
else { \
|
||||
UNSET(sensors.updated, shortname##_UPDATED); \
|
||||
} \
|
||||
}
|
||||
SANITYCHECK1(BaroSensor, bar, Altitude, 1);
|
||||
SANITYCHECK1(AirspeedSensor, ias, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
|
||||
|
||||
if (ISSET(sensors.updated, pos_UPDATED)) {
|
||||
GPSPositionData s;
|
||||
GPSPositionGet(&s);
|
||||
if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) {
|
||||
getNED(&s, sensors.pos);
|
||||
} else {
|
||||
UNSET(sensors.updated, pos_UPDATED);
|
||||
}
|
||||
}
|
||||
|
||||
// at this point sensor state is stored in "sensors" with some rudimentary filtering applied
|
||||
|
||||
// traverse filtering chain
|
||||
|
||||
|
||||
// clear alarms if everything is alright, then schedule callback execution after timeout
|
||||
if (!alarm) {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
}
|
||||
DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, UPDATEMODE_SOONER);
|
||||
}
|
||||
|
||||
static void settingsUpdatedCb(UAVObjEvent *ev)
|
||||
{
|
||||
HomeLocationGet(&homeLocation);
|
||||
|
||||
if (sane(homeLocation.Latitude) && sane(homeLocation.Longitude) && sane(homeLocation.Altitude) && sane(homeLocation.Be[0]) && sane(homeLocation.Be[1]) && sane(homeLocation.Be[2])) {
|
||||
// Compute matrix to convert deltaLLA to NED
|
||||
float lat, alt;
|
||||
lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
|
||||
alt = homeLocation.Altitude;
|
||||
|
||||
LLA2NEDM[0] = alt + 6.378137E6f;
|
||||
LLA2NEDM[1] = cosf(lat) * (alt + 6.378137E6f);
|
||||
LLA2NEDM[2] = -1.0f;
|
||||
|
||||
// TODO: convert positionState to new reference frame and gracefully update EKF state!
|
||||
// needed for long range flights where the reference coordinate is adjusted in flight
|
||||
}
|
||||
|
||||
RevoSettingsGet(&revoSettings);
|
||||
|
||||
if
|
||||
}
|
||||
|
||||
static void sensorUpdatedCb(UAVObjEvent *ev)
|
||||
{
|
||||
if (!ev) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (ev->obj == GyroSensorHandle()) {
|
||||
updatedSensors |= gyr_UPDATED;
|
||||
}
|
||||
|
||||
if (ev->obj == AccelSensorHandle()) {
|
||||
updatedSensors |= acc_UPDATED;
|
||||
}
|
||||
|
||||
if (ev->obj == MagnetoSensorHandle()) {
|
||||
updatedSensors |= mag_UPDATED;
|
||||
}
|
||||
|
||||
if (ev->obj == GPSPositionHandle()) {
|
||||
updatedSensors |= pos_UPDATED;
|
||||
}
|
||||
|
||||
if (ev->obj == GPSVelocityHandle()) {
|
||||
updatedSensors |= vel_UPDATED;
|
||||
}
|
||||
|
||||
if (ev->obj == BaroSensorHandle()) {
|
||||
updatedSensors |= bar_UPDATED;
|
||||
}
|
||||
|
||||
if (ev->obj == AirspeedSensorHandle()) {
|
||||
updatedSensors |= ias_UPDATED;
|
||||
}
|
||||
|
||||
DelayedCallbackDispatch(stateEstimationCallback);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert the GPS LLA position into NED coordinates
|
||||
* @note this method uses a taylor expansion around the home coordinates
|
||||
* to convert to NED which allows it to be done with all floating
|
||||
* calculations
|
||||
* @param[in] Current GPS coordinates
|
||||
* @param[out] NED frame coordinates
|
||||
* @returns 0 for success, -1 for failure
|
||||
*/
|
||||
static void getNED(GPSPositionData *gpsPosition, float *NED)
|
||||
{
|
||||
float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f),
|
||||
DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f),
|
||||
(gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) };
|
||||
|
||||
NED[0] = LLA2NEDM[0] * dL[0];
|
||||
NED[1] = LLA2NEDM[1] * dL[1];
|
||||
NED[2] = LLA2NEDM[2] * dL[2];
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
10
shared/uavobjectdefinition/barostate.ekf
Normal file
10
shared/uavobjectdefinition/barostate.ekf
Normal file
@ -0,0 +1,10 @@
|
||||
<xml>
|
||||
<object name="BaroState" singleinstance="true" settings="false">
|
||||
<description>The filtered altitude estimate.</description>
|
||||
<field name="Altitude" units="m" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
@ -1,7 +1,7 @@
|
||||
<xml>
|
||||
<object name="RevoSettings" singleinstance="true" settings="true">
|
||||
<description>Settings for the revo to control the algorithm and what is updated</description>
|
||||
<field name="FusionAlgorithm" units="" type="enum" elements="1" options="Complementary,INSIndoor,INSOutdoor" defaultvalue="Complementary"/>
|
||||
<field name="FusionAlgorithm" units="" type="enum" elements="1" options="None,Complementary,Complementary+Mag,INS13Indoor,INS13Outdoor,INS16Indoor,INS16Outdoor" defaultvalue="Complementary"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user