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

Merge branch 'corvuscorax/OP-1316_fix-gravity-constants' into next

Browse Source
This commit is contained in:
Corvus Corax 2014-05-04 12:07:22 +02:00
commit 64d18c1a31
5 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/libraries/inc/insgps.h
View File

@ -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]);

9
flight/libraries/insgps13state.c
View File

@ -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;

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

@ -33,6 +33,7 @@
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <altitudefiltersettings.h>
#include <homelocation.h>
#include <CoordinateConversions.h>
@ -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;

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

@ -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

5
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -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);