From 0ea1ccf5c3df367978931b3c312daae8cc836530 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 1 May 2014 15:50:02 +0200 Subject: [PATCH] OP-1316 fix gravity constants --- flight/libraries/inc/insgps.h | 1 + flight/libraries/insgps13state.c | 9 ++++++++- flight/modules/StateEstimation/filteraltitude.c | 6 +++++- flight/modules/StateEstimation/filterekf.c | 1 + flight/modules/VtolPathFollower/vtolpathfollower.c | 5 ++++- 5 files changed, 19 insertions(+), 3 deletions(-) diff --git a/flight/libraries/inc/insgps.h b/flight/libraries/inc/insgps.h index c77d44510..b5736dea3 100644 --- a/flight/libraries/inc/insgps.h +++ b/flight/libraries/inc/insgps.h @@ -67,6 +67,7 @@ void INSSetAccelVar(float accel_var[3]); void INSSetGyroVar(float gyro_var[3]); void INSSetGyroBiasVar(float gyro_bias_var[3]); void INSSetMagNorth(float B[3]); +void INSSetG(float g_e); void INSSetMagVar(float scaled_mag_var[3]); void INSSetBaroVar(float baro_var); void INSPosVelReset(float pos[3], float vel[3]); diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index d014babb1..b65aa2bde 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -91,6 +91,8 @@ static struct EKFData { float H[NUMV][NUMX]; // local magnetic unit vector in NED frame float Be[3]; + // local gravity vector + float g_e; // covariance matrix and state vector float P[NUMX][NUMX]; float X[NUMX]; @@ -286,6 +288,11 @@ void INSSetMagNorth(float B[3]) ekf.Be[2] = B[2] / mag; } +void INSSetG(float g_e) +{ + ekf.g_e = g_e; +} + void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) { float U[6]; @@ -641,7 +648,7 @@ void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]) az; Xdot[5] = 2.0f * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay + - (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81f; + (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + ekf.g_e; // qdot = Q*w Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f; diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index eac70bcf9..6b32cc354 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -33,6 +33,7 @@ #include "inc/stateestimation.h" #include #include +#include #include @@ -65,6 +66,7 @@ struct data { bool first_run; portTickType initTimer; AltitudeFilterSettingsData settings; + float gravity; }; // Private variables @@ -81,6 +83,7 @@ int32_t filterAltitudeInitialize(stateFilter *handle) handle->init = &init; handle->filter = &filter; handle->localdata = pvPortMalloc(sizeof(struct data)); + HomeLocationInitialize(); AttitudeStateInitialize(); AltitudeFilterSettingsInitialize(); AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb); @@ -107,6 +110,7 @@ static int32_t init(stateFilter *self) this->baroLast = 0.0f; this->accelLast = 0.0f; this->first_run = 1; + HomeLocationg_eGet(&this->gravity); return 0; } @@ -145,7 +149,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) AttitudeStateGet(&att); float Rbe[3][3]; Quaternion2R(&att.q1, Rbe); - float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f); + float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + this->gravity); // low pass filter accelerometers this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current; diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 0aa906dda..3fbf72b32 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -356,6 +356,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) } INSSetMagNorth(this->homeLocation.Be); + INSSetG(this->homeLocation.g_e); if (!this->usePos) { // position and velocity variance used in indoor mode diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 3c65e1b29..00f3be6ea 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -140,6 +140,7 @@ int32_t VtolPathFollowerInitialize() AccessoryDesiredInitialize(); PoiLearnSettingsInitialize(); PoiLocationInitialize(); + HomeLocationInitialize(); vtolpathfollower_enabled = true; } else { vtolpathfollower_enabled = false; @@ -159,6 +160,7 @@ static float eastPosIntegral = 0; static float downPosIntegral = 0; static float thrustOffset = 0; +static float gravity; /** * Module thread, should not return. */ @@ -182,6 +184,7 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path + HomeLocationg_eGet(&gravity); SystemSettingsGet(&systemSettings); if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) @@ -682,7 +685,7 @@ static void updateNedAccel() accel_ned[i] += Rbe[j][i] * accel[j]; } } - accel_ned[2] += 9.81f; + accel_ned[2] += gravity; NedAccelData accelData; NedAccelGet(&accelData);