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

filterchain rework

Browse Source
This commit is contained in:
Corvus Corax 2013-05-22 21:11:48 +02:00
parent 8fe159c457
commit b56de3b66b
12 changed files with 450 additions and 424 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,38 +43,43 @@
#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f)) #define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f))
// Private types // Private types
struct data {
// Private variables float altitude;
static float altitude = 0.0f; };
// Private functions // Private functions
static int32_t init(void); static int32_t init(stateFilter *self);
static int32_t filter(stateEstimation *state); static int32_t filter(stateFilter *self, stateEstimation *state);
int32_t filterAirInitialize(stateFilter *handle) int32_t filterAirInitialize(stateFilter *handle)
{ {
handle->init = &init; handle->init = &init;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED; return STACK_REQUIRED;
} }
static int32_t init(void) static int32_t init(stateFilter *self)
{ {
altitude = 0.0f; struct data *this = (struct data *)self->localdata;
this->altitude = 0.0f;
return 0; return 0;
} }
static int32_t filter(stateEstimation *state) static int32_t filter(stateFilter *self, stateEstimation *state)
{ {
struct data *this = (struct data *)self->localdata;
// take static pressure altitude estimation for // take static pressure altitude estimation for
if (ISSET(state->updated, SENSORUPDATES_baro)) { if (ISSET(state->updated, SENSORUPDATES_baro)) {
altitude = state->baro[0]; this->altitude = state->baro[0];
} }
// calculate true airspeed estimation // calculate true airspeed estimation
if (ISSET(state->updated, SENSORUPDATES_airspeed)) { if (ISSET(state->updated, SENSORUPDATES_airspeed)) {
state->air[1] = state->airspeed[0] * IAS2TAS(altitude); state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude);
} }
return 0; return 0;

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

@ -34,6 +34,8 @@
// Private constants // Private constants
#define STACK_REQUIRED 64
// low pass filter configuration to calculate offset // low pass filter configuration to calculate offset
// of barometric altitude sensor // of barometric altitude sensor
// reasoning: updates at: 10 Hz, tau= 300 s settle time // reasoning: updates at: 10 Hz, tau= 300 s settle time
@ -41,49 +43,57 @@
#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
// Private types // Private types
struct data {
float baroOffset;
bool first_run;
};
// Private variables // Private variables
static float baroOffset = 0.0f;
static bool first_run = 1;
// Private functions // Private functions
static int32_t init(void); static int32_t init(stateFilter *self);
static int32_t filter(stateEstimation *state); static int32_t filter(stateFilter *self, stateEstimation *state);
void filterBaroInitialize(stateFilter *handle) int32_t filterBaroInitialize(stateFilter *handle)
{ {
handle->init = &init; handle->init = &init;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
} }
static int32_t init(void) static int32_t init(stateFilter *self)
{ {
baroOffset = 0.0f; struct data *this = (struct data *)self->localdata;
first_run = 1;
this->baroOffset = 0.0f;
this->first_run = 1;
return 0; return 0;
} }
static int32_t filter(stateEstimation *state) static int32_t filter(stateFilter *self, stateEstimation *state)
{ {
if (first_run) { struct data *this = (struct data *)self->localdata;
if (this->first_run) {
// Initialize to current altitude reading at initial location // Initialize to current altitude reading at initial location
if (ISSET(state->updated, bar_UPDATED)) { if (ISSET(state->updated, SENSORUPDATES_baro)) {
first_run = 0; this->first_run = 0;
baroOffset = state->bar[0]; this->baroOffset = state->baro[0];
} }
} else { } else {
// Track barometric altitude offset with a low pass filter // Track barometric altitude offset with a low pass filter
// based on GPS altitude if available // based on GPS altitude if available
if (ISSET(state->updated, pos_UPDATED)) { if (ISSET(state->updated, SENSORUPDATES_pos)) {
baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA) (1.0f - BARO_OFFSET_LOWPASS_ALPHA)
* (-state->pos[2] - state->bar[0]); * (-state->pos[2] - state->baro[0]);
} }
// calculate bias corrected altitude // calculate bias corrected altitude
if (ISSET(state->updated, bar_UPDATED)) { if (ISSET(state->updated, SENSORUPDATES_baro)) {
state->bar[0] -= baroOffset; state->baro[0] -= this->baroOffset;
} }
} }

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

@ -44,34 +44,36 @@
// Private constants // Private constants
#define STACK_REQUIRED 512
// Private types // Private types
struct data {
AttitudeSettingsData attitudeSettings;
HomeLocationData homeLocation;
bool first_run;
bool useMag;
float currentAccel[3];
float currentMag[3];
float gyroBias[3];
bool accelUpdated;
bool magUpdated;
float accel_alpha;
bool accel_filter_enabled;
int32_t timeval;
uint8_t init;
};
// Private variables // Private variables
static AttitudeSettingsData attitudeSettings; bool initialized = 0;
static FlightStatusData flightStatus; static FlightStatusData flightStatus;
static HomeLocationData homeLocation;
static bool initialized = 0;
static bool first_run = 1;
static bool useMag = 0;
static float currentAccel[3];
static float currentMag[3];
static float gyroBias[3];
static bool accelUpdated = 0;
static bool magUpdated = 0;
static float accel_alpha = 0;
static bool accel_filter_enabled = false;
// Private functions // Private functions
static int32_t initwithmag(void); static int32_t initwithmag(stateFilter *self);
static int32_t initwithoutmag(void); static int32_t initwithoutmag(stateFilter *self);
static int32_t maininit(void); static int32_t maininit(stateFilter *self);
static int32_t filter(stateEstimation *state); static int32_t filter(stateFilter *self, stateEstimation *state);
static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], float q[4]); static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]);
static void flightStatusUpdatedCb(UAVObjEvent *ev); static void flightStatusUpdatedCb(UAVObjEvent *ev);
@ -87,52 +89,62 @@ static void globalInit(void)
} }
} }
void filterCFInitialize(stateFilter *handle) int32_t filterCFInitialize(stateFilter *handle)
{ {
globalInit(); globalInit();
handle->init = &initwithoutmag; handle->init = &initwithoutmag;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
} }
void filterCFMInitialize(stateFilter *handle) int32_t filterCFMInitialize(stateFilter *handle)
{ {
globalInit(); globalInit();
handle->init = &initwithmag; handle->init = &initwithmag;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
} }
static int32_t initwithmag(void) static int32_t initwithmag(stateFilter *self)
{ {
useMag = 1; struct data *this = (struct data *)self->localdata;
return maininit();
this->useMag = 1;
return maininit(self);
} }
static int32_t initwithoutmag(void) static int32_t initwithoutmag(stateFilter *self)
{ {
useMag = 0; struct data *this = (struct data *)self->localdata;
return maininit();
this->useMag = 0;
return maininit(self);
} }
static int32_t maininit(void) static int32_t maininit(stateFilter *self)
{ {
first_run = 1; struct data *this = (struct data *)self->localdata;
accelUpdated = 0;
AttitudeSettingsGet(&attitudeSettings); this->first_run = 1;
HomeLocationGet(&homeLocation); this->accelUpdated = 0;
AttitudeSettingsGet(&this->attitudeSettings);
HomeLocationGet(&this->homeLocation);
const float fakeDt = 0.0025f; const float fakeDt = 0.0025f;
if (attitudeSettings.AccelTau < 0.0001f) { if (this->attitudeSettings.AccelTau < 0.0001f) {
accel_alpha = 0; // not trusting this to resolve to 0 this->accel_alpha = 0; // not trusting this to resolve to 0
accel_filter_enabled = false; this->accel_filter_enabled = false;
} else { } else {
accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); this->accel_alpha = expf(-fakeDt / this->attitudeSettings.AccelTau);
accel_filter_enabled = true; this->accel_filter_enabled = true;
} }
// reset gyro Bias // reset gyro Bias
gyroBias[0] = 0.0f; this->gyroBias[0] = 0.0f;
gyroBias[1] = 0.0f; this->gyroBias[1] = 0.0f;
gyroBias[2] = 0.0f; this->gyroBias[2] = 0.0f;
return 0; return 0;
} }
@ -140,47 +152,49 @@ static int32_t maininit(void)
/** /**
* Collect all required state variables, then run complementary filter * Collect all required state variables, then run complementary filter
*/ */
static int32_t filter(stateEstimation *state) static int32_t filter(stateFilter *self, stateEstimation *state)
{ {
struct data *this = (struct data *)self->localdata;
int32_t result = 0; int32_t result = 0;
if (ISSET(state->updated, mag_UPDATED)) { if (ISSET(state->updated, SENSORUPDATES_mag)) {
magUpdated = 1; this->magUpdated = 1;
currentMag[0] = state->mag[0]; this->currentMag[0] = state->mag[0];
currentMag[1] = state->mag[1]; this->currentMag[1] = state->mag[1];
currentMag[2] = state->mag[2]; this->currentMag[2] = state->mag[2];
} }
if (ISSET(state->updated, acc_UPDATED)) { if (ISSET(state->updated, SENSORUPDATES_accel)) {
accelUpdated = 1; this->accelUpdated = 1;
currentAccel[0] = state->acc[0]; this->currentAccel[0] = state->accel[0];
currentAccel[1] = state->acc[1]; this->currentAccel[1] = state->accel[1];
currentAccel[2] = state->acc[2]; this->currentAccel[2] = state->accel[2];
} }
if (ISSET(state->updated, gyr_UPDATED)) { if (ISSET(state->updated, SENSORUPDATES_gyro)) {
if (accelUpdated) { if (this->accelUpdated) {
float att[4]; float attitude[4];
result = complementaryFilter(state->gyr, currentAccel, currentMag, att); result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude);
if (!result) { if (!result) {
state->att[0] = att[0]; state->attitude[0] = attitude[0];
state->att[1] = att[1]; state->attitude[1] = attitude[1];
state->att[2] = att[2]; state->attitude[2] = attitude[2];
state->att[3] = att[3]; state->attitude[3] = attitude[3];
state->updated |= att_UPDATED; state->updated |= SENSORUPDATES_attitude;
} }
accelUpdated = 0; this->accelUpdated = 0;
magUpdated = 0; this->magUpdated = 0;
} }
} }
return result; return result;
} }
static inline void apply_accel_filter(const float *raw, float *filtered) static inline void apply_accel_filter(const struct data *this, const float *raw, float *filtered)
{ {
if (accel_filter_enabled) { if (this->accel_filter_enabled) {
filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); filtered[0] = filtered[0] * this->accel_alpha + raw[0] * (1 - this->accel_alpha);
filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); filtered[1] = filtered[1] * this->accel_alpha + raw[1] * (1 - this->accel_alpha);
filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); filtered[2] = filtered[2] * this->accel_alpha + raw[2] * (1 - this->accel_alpha);
} else { } else {
filtered[0] = raw[0]; filtered[0] = raw[0];
filtered[1] = raw[1]; filtered[1] = raw[1];
@ -188,19 +202,17 @@ static inline void apply_accel_filter(const float *raw, float *filtered)
} }
} }
static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], float q[4]) static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
{ {
static int32_t timeval;
float dT; float dT;
static uint8_t init = 0;
float magKp = 0.0f; // TODO: make this non hardcoded at some point float magKp = 0.0f; // TODO: make this non hardcoded at some point
float magKi = 0.000001f; float magKi = 0.000001f;
// During initialization and // During initialization and
if (first_run) { if (this->first_run) {
#if defined(PIOS_INCLUDE_HMC5883) #if defined(PIOS_INCLUDE_HMC5883)
// wait until mags have been updated // wait until mags have been updated
if (!magUpdated) { if (!this->magUpdated) {
return 1; return 1;
} }
#else #else
@ -210,7 +222,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
#endif #endif
AttitudeStateData attitudeState; // base on previous state AttitudeStateData attitudeState; // base on previous state
AttitudeStateGet(&attitudeState); AttitudeStateGet(&attitudeState);
init = 0; this->init = 0;
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level // so pseudo "north" vector can be estimated even if the board is not level
@ -235,37 +247,37 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, q); RPY2Quaternion(&attitudeState.Roll, attitude);
first_run = 0; this->first_run = 0;
timeval = PIOS_DELAY_GetRaw(); this->timeval = PIOS_DELAY_GetRaw();
return 0; return 0;
} }
if ((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { if ((this->init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias // For first 7 seconds use accels to get gyro bias
attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9f; this->attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23f; this->attitudeSettings.YawBiasRate = 0.23f;
magKp = 1.0f; magKp = 1.0f;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9f; this->attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23f; this->attitudeSettings.YawBiasRate = 0.23f;
magKp = 1.0f; magKp = 1.0f;
init = 0; this->init = 0;
} else if (init == 0) { } else if (this->init == 0) {
// Reload settings (all the rates) // Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings); AttitudeSettingsGet(&this->attitudeSettings);
magKp = 0.01f; magKp = 0.01f;
init = 1; this->init = 1;
} }
// Compute the dT using the cpu clock // Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; dT = PIOS_DELAY_DiffuS(this->timeval) / 1000000.0f;
timeval = PIOS_DELAY_GetRaw(); this->timeval = PIOS_DELAY_GetRaw();
if (dT < 0.001f) { // safe bounds if (dT < 0.001f) { // safe bounds
dT = 0.001f; dT = 0.001f;
} }
@ -274,21 +286,21 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
AttitudeStateGet(&attitudeState); AttitudeStateGet(&attitudeState);
// Get the current attitude estimate // Get the current attitude estimate
quat_copy(&attitudeState.q1, q); quat_copy(&attitudeState.q1, attitude);
float accels_filtered[3]; float accels_filtered[3];
// Apply smoothing to accel values, to reduce vibration noise before main calculations. // Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter(accel, accels_filtered); apply_accel_filter(this, accel, accels_filtered);
// Rotate gravity to body frame and cross with accels // Rotate gravity to body frame and cross with accels
float grot[3]; float grot[3];
grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); grot[0] = -(2.0f * (attitude[1] * attitude[3] - attitude[0] * attitude[2]));
grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1]));
grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]);
float grot_filtered[3]; float grot_filtered[3];
float accel_err[3]; float accel_err[3];
apply_accel_filter(grot, grot_filtered); apply_accel_filter(this, grot, grot_filtered);
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude // Account for accel magnitude
@ -299,7 +311,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
// Account for filtered gravity vector magnitude // Account for filtered gravity vector magnitude
float grot_mag; float grot_mag;
if (accel_filter_enabled) { if (this->accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else { } else {
grot_mag = 1.0f; grot_mag = 1.0f;
@ -313,14 +325,14 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
accel_err[2] /= (accel_mag * grot_mag); accel_err[2] /= (accel_mag * grot_mag);
float mag_err[3] = { 0.0f }; float mag_err[3] = { 0.0f };
if (magUpdated && useMag) { if (this->magUpdated && this->useMag) {
// Rotate gravity to body frame and cross with accels // Rotate gravity to body frame and cross with accels
float brot[3]; float brot[3];
float Rbe[3][3]; float Rbe[3][3];
Quaternion2R(q, Rbe); Quaternion2R(attitude, Rbe);
rot_mult(Rbe, homeLocation.Be, brot); rot_mult(Rbe, this->homeLocation.Be, brot);
float mag_len = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]); float mag_len = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] /= mag_len; mag[0] /= mag_len;
@ -343,57 +355,57 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
} }
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyroBias[0] -= accel_err[0] * attitudeSettings.AccelKi; this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi;
gyroBias[1] -= accel_err[1] * attitudeSettings.AccelKi; this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi;
gyroBias[2] -= mag_err[2] * magKi; this->gyroBias[2] -= mag_err[2] * magKi;
// Correct rates based on integral coefficient // Correct rates based on integral coefficient
gyro[0] -= gyroBias[0]; gyro[0] -= this->gyroBias[0];
gyro[1] -= gyroBias[1]; gyro[1] -= this->gyroBias[1];
gyro[2] -= gyroBias[2]; gyro[2] -= this->gyroBias[2];
float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] }; float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] };
// Correct rates based on proportional coefficient // Correct rates based on proportional coefficient
gyrotmp[0] += accel_err[0] * attitudeSettings.AccelKp / dT; gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT;
gyrotmp[1] += accel_err[1] * attitudeSettings.AccelKp / dT; gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT;
if (useMag) { if (this->useMag) {
gyrotmp[2] += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
} else { } else {
gyrotmp[2] += accel_err[2] * attitudeSettings.AccelKp / dT; gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT;
} }
// Work out time derivative from INSAlgo writeup // Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s // Also accounts for the fact that gyros are in deg/s
float qdot[4]; float qdot[4];
qdot[0] = DEG2RAD(-q[1] * gyrotmp[0] - q[2] * gyrotmp[1] - q[3] * gyrotmp[2]) * dT / 2; qdot[0] = DEG2RAD(-attitude[1] * gyrotmp[0] - attitude[2] * gyrotmp[1] - attitude[3] * gyrotmp[2]) * dT / 2;
qdot[1] = DEG2RAD(q[0] * gyrotmp[0] - q[3] * gyrotmp[1] + q[2] * gyrotmp[2]) * dT / 2; qdot[1] = DEG2RAD(attitude[0] * gyrotmp[0] - attitude[3] * gyrotmp[1] + attitude[2] * gyrotmp[2]) * dT / 2;
qdot[2] = DEG2RAD(q[3] * gyrotmp[0] + q[0] * gyrotmp[1] - q[1] * gyrotmp[2]) * dT / 2; qdot[2] = DEG2RAD(attitude[3] * gyrotmp[0] + attitude[0] * gyrotmp[1] - attitude[1] * gyrotmp[2]) * dT / 2;
qdot[3] = DEG2RAD(-q[2] * gyrotmp[0] + q[1] * gyrotmp[1] + q[0] * gyrotmp[2]) * dT / 2; qdot[3] = DEG2RAD(-attitude[2] * gyrotmp[0] + attitude[1] * gyrotmp[1] + attitude[0] * gyrotmp[2]) * dT / 2;
// Take a time step // Take a time step
q[0] = q[0] + qdot[0]; attitude[0] = attitude[0] + qdot[0];
q[1] = q[1] + qdot[1]; attitude[1] = attitude[1] + qdot[1];
q[2] = q[2] + qdot[2]; attitude[2] = attitude[2] + qdot[2];
q[3] = q[3] + qdot[3]; attitude[3] = attitude[3] + qdot[3];
if (q[0] < 0.0f) { if (attitude[0] < 0.0f) {
q[0] = -q[0]; attitude[0] = -attitude[0];
q[1] = -q[1]; attitude[1] = -attitude[1];
q[2] = -q[2]; attitude[2] = -attitude[2];
q[3] = -q[3]; attitude[3] = -attitude[3];
} }
// Renomalize // Renomalize
float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); float qmag = sqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]);
q[0] = q[0] / qmag; attitude[0] = attitude[0] / qmag;
q[1] = q[1] / qmag; attitude[1] = attitude[1] / qmag;
q[2] = q[2] / qmag; attitude[2] = attitude[2] / qmag;
q[3] = q[3] / qmag; attitude[3] = attitude[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit. // If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN // THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
first_run = 1; this->first_run = 1;
return 2; return 2;
} }

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

@ -44,25 +44,33 @@
// Private constants // Private constants
#define STACK_REQUIRED 2048
// Private types // Private types
struct data {
EKFConfigurationData ekfConfiguration;
HomeLocationData homeLocation;
bool usePos;
int32_t init_stage;
stateEstimation work;
uint32_t ins_last_time;
bool inited;
};
// Private variables // Private variables
static EKFConfigurationData ekfConfiguration;
static HomeLocationData homeLocation;
static bool initialized = 0; static bool initialized = 0;
static bool first_run = 1;
static bool usePos = 0;
// Private functions // Private functions
static int32_t init13i(void); static int32_t init13i(stateFilter *self);
static int32_t init13(void); static int32_t init13(stateFilter *self);
static int32_t maininit(void); static int32_t maininit(stateFilter *self);
static int32_t filter(stateEstimation *state); static int32_t filter(stateFilter *self, stateEstimation *state);
static inline bool invalid(float data);
static inline bool invalid_var(float data); static inline bool invalid_var(float data);
static void globalInit(void); static void globalInit(void);
@ -77,73 +85,90 @@ static void globalInit(void)
} }
} }
void filterEKF13iInitialize(stateFilter *handle) int32_t filterEKF13iInitialize(stateFilter *handle)
{ {
globalInit(); globalInit();
handle->init = &init13i; handle->init = &init13i;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
} }
void filterEKF13Initialize(stateFilter *handle) int32_t filterEKF13Initialize(stateFilter *handle)
{ {
globalInit(); globalInit();
handle->init = &init13; handle->init = &init13;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
} }
// XXX // XXX
// TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through // TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through
// XXX // XXX
void filterEKF16iInitialize(stateFilter *handle) int32_t filterEKF16iInitialize(stateFilter *handle)
{ {
globalInit(); globalInit();
handle->init = &init13i; handle->init = &init13i;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
} }
void filterEKF16Initialize(stateFilter *handle) int32_t filterEKF16Initialize(stateFilter *handle)
{ {
globalInit(); globalInit();
handle->init = &init13; handle->init = &init13;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
} }
static int32_t init13i(void) static int32_t init13i(stateFilter *self)
{ {
usePos = 0; struct data *this = (struct data *)self->localdata;
return maininit();
this->usePos = 0;
return maininit(self);
} }
static int32_t init13(void) static int32_t init13(stateFilter *self)
{ {
usePos = 1; struct data *this = (struct data *)self->localdata;
return maininit();
this->usePos = 1;
return maininit(self);
} }
static int32_t maininit(void) static int32_t maininit(stateFilter *self)
{ {
first_run = 1; struct data *this = (struct data *)self->localdata;
EKFConfigurationGet(&ekfConfiguration); this->inited = false;
this->init_stage = 0;
this->work.updated = 0;
this->ins_last_time = PIOS_DELAY_GetRaw();
EKFConfigurationGet(&this->ekfConfiguration);
int t; int t;
// plausibility check // plausibility check
for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.P[t])) { if (invalid_var(this->ekfConfiguration.P[t])) {
return 2; return 2;
} }
} }
for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.Q[t])) { if (invalid_var(this->ekfConfiguration.Q[t])) {
return 2; return 2;
} }
} }
for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.R[t])) { if (invalid_var(this->ekfConfiguration.R[t])) {
return 2; return 2;
} }
} }
HomeLocationGet(&homeLocation); HomeLocationGet(&this->homeLocation);
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false) // switching between indoor and outdoor mode with Set = false)
if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) { if ((this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2] < 1e-5f)) {
return 2; return 2;
} }
@ -154,14 +179,9 @@ static int32_t maininit(void)
/** /**
* Collect all required state variables, then run complementary filter * Collect all required state variables, then run complementary filter
*/ */
static int32_t filter(stateEstimation *state) static int32_t filter(stateFilter *self, stateEstimation *state)
{ {
static int32_t init_stage; struct data *this = (struct data *)self->localdata;
static stateEstimation work;
static uint32_t ins_last_time = 0;
static bool inited;
const float zeros[3] = { 0.0f, 0.0f, 0.0f }; const float zeros[3] = { 0.0f, 0.0f, 0.0f };
@ -169,58 +189,52 @@ static int32_t filter(stateEstimation *state)
float dT; float dT;
uint16_t sensors = 0; uint16_t sensors = 0;
if (inited) { this->work.updated |= state->updated;
work.updated = 0;
}
if (first_run) {
first_run = false;
inited = false;
init_stage = 0;
work.updated = 0;
ins_last_time = PIOS_DELAY_GetRaw();
return 1;
}
work.updated |= state->updated;
// Get most recent data // Get most recent data
#define UPDATE(shortname, num) \ #define UPDATE(shortname, num) \
if (ISSET(state->updated, shortname##_UPDATED)) { \ if (ISSET(state->updated, SENSORUPDATES_##shortname)) { \
uint8_t t; \ uint8_t t; \
for (t = 0; t < num; t++) { \ for (t = 0; t < num; t++) { \
work.shortname[t] = state->shortname[t]; \ this->work.shortname[t] = state->shortname[t]; \
} \ } \
} }
UPDATE(gyr, 3); UPDATE(gyro, 3);
UPDATE(acc, 3); UPDATE(accel, 3);
UPDATE(mag, 3); UPDATE(mag, 3);
UPDATE(bar, 1); UPDATE(baro, 1);
UPDATE(pos, 3); UPDATE(pos, 3);
UPDATE(vel, 3); UPDATE(vel, 3);
UPDATE(air, 2); UPDATE(airspeed, 2);
// 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);
return 0;
}
if (usePos) { if (this->usePos) {
GPSPositionData gpsData; GPSPositionData gpsData;
GPSPositionGet(&gpsData); GPSPositionGet(&gpsData);
// Have a minimum requirement for gps usage // Have a minimum requirement for gps usage
if ((gpsData.Satellites < 7) || if ((gpsData.Satellites < 7) ||
(gpsData.PDOP > 4.0f) || (gpsData.PDOP > 4.0f) ||
(gpsData.Latitude == 0 && gpsData.Longitude == 0) || (gpsData.Latitude == 0 && gpsData.Longitude == 0) ||
(homeLocation.Set != HOMELOCATION_SET_TRUE)) { (this->homeLocation.Set != HOMELOCATION_SET_TRUE)) {
UNSET(state->updated, pos_UPDATED); UNSET(state->updated, SENSORUPDATES_pos);
UNSET(state->updated, vel_UPDATED); UNSET(state->updated, SENSORUPDATES_vel);
UNSET(work.updated, pos_UPDATED); UNSET(this->work.updated, SENSORUPDATES_pos);
UNSET(work.updated, vel_UPDATED); UNSET(this->work.updated, SENSORUPDATES_vel);
} }
} }
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw(); this->ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches // This should only happen at start up or at mode switches
if (dT > 0.01f) { if (dT > 0.01f) {
@ -229,28 +243,28 @@ static int32_t filter(stateEstimation *state)
dT = 0.001f; dT = 0.001f;
} }
if (!inited && ISSET(work.updated, mag_UPDATED) && ISSET(work.updated, bar_UPDATED) && ISSET(work.updated, pos_UPDATED)) { if (!this->inited && ISSET(this->work.updated, SENSORUPDATES_mag) && ISSET(this->work.updated, SENSORUPDATES_baro) && ISSET(this->work.updated, SENSORUPDATES_pos)) {
// Don't initialize until all sensors are read // Don't initialize until all sensors are read
if (init_stage == 0) { if (this->init_stage == 0) {
// Reset the INS algorithm // Reset the INS algorithm
INSGPSInit(); INSGPSInit();
INSSetMagVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_MAGX], INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGY], this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] }
); );
INSSetAccelVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] }
); );
INSSetGyroVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], INSSetGyroVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] }
); );
INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] }
); );
INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); INSSetBaroVar(this->ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]);
// Initialize the gyro bias // Initialize the gyro bias
float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; float gyro_bias[3] = { 0.0f, 0.0f, 0.0f };
@ -261,15 +275,15 @@ static int32_t filter(stateEstimation *state)
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level // so pseudo "north" vector can be estimated even if the board is not level
attitudeState.Roll = atan2f(-work.acc[1], -work.acc[2]); attitudeState.Roll = atan2f(-this->work.accel[1], -this->work.accel[2]);
float zn = cosf(attitudeState.Roll) * work.mag[2] + sinf(attitudeState.Roll) * work.mag[1]; float zn = cosf(attitudeState.Roll) * this->work.mag[2] + sinf(attitudeState.Roll) * this->work.mag[1];
float yn = cosf(attitudeState.Roll) * work.mag[1] - sinf(attitudeState.Roll) * work.mag[2]; float yn = cosf(attitudeState.Roll) * this->work.mag[1] - sinf(attitudeState.Roll) * this->work.mag[2];
// rotate accels z vector according to roll // rotate accels z vector according to roll
float azn = cosf(attitudeState.Roll) * work.acc[2] + sinf(attitudeState.Roll) * work.acc[1]; float azn = cosf(attitudeState.Roll) * this->work.accel[2] + sinf(attitudeState.Roll) * this->work.accel[1];
attitudeState.Pitch = atan2f(work.acc[0], -azn); attitudeState.Pitch = atan2f(this->work.accel[0], -azn);
float xn = cosf(attitudeState.Pitch) * work.mag[0] + sinf(attitudeState.Pitch) * zn; float xn = cosf(attitudeState.Pitch) * this->work.mag[0] + sinf(attitudeState.Pitch) * zn;
attitudeState.Yaw = atan2f(-yn, xn); attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack // TODO: This is still a hack
@ -282,122 +296,125 @@ static int32_t filter(stateEstimation *state)
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, work.att); RPY2Quaternion(&attitudeState.Roll, this->work.attitude);
INSSetState(work.pos, (float *)zeros, work.att, (float *)zeros, (float *)zeros); INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros);
INSResetP(ekfConfiguration.P); INSResetP(this->ekfConfiguration.P);
} else { } else {
// Run prediction a bit before any corrections // Run prediction a bit before any corrections
float gyros[3] = { DEG2RAD(work.gyr[0]), DEG2RAD(work.gyr[1]), DEG2RAD(work.gyr[2]) }; float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
INSStatePrediction(gyros, work.acc, dT); INSStatePrediction(gyros, this->work.accel, dT);
state->att[0] = Nav.q[0]; // Copy the attitude into the state
state->att[1] = Nav.q[1]; // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
state->att[2] = Nav.q[2]; state->attitude[0] = Nav.q[0];
state->att[3] = Nav.q[3]; state->attitude[1] = Nav.q[1];
state->gyr[0] += Nav.gyro_bias[0]; state->attitude[2] = Nav.q[2];
state->gyr[1] += Nav.gyro_bias[1]; state->attitude[3] = Nav.q[3];
state->gyr[2] += Nav.gyro_bias[2]; state->gyro[0] += Nav.gyro_bias[0];
state->gyro[1] += Nav.gyro_bias[1];
state->gyro[2] += Nav.gyro_bias[2];
state->pos[0] = Nav.Pos[0]; state->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1]; state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2]; state->pos[2] = Nav.Pos[2];
state->vel[0] = Nav.Vel[0]; state->vel[0] = Nav.Vel[0];
state->vel[1] = Nav.Vel[1]; state->vel[1] = Nav.Vel[1];
state->vel[2] = Nav.Vel[2]; state->vel[2] = Nav.Vel[2];
state->updated |= att_UPDATED | pos_UPDATED | vel_UPDATED | gyr_UPDATED; state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
} }
init_stage++; this->init_stage++;
if (init_stage > 10) { if (this->init_stage > 10) {
inited = true; this->inited = true;
} }
return 0; return 0;
} }
if (!inited) { if (!this->inited) {
return 1; return 1;
} }
float gyros[3] = { DEG2RAD(work.gyr[0]), DEG2RAD(work.gyr[1]), DEG2RAD(work.gyr[2]) }; float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
// Advance the state estimate // Advance the state estimate
INSStatePrediction(gyros, work.acc, dT); INSStatePrediction(gyros, this->work.accel, dT);
// Copy the attitude into the UAVO // Copy the attitude into the state
state->att[0] = Nav.q[0]; // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
state->att[1] = Nav.q[1]; state->attitude[0] = Nav.q[0];
state->att[2] = Nav.q[2]; state->attitude[1] = Nav.q[1];
state->att[3] = Nav.q[3]; state->attitude[2] = Nav.q[2];
state->gyr[0] += Nav.gyro_bias[0]; state->attitude[3] = Nav.q[3];
state->gyr[1] += Nav.gyro_bias[1]; state->gyro[0] += Nav.gyro_bias[0];
state->gyr[2] += Nav.gyro_bias[2]; state->gyro[1] += Nav.gyro_bias[1];
state->gyro[2] += Nav.gyro_bias[2];
state->pos[0] = Nav.Pos[0]; state->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1]; state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2]; state->pos[2] = Nav.Pos[2];
state->vel[0] = Nav.Vel[0]; state->vel[0] = Nav.Vel[0];
state->vel[1] = Nav.Vel[1]; state->vel[1] = Nav.Vel[1];
state->vel[2] = Nav.Vel[2]; state->vel[2] = Nav.Vel[2];
state->updated |= att_UPDATED | pos_UPDATED | vel_UPDATED | gyr_UPDATED; state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
// Advance the covariance estimate // Advance the covariance estimate
INSCovariancePrediction(dT); INSCovariancePrediction(dT);
if (ISSET(work.updated, mag_UPDATED)) { if (ISSET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS; sensors |= MAG_SENSORS;
} }
if (ISSET(work.updated, bar_UPDATED)) { if (ISSET(this->work.updated, SENSORUPDATES_baro)) {
sensors |= BARO_SENSOR; sensors |= BARO_SENSOR;
} }
INSSetMagNorth(homeLocation.Be); INSSetMagNorth(this->homeLocation.Be);
if (!usePos) { if (!this->usePos) {
// position and velocity variance used in indoor mode // position and velocity variance used in indoor mode
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], (float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] } this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] }
); );
} else { } else {
// position and velocity variance used in outdoor mode // position and velocity variance used in outdoor mode
INSSetPosVelVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], INSSetPosVelVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
(float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], (float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] } this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] }
); );
} }
if (ISSET(work.updated, pos_UPDATED)) { if (ISSET(this->work.updated, SENSORUPDATES_pos)) {
sensors |= POS_SENSORS; sensors |= POS_SENSORS;
} }
if (ISSET(work.updated, vel_UPDATED)) { if (ISSET(this->work.updated, SENSORUPDATES_vel)) {
sensors |= HORIZ_SENSORS | VERT_SENSORS; sensors |= HORIZ_SENSORS | VERT_SENSORS;
} }
if (ISSET(work.updated, air_UPDATED) && ((!ISSET(work.updated, vel_UPDATED) && !ISSET(work.updated, pos_UPDATED)) | !usePos)) { if (ISSET(this->work.updated, SENSORUPDATES_airspeed) && ((!ISSET(this->work.updated, SENSORUPDATES_vel) && !ISSET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) {
// HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance // HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS; sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], (float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] } this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] }
); );
// rotate airspeed vector into NED frame - airspeed is measured in X axis only // rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3]; float R[3][3];
Quaternion2R(Nav.q, R); Quaternion2R(Nav.q, R);
float vtas[3] = { work.air[1], 0.0f, 0.0f }; float vtas[3] = { this->work.airspeed[1], 0.0f, 0.0f };
rot_mult(R, vtas, work.vel); rot_mult(R, vtas, this->work.vel);
} }
/* /*
@ -405,7 +422,7 @@ static int32_t filter(stateEstimation *state)
* although probably should occur within INS itself * although probably should occur within INS itself
*/ */
if (sensors) { if (sensors) {
INSCorrection(work.mag, work.pos, work.vel, work.bar[0], sensors); INSCorrection(this->work.mag, this->work.pos, this->work.vel, this->work.baro[0], sensors);
} }
EKFStateVarianceData vardata; EKFStateVarianceData vardata;
@ -413,22 +430,16 @@ static int32_t filter(stateEstimation *state)
INSGetP(vardata.P); INSGetP(vardata.P);
EKFStateVarianceSet(&vardata); EKFStateVarianceSet(&vardata);
return 0; // all sensor data has been used, reset!
} this->work.updated = 0;
// check for invalid values return 0;
static inline bool invalid(float data)
{
if (isnan(data) || isinf(data)) {
return true;
}
return false;
} }
// check for invalid variance values // check for invalid variance values
static inline bool invalid_var(float data) static inline bool invalid_var(float data)
{ {
if (invalid(data)) { if (isnan(data) || isinf(data)) {
return true; return true;
} }
if (data < 1e-15f) { // var should not be close to zero. And not negative either. if (data < 1e-15f) { // var should not be close to zero. And not negative either.

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

@ -38,41 +38,50 @@
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
// Private constants // Private constants
//
#define STACK_REQUIRED 256
// Private types // Private types
struct data {
HomeLocationData homeLocation;
RevoCalibrationData revoCalibration;
float magBias[3];
};
// Private variables // Private variables
static HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration;
static float magBias[3] = { 0 };
// Private functions // Private functions
static int32_t init(void); static int32_t init(stateFilter *self);
static int32_t filter(stateEstimation *state); static int32_t filter(stateFilter *self, stateEstimation *state);
static void magOffsetEstimation(float mag[3]); static void magOffsetEstimation(struct data *this, float mag[3]);
void filterMagInitialize(stateFilter *handle) int32_t filterMagInitialize(stateFilter *handle)
{ {
handle->init = &init; handle->init = &init;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
} }
static int32_t init(void) static int32_t init(stateFilter *self)
{ {
magBias[0] = magBias[1] = magBias[2] = 0.0f; struct data *this = (struct data *)self->localdata;
HomeLocationGet(&homeLocation);
RevoCalibrationGet(&revoCalibration); this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
HomeLocationGet(&this->homeLocation);
RevoCalibrationGet(&this->revoCalibration);
return 0; return 0;
} }
static int32_t filter(stateEstimation *state) static int32_t filter(stateFilter *self, stateEstimation *state)
{ {
if (ISSET(state->updated, mag_UPDATED)) { struct data *this = (struct data *)self->localdata;
if (revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(state->mag); if (ISSET(state->updated, SENSORUPDATES_mag)) {
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
} }
} }
@ -84,7 +93,7 @@ static int32_t filter(stateEstimation *state)
* Magmeter Offset Cancellation: Theory and Implementation, * Magmeter Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011 * revisited William Premerlani, October 14, 2011
*/ */
static void magOffsetEstimation(float mag[3]) static void magOffsetEstimation(struct data *this, float mag[3])
{ {
#if 0 #if 0
// Constants, to possibly go into a UAVO // Constants, to possibly go into a UAVO
@ -127,10 +136,10 @@ static void magOffsetEstimation(float mag[3])
} }
#else // if 0 #else // if 0
const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); const float Rxy = sqrtf(this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1]);
const float Rz = homeLocation.Be[2]; const float Rz = this->homeLocation.Be[2];
const float rate = revoCalibration.MagBiasNullingRate; const float rate = this->revoCalibration.MagBiasNullingRate;
float Rot[3][3]; float Rot[3][3];
float B_e[3]; float B_e[3];
float xy[2]; float xy[2];
@ -162,15 +171,15 @@ static void magOffsetEstimation(float mag[3])
if (!isnan(delta[0]) && !isinf(delta[0]) && if (!isnan(delta[0]) && !isinf(delta[0]) &&
!isnan(delta[1]) && !isinf(delta[1]) && !isnan(delta[1]) && !isinf(delta[1]) &&
!isnan(delta[2]) && !isinf(delta[2])) { !isnan(delta[2]) && !isinf(delta[2])) {
magBias[0] += delta[0]; this->magBias[0] += delta[0];
magBias[1] += delta[1]; this->magBias[1] += delta[1];
magBias[2] += delta[2]; this->magBias[2] += delta[2];
} }
// Add bias to state estimation // Add bias to state estimation
mag[0] += magBias[0]; mag[0] += this->magBias[0];
mag[1] += magBias[1]; mag[1] += this->magBias[1];
mag[2] += magBias[2]; mag[2] += this->magBias[2];
#endif // if 0 #endif // if 0
} }

18
flight/modules/StateEstimation/filterstationary.c
View File

@ -33,38 +33,42 @@
// Private constants // Private constants
#define STACK_REQUIRED 64
// Private types // Private types
// Private variables // Private variables
// Private functions // Private functions
static int32_t init(void); static int32_t init(stateFilter *self);
static int32_t filter(stateEstimation *state); static int32_t filter(stateFilter *self, stateEstimation *state);
void filterStationaryInitialize(stateFilter *handle) int32_t filterStationaryInitialize(stateFilter *handle)
{ {
handle->init = &init; handle->init = &init;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = NULL;
return STACK_REQUIRED;
} }
static int32_t init(void) static int32_t init(__attribute__((unused)) stateFilter *self)
{ {
return 0; return 0;
} }
static int32_t filter(stateEstimation *state) static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
{ {
state->pos[0] = 0.0f; state->pos[0] = 0.0f;
state->pos[1] = 0.0f; state->pos[1] = 0.0f;
state->pos[2] = 0.0f; state->pos[2] = 0.0f;
state->updated |= pos_UPDATED; state->updated |= SENSORUPDATES_pos;
state->vel[0] = 0.0f; state->vel[0] = 0.0f;
state->vel[1] = 0.0f; state->vel[1] = 0.0f;
state->vel[2] = 0.0f; state->vel[2] = 0.0f;
state->updated |= vel_UPDATED; state->updated |= SENSORUPDATES_vel;
return 0; return 0;
} }

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

@ -42,7 +42,6 @@
#include <gyrostate.h> #include <gyrostate.h>
#include <accelstate.h> #include <accelstate.h>
#include <magstate.h> #include <magstate.h>
#include <barostate.h>
#include <airspeedstate.h> #include <airspeedstate.h>
#include <attitudestate.h> #include <attitudestate.h>
#include <positionstate.h> #include <positionstate.h>
@ -55,7 +54,7 @@
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
// Private constants // Private constants
#define STACK_SIZE_BYTES 2048 #define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define TIMEOUT_MS 100 #define TIMEOUT_MS 100
@ -185,7 +184,7 @@ static void StateEstimationCb(void);
static void getNED(GPSPositionData *gpsPosition, float *NED); static void getNED(GPSPositionData *gpsPosition, float *NED);
static bool sane(float value); static bool sane(float value);
static inline int32_t maxint32_t(int32_t a, int2_t b) static inline int32_t maxint32_t(int32_t a, int32_t b)
{ {
if (a > b) { if (a > b) {
return a; return a;
@ -212,7 +211,6 @@ int32_t StateEstimationInitialize(void)
GyroStateInitialize(); GyroStateInitialize();
AccelStateInitialize(); AccelStateInitialize();
MagStateInitialize(); MagStateInitialize();
BaroStateInitialize();
AirspeedStateInitialize(); AirspeedStateInitialize();
PositionStateInitialize(); PositionStateInitialize();
VelocityStateInitialize(); VelocityStateInitialize();
@ -316,7 +314,7 @@ static void StateEstimationCb(void)
current = (filterPipeline *)newFilterChain; current = (filterPipeline *)newFilterChain;
bool error = 0; bool error = 0;
while (current != NULL) { while (current != NULL) {
int32_t result = current->filter->init(); int32_t result = current->filter->init((stateFilter *)current->filter);
if (result != 0) { if (result != 0) {
error = 1; error = 1;
break; break;
@ -340,7 +338,7 @@ static void StateEstimationCb(void)
// most sensors get only rudimentary sanity checks // most sensors get only rudimentary sanity checks
#define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \ #define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \
if (ISSET(states.updated, shortname##_UPDATED)) { \ if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \ sensorname##Data s; \
sensorname##Get(&s); \ sensorname##Get(&s); \
if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \ if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \
@ -349,36 +347,36 @@ static void StateEstimationCb(void)
states.shortname[2] = s.a3; \ states.shortname[2] = s.a3; \
} \ } \
else { \ else { \
UNSET(states.updated, shortname##_UPDATED); \ UNSET(states.updated, SENSORUPDATES_##shortname); \
} \ } \
} }
SANITYCHECK3(GyroSensor, gyr, x, y, z); SANITYCHECK3(GyroSensor, gyro, x, y, z);
SANITYCHECK3(AccelSensor, acc, x, y, z); SANITYCHECK3(AccelSensor, accel, x, y, z);
SANITYCHECK3(MagSensor, mag, x, y, z); SANITYCHECK3(MagSensor, mag, x, y, z);
SANITYCHECK3(GPSVelocity, vel, North, East, Down); SANITYCHECK3(GPSVelocity, vel, North, East, Down);
#define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ #define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \
if (ISSET(states.updated, shortname##_UPDATED)) { \ if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \ sensorname##Data s; \
sensorname##Get(&s); \ sensorname##Get(&s); \
if (sane(s.a1) && EXTRACHECK) { \ if (sane(s.a1) && EXTRACHECK) { \
states.shortname[0] = s.a1; \ states.shortname[0] = s.a1; \
} \ } \
else { \ else { \
UNSET(states.updated, shortname##_UPDATED); \ UNSET(states.updated, SENSORUPDATES_##shortname); \
} \ } \
} }
SANITYCHECK1(BaroSensor, bar, Altitude, 1); SANITYCHECK1(BaroSensor, baro, Altitude, 1);
SANITYCHECK1(AirspeedSensor, air, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); SANITYCHECK1(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
states.air[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter 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 GPSPosition is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons // GPS is a tiny bit more tricky as GPSPosition is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons
if (ISSET(states.updated, pos_UPDATED)) { if (ISSET(states.updated, SENSORUPDATES_pos)) {
GPSPositionData s; GPSPositionData s;
GPSPositionGet(&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)) { 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); getNED(&s, states.pos);
} else { } else {
UNSET(states.updated, pos_UPDATED); UNSET(states.updated, SENSORUPDATES_pos);
} }
} }
@ -395,7 +393,7 @@ static void StateEstimationCb(void)
case RUNSTATE_FILTER: case RUNSTATE_FILTER:
if (current != NULL) { if (current != NULL) {
int32_t result = current->filter->filter(&states); int32_t result = current->filter->filter((stateFilter *)current->filter, &states);
if (result > alarm) { if (result > alarm) {
alarm = result; alarm = result;
} }
@ -413,7 +411,7 @@ static void StateEstimationCb(void)
// the final output of filters is saved in state variables // the final output of filters is saved in state variables
#define STORE3(statename, shortname, a1, a2, a3) \ #define STORE3(statename, shortname, a1, a2, a3) \
if (ISSET(states.updated, shortname##_UPDATED)) { \ if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
statename##Data s; \ statename##Data s; \
statename##Get(&s); \ statename##Get(&s); \
s.a1 = states.shortname[0]; \ s.a1 = states.shortname[0]; \
@ -421,36 +419,28 @@ static void StateEstimationCb(void)
s.a3 = states.shortname[2]; \ s.a3 = states.shortname[2]; \
statename##Set(&s); \ statename##Set(&s); \
} }
STORE3(GyroState, gyr, x, y, z); STORE3(GyroState, gyro, x, y, z);
STORE3(AccelState, acc, x, y, z); STORE3(AccelState, accel, x, y, z);
STORE3(MagState, mag, x, y, z); STORE3(MagState, mag, x, y, z);
STORE3(PositionState, pos, North, East, Down); STORE3(PositionState, pos, North, East, Down);
STORE3(VelocityState, vel, North, East, Down); STORE3(VelocityState, vel, North, East, Down);
#define STORE2(statename, shortname, a1, a2) \ #define STORE2(statename, shortname, a1, a2) \
if (ISSET(states.updated, shortname##_UPDATED)) { \ if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
statename##Data s; \ statename##Data s; \
statename##Get(&s); \ statename##Get(&s); \
s.a1 = states.shortname[0]; \ s.a1 = states.shortname[0]; \
s.a2 = states.shortname[1]; \ s.a2 = states.shortname[1]; \
statename##Set(&s); \ statename##Set(&s); \
} }
STORE2(AirspeedState, air, CalibratedAirspeed, TrueAirspeed); STORE2(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
#define STORE1(statename, shortname, a1) \
if (ISSET(states.updated, shortname##_UPDATED)) { \
statename##Data s; \
statename##Get(&s); \
s.a1 = states.shortname[0]; \
statename##Set(&s); \
}
STORE1(BaroState, bar, Altitude);
// attitude nees manual conversion from quaternion to euler // attitude nees manual conversion from quaternion to euler
if (ISSET(states.updated, att_UPDATED)) { \ if (ISSET(states.updated, SENSORUPDATES_attitude)) { \
AttitudeStateData s; AttitudeStateData s;
AttitudeStateGet(&s); AttitudeStateGet(&s);
s.q1 = states.att[0]; s.q1 = states.attitude[0];
s.q2 = states.att[1]; s.q2 = states.attitude[1];
s.q3 = states.att[2]; s.q3 = states.attitude[2];
s.q4 = states.att[3]; s.q4 = states.attitude[3];
Quaternion2RPY(&s.q1, &s.Roll); Quaternion2RPY(&s.q1, &s.Roll);
AttitudeStateSet(&s); AttitudeStateSet(&s);
} }
@ -528,31 +518,31 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
} }
if (ev->obj == GyroSensorHandle()) { if (ev->obj == GyroSensorHandle()) {
updatedSensors |= gyr_UPDATED; updatedSensors |= SENSORUPDATES_gyro;
} }
if (ev->obj == AccelSensorHandle()) { if (ev->obj == AccelSensorHandle()) {
updatedSensors |= acc_UPDATED; updatedSensors |= SENSORUPDATES_accel;
} }
if (ev->obj == MagSensorHandle()) { if (ev->obj == MagSensorHandle()) {
updatedSensors |= mag_UPDATED; updatedSensors |= SENSORUPDATES_mag;
} }
if (ev->obj == GPSPositionHandle()) { if (ev->obj == GPSPositionHandle()) {
updatedSensors |= pos_UPDATED; updatedSensors |= SENSORUPDATES_pos;
} }
if (ev->obj == GPSVelocityHandle()) { if (ev->obj == GPSVelocityHandle()) {
updatedSensors |= vel_UPDATED; updatedSensors |= SENSORUPDATES_vel;
} }
if (ev->obj == BaroSensorHandle()) { if (ev->obj == BaroSensorHandle()) {
updatedSensors |= bar_UPDATED; updatedSensors |= SENSORUPDATES_baro;
} }
if (ev->obj == AirspeedSensorHandle()) { if (ev->obj == AirspeedSensorHandle()) {
updatedSensors |= air_UPDATED; updatedSensors |= SENSORUPDATES_airspeed;
} }
DelayedCallbackDispatch(stateEstimationCallback); DelayedCallbackDispatch(stateEstimationCallback);

1
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -33,7 +33,6 @@ UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += barostate
UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedstate UAVOBJSRCFILENAMES += airspeedstate

1
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -38,7 +38,6 @@ UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += barostate
UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedstate UAVOBJSRCFILENAMES += airspeedstate

1
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -38,7 +38,6 @@ UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += barostate
UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedstate UAVOBJSRCFILENAMES += airspeedstate

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -25,7 +25,6 @@ OTHER_FILES += UAVObjects.pluginspec
# Add in all of the synthetic/generated uavobject files # Add in all of the synthetic/generated uavobject files
HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/barosensor.h \ $$UAVOBJECT_SYNTHETICS/barosensor.h \
$$UAVOBJECT_SYNTHETICS/barostate.h \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.h \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.h \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.h \
$$UAVOBJECT_SYNTHETICS/airspeedstate.h \ $$UAVOBJECT_SYNTHETICS/airspeedstate.h \
@ -110,7 +109,6 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/barosensor.cpp \ $$UAVOBJECT_SYNTHETICS/barosensor.cpp \
$$UAVOBJECT_SYNTHETICS/barostate.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \

10
shared/uavobjectdefinition/barostate.xml
View File

@ -1,10 +0,0 @@
<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>