rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

some last issues from review

Browse Source
This commit is contained in:
Corvus Corax 2013-05-22 22:58:14 +02:00
parent 9b95af2006
commit 843db63cf6
9 changed files with 71 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/modules/StateEstimation/filterair.c
View File

@ -74,11 +74,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
struct data *this = (struct data *)self->localdata;
// take static pressure altitude estimation for
if (ISSET(state->updated, SENSORUPDATES_baro)) {
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->altitude = state->baro[0];
}
// calculate true airspeed estimation
if (ISSET(state->updated, SENSORUPDATES_airspeed)) {
if (IS_SET(state->updated, SENSORUPDATES_airspeed)) {
state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude);
}

6
flight/modules/StateEstimation/filterbaro.c
View File

@ -79,20 +79,20 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (this->first_run) {
// Initialize to current altitude reading at initial location
if (ISSET(state->updated, SENSORUPDATES_baro)) {
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0;
this->baroOffset = state->baro[0];
}
} else {
// Track barometric altitude offset with a low pass filter
// based on GPS altitude if available
if (ISSET(state->updated, SENSORUPDATES_pos)) {
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA)
* (-state->pos[2] - state->baro[0]);
}
// calculate bias corrected altitude
if (ISSET(state->updated, SENSORUPDATES_baro)) {
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
state->baro[0] -= this->baroOffset;
}
}

6
flight/modules/StateEstimation/filtercf.c
View File

@ -158,19 +158,19 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
int32_t result = 0;
if (ISSET(state->updated, SENSORUPDATES_mag)) {
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
this->magUpdated = 1;
this->currentMag[0] = state->mag[0];
this->currentMag[1] = state->mag[1];
this->currentMag[2] = state->mag[2];
}
if (ISSET(state->updated, SENSORUPDATES_accel)) {
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
this->accelUpdated = 1;
this->currentAccel[0] = state->accel[0];
this->currentAccel[1] = state->accel[1];
this->currentAccel[2] = state->accel[2];
}
if (ISSET(state->updated, SENSORUPDATES_gyro)) {
if (IS_SET(state->updated, SENSORUPDATES_gyro)) {
if (this->accelUpdated) {
float attitude[4];
result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude);

32
flight/modules/StateEstimation/filterekf.c
View File

@ -193,7 +193,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// Get most recent data
#define UPDATE(shortname, num) \
if (ISSET(state->updated, SENSORUPDATES_##shortname)) { \
if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \
uint8_t t; \
for (t = 0; t < num; t++) { \
this->work.shortname[t] = state->shortname[t]; \
@ -210,11 +210,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// check whether mandatory updates are present accels must have been supplied already,
// and gyros must be supplied just now for a prediction step to take place
// ("gyros last" rule for multi object synchronization)
if (!(ISSET(this->work.updated, SENSORUPDATES_accel) && ISSET(state->updated, SENSORUPDATES_gyro))) {
UNSET(state->updated, SENSORUPDATES_pos);
UNSET(state->updated, SENSORUPDATES_vel);
UNSET(state->updated, SENSORUPDATES_attitude);
UNSET(state->updated, SENSORUPDATES_gyro);
if (!(IS_SET(this->work.updated, SENSORUPDATES_accel) && IS_SET(state->updated, SENSORUPDATES_gyro))) {
UNSET_MASK(state->updated, SENSORUPDATES_pos);
UNSET_MASK(state->updated, SENSORUPDATES_vel);
UNSET_MASK(state->updated, SENSORUPDATES_attitude);
UNSET_MASK(state->updated, SENSORUPDATES_gyro);
return 0;
}
@ -226,10 +226,10 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
(gpsData.PDOP > 4.0f) ||
(gpsData.Latitude == 0 && gpsData.Longitude == 0) ||
(this->homeLocation.Set != HOMELOCATION_SET_TRUE)) {
UNSET(state->updated, SENSORUPDATES_pos);
UNSET(state->updated, SENSORUPDATES_vel);
UNSET(this->work.updated, SENSORUPDATES_pos);
UNSET(this->work.updated, SENSORUPDATES_vel);
UNSET_MASK(state->updated, SENSORUPDATES_pos);
UNSET_MASK(state->updated, SENSORUPDATES_vel);
UNSET_MASK(this->work.updated, SENSORUPDATES_pos);
UNSET_MASK(this->work.updated, SENSORUPDATES_vel);
}
}
@ -243,7 +243,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
dT = 0.001f;
}
if (!this->inited && ISSET(this->work.updated, SENSORUPDATES_mag) && ISSET(this->work.updated, SENSORUPDATES_baro) && ISSET(this->work.updated, SENSORUPDATES_pos)) {
if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) {
// Don't initialize until all sensors are read
if (this->init_stage == 0) {
// Reset the INS algorithm
@ -362,11 +362,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// Advance the covariance estimate
INSCovariancePrediction(dT);
if (ISSET(this->work.updated, SENSORUPDATES_mag)) {
if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS;
}
if (ISSET(this->work.updated, SENSORUPDATES_baro)) {
if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {
sensors |= BARO_SENSOR;
}
@ -392,15 +392,15 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
);
}
if (ISSET(this->work.updated, SENSORUPDATES_pos)) {
if (IS_SET(this->work.updated, SENSORUPDATES_pos)) {
sensors |= POS_SENSORS;
}
if (ISSET(this->work.updated, SENSORUPDATES_vel)) {
if (IS_SET(this->work.updated, SENSORUPDATES_vel)) {
sensors |= HORIZ_SENSORS | VERT_SENSORS;
}
if (ISSET(this->work.updated, SENSORUPDATES_airspeed) && ((!ISSET(this->work.updated, SENSORUPDATES_vel) && !ISSET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) {
if (IS_SET(this->work.updated, SENSORUPDATES_airspeed) && ((!IS_SET(this->work.updated, SENSORUPDATES_vel) && !IS_SET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) {
// HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],

2
flight/modules/StateEstimation/filtermag.c
View File

@ -79,7 +79,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
if (ISSET(state->updated, SENSORUPDATES_mag)) {
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
}

4
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -55,10 +55,6 @@ typedef struct {
sensorUpdates updated;
} stateEstimation;
#define ISSET(bitfield, bit) ((bitfield) & (bit) ? 1 : 0)
#define UNSET(bitfield, bit) (bitfield) &= ~(bit)
typedef struct stateFilterStruct {
int32_t (*init)(struct stateFilterStruct *self);
int32_t (*filter)(struct stateFilterStruct *self, stateEstimation *state);

84
flight/modules/StateEstimation/stateestimation.c
View File

@ -36,7 +36,7 @@
#include <magsensor.h>
#include <barosensor.h>
#include <airspeedsensor.h>
#include <gpspositionsensor.h>
#include <positionsensor.h>
#include <gpsvelocitysensor.h>
#include <gyrostate.h>
@ -57,7 +57,7 @@
#define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define TIMEOUT_MS 100
#define TIMEOUT_MS 10
// Private types
@ -181,8 +181,6 @@ static const filterPipeline *ekf16Queue = &(filterPipeline) {
static void settingsUpdatedCb(UAVObjEvent *objEv);
static void sensorUpdatedCb(UAVObjEvent *objEv);
static void StateEstimationCb(void);
static void getNED(GPSPositionSensorData *gpsPosition, float *NED);
static bool sane(float value);
static inline int32_t maxint32_t(int32_t a, int32_t b)
{
@ -205,7 +203,7 @@ int32_t StateEstimationInitialize(void)
MagSensorInitialize();
BaroSensorInitialize();
AirspeedSensorInitialize();
GPSPositionSensorInitialize();
PositionSensorInitialize();
GPSVelocitySensorInitialize();
GyroStateInitialize();
@ -223,7 +221,7 @@ int32_t StateEstimationInitialize(void)
MagSensorConnectCallback(&sensorUpdatedCb);
BaroSensorConnectCallback(&sensorUpdatedCb);
AirspeedSensorConnectCallback(&sensorUpdatedCb);
GPSPositionSensorConnectCallback(&sensorUpdatedCb);
PositionSensorConnectCallback(&sensorUpdatedCb);
GPSVelocitySensorConnectCallback(&sensorUpdatedCb);
uint32_t stack_required = STACK_SIZE_BYTES;
@ -271,6 +269,7 @@ static void StateEstimationCb(void)
static uint16_t alarmcounter = 0;
static filterPipeline *current;
static stateEstimation states;
static uint32_t last_time;
switch (runState) {
case RUNSTATE_LOAD:
@ -279,7 +278,11 @@ static void StateEstimationCb(void)
// set alarm to warning if called through timeout
if (updatedSensors == 0) {
alarm = 1;
if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
alarm = 1;
}
} else {
last_time = PIOS_DELAY_GetRaw();
}
// check if a new filter chain should be initialized
@ -338,48 +341,38 @@ static void StateEstimationCb(void)
// most sensors get only rudimentary sanity checks
#define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \
if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
sensorname##Get(&s); \
if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \
if (ISREAL(s.a1) && ISREAL(s.a2) && ISREAL(s.a3)) { \
states.shortname[0] = s.a1; \
states.shortname[1] = s.a2; \
states.shortname[2] = s.a3; \
} \
else { \
UNSET(states.updated, SENSORUPDATES_##shortname); \
UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
} \
}
SANITYCHECK3(GyroSensor, gyro, x, y, z);
SANITYCHECK3(AccelSensor, accel, x, y, z);
SANITYCHECK3(MagSensor, mag, x, y, z);
SANITYCHECK3(PositionSensor, pos, North, East, Down);
SANITYCHECK3(GPSVelocitySensor, vel, North, East, Down);
#define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \
if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
sensorname##Get(&s); \
if (sane(s.a1) && EXTRACHECK) { \
if (ISREAL(s.a1) && EXTRACHECK) { \
states.shortname[0] = s.a1; \
} \
else { \
UNSET(states.updated, SENSORUPDATES_##shortname); \
UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
} \
}
SANITYCHECK1(BaroSensor, baro, Altitude, 1);
SANITYCHECK1(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter
// GPS is a tiny bit more tricky as GPSPositionSensor is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons
if (ISSET(states.updated, SENSORUPDATES_pos)) {
GPSPositionSensorData s;
GPSPositionSensorGet(&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, states.pos);
} else {
UNSET(states.updated, SENSORUPDATES_pos);
}
}
// at this point sensor state is stored in "states" with some rudimentary filtering applied
// apply all filters in the current filter chain
@ -411,7 +404,7 @@ static void StateEstimationCb(void)
// the final output of filters is saved in state variables
#define STORE3(statename, shortname, a1, a2, a3) \
if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
statename##Data s; \
statename##Get(&s); \
s.a1 = states.shortname[0]; \
@ -425,7 +418,7 @@ static void StateEstimationCb(void)
STORE3(PositionState, pos, North, East, Down);
STORE3(VelocityState, vel, North, East, Down);
#define STORE2(statename, shortname, a1, a2) \
if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
statename##Data s; \
statename##Get(&s); \
s.a1 = states.shortname[0]; \
@ -434,7 +427,7 @@ static void StateEstimationCb(void)
}
STORE2(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
// attitude nees manual conversion from quaternion to euler
if (ISSET(states.updated, SENSORUPDATES_attitude)) { \
if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
AttitudeStateData s;
AttitudeStateGet(&s);
s.q1 = states.attitude[0];
@ -490,7 +483,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
HomeLocationGet((HomeLocationData *)&homeLocation);
if (sane(homeLocation.Latitude) && sane(homeLocation.Longitude) && sane(homeLocation.Altitude) && sane(homeLocation.Be[0]) && sane(homeLocation.Be[1]) && sane(homeLocation.Be[2])) {
if (ISREAL(homeLocation.Latitude) && ISREAL(homeLocation.Longitude) && ISREAL(homeLocation.Altitude) && ISREAL(homeLocation.Be[0]) && ISREAL(homeLocation.Be[1]) && ISREAL(homeLocation.Be[2])) {
// Compute matrix to convert deltaLLA to NED
float lat, alt;
lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
@ -529,7 +522,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
updatedSensors |= SENSORUPDATES_mag;
}
if (ev->obj == GPSPositionSensorHandle()) {
if (ev->obj == PositionSensorHandle()) {
updatedSensors |= SENSORUPDATES_pos;
}
@ -548,39 +541,6 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
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(GPSPositionSensorData *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];
}
/**
* @brief sanity check for float values
* @note makes sure a float value is safe for further processing, ie not NAN and not INF
* @param[in] float value
* @returns true for safe and false for unsafe
*/
static bool sane(float value)
{
if (isnan(value) || isinf(value)) {
return false;
}
return true;
}
/**
* @}

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

@ -46,11 +46,19 @@
#define M_EULER_F 0.57721566490153286060651209008f /* Euler constant */
// Conversion macro
#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F))
#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f))
#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F))
#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f))
// Useful math macros
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define ISREAL(f) (!isnan(f) && !isinf(f))
// Bitfield access
#define IS_SET(field, mask) (((field) & (mask)) == (mask))
#define SET_MASK(field, mask) (field) |= (mask)
#define UNSET_MASK(field, mask) (field) &= ~(mask)
#endif // PIOS_MATH_H

12
shared/uavobjectdefinition/positionsensor.xml Normal file
View File

@ -0,0 +1,12 @@
<xml>
<object name="PositionSensor" singleinstance="true" settings="false">
<description>Contains the position measured relative to @ref HomeLocation</description>
<field name="North" units="m" type="float" elements="1"/>
<field name="East" units="m" type="float" elements="1"/>
<field name="Down" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="periodic" period="1000"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>