forked from Archive/PX4-Autopilot
Merged estimator fixes and mavlink rate config bits
This commit is contained in:
Lorenz Meier
commit
28a31708f9
|
|
@ -428,11 +428,10 @@ then
|
|||
#
|
||||
sh /etc/init.d/rc.sensors
|
||||
|
||||
if [ $HIL == no ]
|
||||
then
|
||||
echo "[init] Start logging"
|
||||
sh /etc/init.d/rc.logging
|
||||
fi
|
||||
#
|
||||
# Start logging in all modes, including HIL
|
||||
#
|
||||
sh /etc/init.d/rc.logging
|
||||
|
||||
if [ $GPS == yes ]
|
||||
then
|
||||
|
|
|
|||
|
|
@ -544,7 +544,7 @@ void MPU6000::reset()
|
|||
write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
|
||||
irqrestore(state);
|
||||
|
||||
up_udelay(1000);
|
||||
usleep(1000);
|
||||
|
||||
// SAMPLE RATE
|
||||
_set_sample_rate(_sample_rate);
|
||||
|
|
|
|||
|
|
@ -1591,7 +1591,6 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
|
|||
switch (sp_man->mode_switch) {
|
||||
case SWITCH_POS_NONE:
|
||||
res = TRANSITION_NOT_CHANGED;
|
||||
warnx("NONE");
|
||||
break;
|
||||
|
||||
case SWITCH_POS_OFF: // MANUAL
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,247 @@
|
|||
#pragma once
|
||||
|
||||
#include "estimator_utilities.h"
|
||||
|
||||
class AttPosEKF {
|
||||
|
||||
public:
|
||||
|
||||
AttPosEKF();
|
||||
~AttPosEKF();
|
||||
|
||||
/* ##############################################
|
||||
*
|
||||
* M A I N F I L T E R P A R A M E T E R S
|
||||
*
|
||||
* ########################################### */
|
||||
|
||||
/*
|
||||
* parameters are defined here and initialised in
|
||||
* the InitialiseParameters() (which is just 20 lines down)
|
||||
*/
|
||||
|
||||
float covTimeStepMax; // maximum time allowed between covariance predictions
|
||||
float covDelAngMax; // maximum delta angle between covariance predictions
|
||||
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
|
||||
float yawVarScale;
|
||||
float windVelSigma;
|
||||
float dAngBiasSigma;
|
||||
float dVelBiasSigma;
|
||||
float magEarthSigma;
|
||||
float magBodySigma;
|
||||
float gndHgtSigma;
|
||||
|
||||
float vneSigma;
|
||||
float vdSigma;
|
||||
float posNeSigma;
|
||||
float posDSigma;
|
||||
float magMeasurementSigma;
|
||||
float airspeedMeasurementSigma;
|
||||
|
||||
float gyroProcessNoise;
|
||||
float accelProcessNoise;
|
||||
|
||||
float EAS2TAS; // ratio f true to equivalent airspeed
|
||||
|
||||
void InitialiseParameters()
|
||||
{
|
||||
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
|
||||
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
|
||||
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
EAS2TAS = 1.0f;
|
||||
|
||||
yawVarScale = 1.0f;
|
||||
windVelSigma = 0.1f;
|
||||
dAngBiasSigma = 5.0e-7f;
|
||||
dVelBiasSigma = 1e-4f;
|
||||
magEarthSigma = 3.0e-4f;
|
||||
magBodySigma = 3.0e-4f;
|
||||
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
|
||||
|
||||
vneSigma = 0.2f;
|
||||
vdSigma = 0.3f;
|
||||
posNeSigma = 2.0f;
|
||||
posDSigma = 2.0f;
|
||||
|
||||
magMeasurementSigma = 0.05;
|
||||
airspeedMeasurementSigma = 1.4f;
|
||||
gyroProcessNoise = 1.4544411e-2f;
|
||||
accelProcessNoise = 0.5f;
|
||||
}
|
||||
|
||||
// Global variables
|
||||
float KH[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float KHP[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float P[n_states][n_states]; // covariance matrix
|
||||
float Kfusion[n_states]; // Kalman gains
|
||||
float states[n_states]; // state matrix
|
||||
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
|
||||
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
|
||||
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
|
||||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
Vector3f dVelIMU;
|
||||
Vector3f dAngIMU;
|
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
|
||||
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
|
||||
float innovVelPos[6]; // innovation output
|
||||
float varInnovVelPos[6]; // innovation variance output
|
||||
|
||||
float velNED[3]; // North, East, Down velocity obs (m/s)
|
||||
float posNE[2]; // North, East position obs (m)
|
||||
float hgtMea; // measured height (m)
|
||||
float posNED[3]; // North, East Down position (m)
|
||||
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float innovVtas; // innovation output
|
||||
float varInnovVtas; // innovation variance output
|
||||
float VtasMeas; // true airspeed measurement (m/s)
|
||||
float magDeclination;
|
||||
float latRef; // WGS-84 latitude of reference point (rad)
|
||||
float lonRef; // WGS-84 longitude of reference point (rad)
|
||||
float hgtRef; // WGS-84 height of reference point (m)
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
|
||||
// GPS input data variables
|
||||
float gpsCourse;
|
||||
float gpsVelD;
|
||||
float gpsLat;
|
||||
float gpsLon;
|
||||
float gpsHgt;
|
||||
uint8_t GPSstatus;
|
||||
|
||||
// Baro input
|
||||
float baroHgt;
|
||||
|
||||
bool statesInitialised;
|
||||
|
||||
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
|
||||
bool fuseMagData; // boolean true when magnetometer data is to be fused
|
||||
bool fuseVtasData; // boolean true when airspeed data is to be fused
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
bool useAirspeed; ///< boolean true if airspeed data is being used
|
||||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
|
||||
struct ekf_status_report current_ekf_state;
|
||||
struct ekf_status_report last_ekf_error;
|
||||
|
||||
bool numericalProtection;
|
||||
|
||||
unsigned storeIndex;
|
||||
|
||||
|
||||
void UpdateStrapdownEquationsNED();
|
||||
|
||||
void CovariancePrediction(float dt);
|
||||
|
||||
void FuseVelposNED();
|
||||
|
||||
void FuseMagnetometer();
|
||||
|
||||
void FuseAirspeed();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
|
||||
|
||||
// store staes along with system time stamp in msces
|
||||
void StoreStates(uint64_t timestamp_ms);
|
||||
|
||||
/**
|
||||
* Recall the state vector.
|
||||
*
|
||||
* Recalls the vector stored at closest time to the one specified by msec
|
||||
*
|
||||
* @return zero on success, integer indicating the number of invalid states on failure.
|
||||
* Does only copy valid states, if the statesForFusion vector was initialized
|
||||
* correctly by the caller, the result can be safely used, but is a mixture
|
||||
* time-wise where valid states were updated and invalid remained at the old
|
||||
* value.
|
||||
*/
|
||||
int RecallStates(float statesForFusion[n_states], uint64_t msec);
|
||||
|
||||
void ResetStoredStates();
|
||||
|
||||
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
|
||||
|
||||
void calcEarthRateNED(Vector3f &omega, float latitude);
|
||||
|
||||
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
|
||||
|
||||
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
|
||||
|
||||
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
|
||||
|
||||
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
|
||||
|
||||
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
|
||||
|
||||
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
||||
|
||||
static float sq(float valIn);
|
||||
|
||||
void OnGroundCheck();
|
||||
|
||||
void CovarianceInit();
|
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
|
||||
|
||||
float ConstrainFloat(float val, float min, float max);
|
||||
|
||||
void ConstrainVariances();
|
||||
|
||||
void ConstrainStates();
|
||||
|
||||
void ForceSymmetry();
|
||||
|
||||
int CheckAndBound();
|
||||
|
||||
void ResetPosition();
|
||||
|
||||
void ResetVelocity();
|
||||
|
||||
void ZeroVariables();
|
||||
|
||||
void GetFilterState(struct ekf_status_report *state);
|
||||
|
||||
void GetLastErrorState(struct ekf_status_report *last_error);
|
||||
|
||||
bool StatesNaN(struct ekf_status_report *err_report);
|
||||
void FillErrorReport(struct ekf_status_report *err);
|
||||
|
||||
void InitializeDynamic(float (&initvelNED)[3], float declination);
|
||||
|
||||
protected:
|
||||
|
||||
bool FilterHealthy();
|
||||
|
||||
void ResetHeight(void);
|
||||
|
||||
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
|
||||
|
||||
};
|
||||
|
||||
uint32_t millis();
|
||||
|
||||
|
|
@ -1,143 +1,9 @@
|
|||
#include "estimator.h"
|
||||
#include "estimator_23states.h"
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
// Define EKF_DEBUG here to enable the debug print calls
|
||||
// if the macro is not set, these will be completely
|
||||
// optimized out by the compiler.
|
||||
//#define EKF_DEBUG
|
||||
|
||||
#ifdef EKF_DEBUG
|
||||
#include <stdio.h>
|
||||
|
||||
static void
|
||||
ekf_debug_print(const char *fmt, va_list args)
|
||||
{
|
||||
fprintf(stderr, "%s: ", "[ekf]");
|
||||
vfprintf(stderr, fmt, args);
|
||||
|
||||
fprintf(stderr, "\n");
|
||||
}
|
||||
|
||||
static void
|
||||
ekf_debug(const char *fmt, ...)
|
||||
{
|
||||
va_list args;
|
||||
|
||||
va_start(args, fmt);
|
||||
ekf_debug_print(fmt, args);
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
static void ekf_debug(const char *fmt, ...) { while(0){} }
|
||||
#endif
|
||||
|
||||
float Vector3f::length(void) const
|
||||
{
|
||||
return sqrt(x*x + y*y + z*z);
|
||||
}
|
||||
|
||||
void Vector3f::zero(void)
|
||||
{
|
||||
x = 0.0f;
|
||||
y = 0.0f;
|
||||
z = 0.0f;
|
||||
}
|
||||
|
||||
Mat3f::Mat3f() {
|
||||
identity();
|
||||
}
|
||||
|
||||
void Mat3f::identity() {
|
||||
x.x = 1.0f;
|
||||
x.y = 0.0f;
|
||||
x.z = 0.0f;
|
||||
|
||||
y.x = 0.0f;
|
||||
y.y = 1.0f;
|
||||
y.z = 0.0f;
|
||||
|
||||
z.x = 0.0f;
|
||||
z.y = 0.0f;
|
||||
z.z = 1.0f;
|
||||
}
|
||||
|
||||
Mat3f Mat3f::transpose(void) const
|
||||
{
|
||||
Mat3f ret = *this;
|
||||
swap_var(ret.x.y, ret.y.x);
|
||||
swap_var(ret.x.z, ret.z.x);
|
||||
swap_var(ret.y.z, ret.z.y);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// overload + operator to provide a vector addition
|
||||
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x + vecIn2.x;
|
||||
vecOut.y = vecIn1.y + vecIn2.y;
|
||||
vecOut.z = vecIn1.z + vecIn2.z;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload - operator to provide a vector subtraction
|
||||
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x - vecIn2.x;
|
||||
vecOut.y = vecIn1.y - vecIn2.y;
|
||||
vecOut.z = vecIn1.z - vecIn2.z;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a matrix vector product
|
||||
Vector3f operator*( Mat3f matIn, Vector3f vecIn)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
|
||||
vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
|
||||
vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload % operator to provide a vector cross product
|
||||
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
|
||||
vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
|
||||
vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a vector scaler product
|
||||
Vector3f operator*(Vector3f vecIn1, float sclIn1)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x * sclIn1;
|
||||
vecOut.y = vecIn1.y * sclIn1;
|
||||
vecOut.z = vecIn1.z * sclIn1;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a vector scaler product
|
||||
Vector3f operator*(float sclIn1, Vector3f vecIn1)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x * sclIn1;
|
||||
vecOut.y = vecIn1.y * sclIn1;
|
||||
vecOut.z = vecIn1.z * sclIn1;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
void swap_var(float &d1, float &d2)
|
||||
{
|
||||
float tmp = d1;
|
||||
d1 = d2;
|
||||
d2 = tmp;
|
||||
}
|
||||
#define EKF_COVARIANCE_DIVERGED 1.0e8f
|
||||
|
||||
AttPosEKF::AttPosEKF()
|
||||
|
||||
|
|
@ -145,7 +11,42 @@ AttPosEKF::AttPosEKF()
|
|||
* instead to allow clean in-air re-initialization.
|
||||
*/
|
||||
{
|
||||
summedDelAng.zero();
|
||||
summedDelVel.zero();
|
||||
|
||||
fusionModeGPS = 0;
|
||||
fuseVelData = false;
|
||||
fusePosData = false;
|
||||
fuseHgtData = false;
|
||||
fuseMagData = false;
|
||||
fuseVtasData = false;
|
||||
onGround = true;
|
||||
staticMode = true;
|
||||
useAirspeed = true;
|
||||
useCompass = true;
|
||||
useRangeFinder = true;
|
||||
numericalProtection = true;
|
||||
refSet = false;
|
||||
storeIndex = 0;
|
||||
gpsHgt = 0.0f;
|
||||
baroHgt = 0.0f;
|
||||
GPSstatus = 0;
|
||||
VtasMeas = 0.0f;
|
||||
magDeclination = 0.0f;
|
||||
dAngIMU.zero();
|
||||
dVelIMU.zero();
|
||||
velNED[0] = 0.0f;
|
||||
velNED[1] = 0.0f;
|
||||
velNED[2] = 0.0f;
|
||||
accelGPSNED[0] = 0.0f;
|
||||
accelGPSNED[1] = 0.0f;
|
||||
accelGPSNED[2] = 0.0f;
|
||||
delAngTotal.zero();
|
||||
ekfDiverged = false;
|
||||
lastReset = 0;
|
||||
|
||||
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
|
||||
memset(¤t_ekf_state, 0, sizeof(current_ekf_state));
|
||||
ZeroVariables();
|
||||
InitialiseParameters();
|
||||
}
|
||||
|
|
@ -181,6 +82,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
|
|||
dVelIMU.y = dVelIMU.y;
|
||||
dVelIMU.z = dVelIMU.z - states[13];
|
||||
|
||||
delAngTotal.x += correctedDelAng.x;
|
||||
delAngTotal.y += correctedDelAng.y;
|
||||
delAngTotal.z += correctedDelAng.z;
|
||||
|
||||
// Save current measurements
|
||||
Vector3f prevDelAng = correctedDelAng;
|
||||
|
||||
|
|
@ -199,8 +104,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
|
|||
}
|
||||
else
|
||||
{
|
||||
deltaQuat[0] = cosf(0.5f*rotationMag);
|
||||
float rotScaler = (sinf(0.5f*rotationMag))/rotationMag;
|
||||
// We are using double here as we are unsure how small
|
||||
// the angle differences are and if we get into numeric
|
||||
// issues with float. The runtime impact is not measurable
|
||||
// for these quantities.
|
||||
deltaQuat[0] = cos(0.5*(double)rotationMag);
|
||||
float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag;
|
||||
deltaQuat[1] = correctedDelAng.x*rotScaler;
|
||||
deltaQuat[2] = correctedDelAng.y*rotScaler;
|
||||
deltaQuat[3] = correctedDelAng.z*rotScaler;
|
||||
|
|
@ -312,7 +221,8 @@ void AttPosEKF::CovariancePrediction(float dt)
|
|||
float nextP[n_states][n_states];
|
||||
|
||||
// calculate covariance prediction process noise
|
||||
for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f;
|
||||
for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f;
|
||||
for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f;
|
||||
for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
|
||||
// scale gyro bias noise when on ground to allow for faster bias estimation
|
||||
for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
|
||||
|
|
@ -977,20 +887,20 @@ void AttPosEKF::CovariancePrediction(float dt)
|
|||
// propagate
|
||||
for (unsigned i = 0; i <= 13; i++) {
|
||||
P[i][i] = nextP[i][i];
|
||||
}
|
||||
|
||||
// force symmetry for observable states
|
||||
// force zero for non-observable states
|
||||
for (unsigned i = 1; i < n_states; i++)
|
||||
// force symmetry for observable states
|
||||
// force zero for non-observable states
|
||||
for (unsigned i = 1; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < i; j++)
|
||||
{
|
||||
for (uint8_t j = 0; j < i; j++)
|
||||
{
|
||||
if ((i > 13) || (j > 13)) {
|
||||
P[i][j] = 0.0f;
|
||||
} else {
|
||||
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
|
||||
}
|
||||
P[j][i] = P[i][j];
|
||||
if ((i > 13) || (j > 13)) {
|
||||
P[i][j] = 0.0f;
|
||||
} else {
|
||||
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
|
||||
}
|
||||
P[j][i] = P[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1020,9 +930,9 @@ void AttPosEKF::FuseVelposNED()
|
|||
{
|
||||
|
||||
// declare variables used by fault isolation logic
|
||||
uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure
|
||||
uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
|
||||
uint32_t hgtRetryTime = 5000; // height measurement retry time
|
||||
uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure
|
||||
uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available
|
||||
uint32_t hgtRetryTime = 500; // height measurement retry time
|
||||
uint32_t horizRetryTime;
|
||||
|
||||
// declare variables used to check measurement errors
|
||||
|
|
@ -1178,7 +1088,7 @@ void AttPosEKF::FuseVelposNED()
|
|||
stateIndex = 4 + obsIndex;
|
||||
// Calculate the measurement innovation, using states from a
|
||||
// different time coordinate if fusing height data
|
||||
if (obsIndex >= 0 && obsIndex <= 2)
|
||||
if (obsIndex <= 2)
|
||||
{
|
||||
innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
|
||||
}
|
||||
|
|
@ -1193,7 +1103,7 @@ void AttPosEKF::FuseVelposNED()
|
|||
// Calculate the Kalman Gain
|
||||
// Calculate innovation variances - also used for data logging
|
||||
varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
|
||||
SK = 1.0/varInnovVelPos[obsIndex];
|
||||
SK = 1.0/(double)varInnovVelPos[obsIndex];
|
||||
for (uint8_t i= 0; i<=indexLimit; i++)
|
||||
{
|
||||
Kfusion[i] = P[i][stateIndex]*SK;
|
||||
|
|
@ -1277,7 +1187,7 @@ void AttPosEKF::FuseMagnetometer()
|
|||
// data fit is the only assumption we can make
|
||||
// so we might as well take advantage of the computational efficiencies
|
||||
// associated with sequential fusion
|
||||
if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2))
|
||||
if (useCompass && fuseMagData && (obsIndex < 3))
|
||||
{
|
||||
// Limit range of states modified when on ground
|
||||
if(!onGround)
|
||||
|
|
@ -1293,7 +1203,7 @@ void AttPosEKF::FuseMagnetometer()
|
|||
// three prediction time steps.
|
||||
|
||||
// Calculate observation jacobians and Kalman gains
|
||||
if (fuseMagData)
|
||||
if (obsIndex == 0)
|
||||
{
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtMagMeasTime[0];
|
||||
|
|
@ -1388,11 +1298,6 @@ void AttPosEKF::FuseMagnetometer()
|
|||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
|
||||
varInnovMag[0] = 1.0f/SK_MX[0];
|
||||
innovMag[0] = MagPred[0] - magData.x;
|
||||
|
||||
// reset the observation index to 0 (we start by fusing the X
|
||||
// measurement)
|
||||
obsIndex = 0;
|
||||
fuseMagData = false;
|
||||
}
|
||||
else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
{
|
||||
|
|
@ -1508,7 +1413,7 @@ void AttPosEKF::FuseMagnetometer()
|
|||
}
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
|
||||
if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f)
|
||||
{
|
||||
// correct the state vector
|
||||
for (uint8_t j= 0; j < indexLimit; j++)
|
||||
|
|
@ -1517,7 +1422,7 @@ void AttPosEKF::FuseMagnetometer()
|
|||
}
|
||||
// normalise the quaternion states
|
||||
float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
|
||||
if (quatMag > 1e-12)
|
||||
if (quatMag > 1e-12f)
|
||||
{
|
||||
for (uint8_t j= 0; j<=3; j++)
|
||||
{
|
||||
|
|
@ -1612,7 +1517,7 @@ void AttPosEKF::FuseAirspeed()
|
|||
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
|
||||
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
|
||||
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
|
||||
|
||||
|
||||
float H_TAS[n_states];
|
||||
for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f;
|
||||
H_TAS[4] = SH_TAS[2];
|
||||
|
|
@ -1661,7 +1566,7 @@ void AttPosEKF::FuseAirspeed()
|
|||
// Calculate the measurement innovation
|
||||
innovVtas = VtasPred - VtasMeas;
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((innovVtas*innovVtas*SK_TAS) < 25.0)
|
||||
if ((innovVtas*innovVtas*SK_TAS) < 25.0f)
|
||||
{
|
||||
// correct the state vector
|
||||
for (uint8_t j=0; j <= 22; j++)
|
||||
|
|
@ -1758,7 +1663,7 @@ void AttPosEKF::FuseRangeFinder()
|
|||
|
||||
// Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data
|
||||
SH_RNG[4] = sin(rngFinderPitch);
|
||||
cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch);
|
||||
cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch);
|
||||
if (useRangeFinder && cosRngTilt > 0.87f)
|
||||
{
|
||||
// Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset
|
||||
|
|
@ -1855,21 +1760,21 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
|
|||
|
||||
int64_t bestTimeDelta = 200;
|
||||
unsigned bestStoreIndex = 0;
|
||||
for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
|
||||
for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++)
|
||||
{
|
||||
// Work around a GCC compiler bug - we know 64bit support on ARM is
|
||||
// sketchy in GCC.
|
||||
uint64_t timeDelta;
|
||||
|
||||
if (msec > statetimeStamp[storeIndex]) {
|
||||
timeDelta = msec - statetimeStamp[storeIndex];
|
||||
if (msec > statetimeStamp[storeIndexLocal]) {
|
||||
timeDelta = msec - statetimeStamp[storeIndexLocal];
|
||||
} else {
|
||||
timeDelta = statetimeStamp[storeIndex] - msec;
|
||||
timeDelta = statetimeStamp[storeIndexLocal] - msec;
|
||||
}
|
||||
|
||||
if (timeDelta < bestTimeDelta)
|
||||
if (timeDelta < (uint64_t)bestTimeDelta)
|
||||
{
|
||||
bestStoreIndex = storeIndex;
|
||||
bestStoreIndex = storeIndexLocal;
|
||||
bestTimeDelta = timeDelta;
|
||||
}
|
||||
}
|
||||
|
|
@ -1926,7 +1831,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
|
|||
Tnb.y.z = 2*(q23 + q01);
|
||||
}
|
||||
|
||||
void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
|
||||
void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4])
|
||||
{
|
||||
// Calculate the body to nav cosine matrix
|
||||
float q00 = sq(quat[0]);
|
||||
|
|
@ -1940,15 +1845,15 @@ void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
|
|||
float q13 = quat[1]*quat[3];
|
||||
float q23 = quat[2]*quat[3];
|
||||
|
||||
Tbn.x.x = q00 + q11 - q22 - q33;
|
||||
Tbn.y.y = q00 - q11 + q22 - q33;
|
||||
Tbn.z.z = q00 - q11 - q22 + q33;
|
||||
Tbn.x.y = 2*(q12 - q03);
|
||||
Tbn.x.z = 2*(q13 + q02);
|
||||
Tbn.y.x = 2*(q12 + q03);
|
||||
Tbn.y.z = 2*(q23 - q01);
|
||||
Tbn.z.x = 2*(q13 - q02);
|
||||
Tbn.z.y = 2*(q23 + q01);
|
||||
Tbn_ret.x.x = q00 + q11 - q22 - q33;
|
||||
Tbn_ret.y.y = q00 - q11 + q22 - q33;
|
||||
Tbn_ret.z.z = q00 - q11 - q22 + q33;
|
||||
Tbn_ret.x.y = 2*(q12 - q03);
|
||||
Tbn_ret.x.z = 2*(q13 + q02);
|
||||
Tbn_ret.y.x = 2*(q12 + q03);
|
||||
Tbn_ret.y.z = 2*(q23 - q01);
|
||||
Tbn_ret.z.x = 2*(q13 - q02);
|
||||
Tbn_ret.z.y = 2*(q23 + q01);
|
||||
}
|
||||
|
||||
void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
|
||||
|
|
@ -1979,17 +1884,17 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd,
|
|||
velNED[2] = gpsVelD;
|
||||
}
|
||||
|
||||
void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef)
|
||||
void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
|
||||
{
|
||||
posNED[0] = earthRadius * (lat - latRef);
|
||||
posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
|
||||
posNED[2] = -(hgt - hgtRef);
|
||||
posNED[0] = earthRadius * (lat - latReference);
|
||||
posNED[1] = earthRadius * cos(latReference) * (lon - lonReference);
|
||||
posNED[2] = -(hgt - hgtReference);
|
||||
}
|
||||
|
||||
void AttPosEKF::calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef)
|
||||
void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
|
||||
{
|
||||
lat = latRef + posNED[0] * earthRadiusInv;
|
||||
lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
|
||||
lat = latRef + (double)posNED[0] * earthRadiusInv;
|
||||
lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef);
|
||||
hgt = hgtRef - posNED[2];
|
||||
}
|
||||
|
||||
|
|
@ -2194,10 +2099,71 @@ void AttPosEKF::ForceSymmetry()
|
|||
{
|
||||
P[i][j] = 0.5f * (P[i][j] + P[j][i]);
|
||||
P[j][i] = P[i][j];
|
||||
|
||||
if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) ||
|
||||
(fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) {
|
||||
current_ekf_state.covariancesExcessive = true;
|
||||
current_ekf_state.error |= true;
|
||||
InitializeDynamic(velNED, magDeclination);
|
||||
return;
|
||||
}
|
||||
|
||||
float symmetric = 0.5f * (P[i][j] + P[j][i]);
|
||||
P[i][j] = symmetric;
|
||||
P[j][i] = symmetric;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool AttPosEKF::GyroOffsetsDiverged()
|
||||
{
|
||||
// Detect divergence by looking for rapid changes of the gyro offset
|
||||
Vector3f current_bias;
|
||||
current_bias.x = states[10];
|
||||
current_bias.y = states[11];
|
||||
current_bias.z = states[12];
|
||||
|
||||
Vector3f delta = current_bias - lastGyroOffset;
|
||||
float delta_len = delta.length();
|
||||
float delta_len_scaled = 0.0f;
|
||||
|
||||
// Protect against division by zero
|
||||
if (delta_len > 0.0f) {
|
||||
float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f);
|
||||
delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU;
|
||||
}
|
||||
|
||||
bool diverged = (delta_len_scaled > 1.0f);
|
||||
lastGyroOffset = current_bias;
|
||||
current_ekf_state.error |= diverged;
|
||||
current_ekf_state.gyroOffsetsExcessive = diverged;
|
||||
|
||||
return diverged;
|
||||
}
|
||||
|
||||
bool AttPosEKF::VelNEDDiverged()
|
||||
{
|
||||
Vector3f current_vel;
|
||||
current_vel.x = states[4];
|
||||
current_vel.y = states[5];
|
||||
current_vel.z = states[6];
|
||||
|
||||
Vector3f gps_vel;
|
||||
gps_vel.x = velNED[0];
|
||||
gps_vel.y = velNED[1];
|
||||
gps_vel.z = velNED[2];
|
||||
|
||||
Vector3f delta = current_vel - gps_vel;
|
||||
float delta_len = delta.length();
|
||||
|
||||
bool excessive = (delta_len > 20.0f);
|
||||
|
||||
current_ekf_state.error |= excessive;
|
||||
current_ekf_state.velOffsetExcessive = excessive;
|
||||
|
||||
return excessive;
|
||||
}
|
||||
|
||||
bool AttPosEKF::FilterHealthy()
|
||||
{
|
||||
if (!statesInitialised) {
|
||||
|
|
@ -2262,42 +2228,26 @@ void AttPosEKF::ResetVelocity(void)
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
|
||||
{
|
||||
for (unsigned i = 0; i < n_states; i++)
|
||||
{
|
||||
err->states[i] = states[i];
|
||||
}
|
||||
|
||||
err->velHealth = current_ekf_state.velHealth;
|
||||
err->posHealth = current_ekf_state.posHealth;
|
||||
err->hgtHealth = current_ekf_state.hgtHealth;
|
||||
err->velTimeout = current_ekf_state.velTimeout;
|
||||
err->posTimeout = current_ekf_state.posTimeout;
|
||||
err->hgtTimeout = current_ekf_state.hgtTimeout;
|
||||
}
|
||||
|
||||
bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
|
||||
bool AttPosEKF::StatesNaN() {
|
||||
bool err = false;
|
||||
|
||||
// check all integrators
|
||||
if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) {
|
||||
err_report->statesNaN = true;
|
||||
current_ekf_state.angNaN = true;
|
||||
ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
|
||||
err = true;
|
||||
goto out;
|
||||
} // delta angles
|
||||
|
||||
if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) {
|
||||
err_report->statesNaN = true;
|
||||
current_ekf_state.angNaN = true;
|
||||
ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
|
||||
err = true;
|
||||
goto out;
|
||||
} // delta angles
|
||||
|
||||
if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) {
|
||||
err_report->statesNaN = true;
|
||||
current_ekf_state.summedDelVelNaN = true;
|
||||
ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
|
||||
err = true;
|
||||
goto out;
|
||||
|
|
@ -2308,7 +2258,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
|
|||
for (unsigned j = 0; j < n_states; j++) {
|
||||
if (!isfinite(KH[i][j])) {
|
||||
|
||||
err_report->covarianceNaN = true;
|
||||
current_ekf_state.KHNaN = true;
|
||||
err = true;
|
||||
ekf_debug("KH NaN");
|
||||
goto out;
|
||||
|
|
@ -2316,7 +2266,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
|
|||
|
||||
if (!isfinite(KHP[i][j])) {
|
||||
|
||||
err_report->covarianceNaN = true;
|
||||
current_ekf_state.KHPNaN = true;
|
||||
err = true;
|
||||
ekf_debug("KHP NaN");
|
||||
goto out;
|
||||
|
|
@ -2324,7 +2274,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
|
|||
|
||||
if (!isfinite(P[i][j])) {
|
||||
|
||||
err_report->covarianceNaN = true;
|
||||
current_ekf_state.covarianceNaN = true;
|
||||
err = true;
|
||||
ekf_debug("P NaN");
|
||||
} // covariance matrix
|
||||
|
|
@ -2332,7 +2282,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
|
|||
|
||||
if (!isfinite(Kfusion[i])) {
|
||||
|
||||
err_report->kalmanGainsNaN = true;
|
||||
current_ekf_state.kalmanGainsNaN = true;
|
||||
ekf_debug("Kfusion NaN");
|
||||
err = true;
|
||||
goto out;
|
||||
|
|
@ -2340,7 +2290,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
|
|||
|
||||
if (!isfinite(states[i])) {
|
||||
|
||||
err_report->statesNaN = true;
|
||||
current_ekf_state.statesNaN = true;
|
||||
ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);
|
||||
err = true;
|
||||
goto out;
|
||||
|
|
@ -2349,7 +2299,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
|
|||
|
||||
out:
|
||||
if (err) {
|
||||
FillErrorReport(err_report);
|
||||
current_ekf_state.error |= true;
|
||||
}
|
||||
|
||||
return err;
|
||||
|
|
@ -2365,47 +2315,105 @@ out:
|
|||
* updated, but before any of the fusion steps are
|
||||
* executed.
|
||||
*/
|
||||
int AttPosEKF::CheckAndBound()
|
||||
int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
|
||||
{
|
||||
|
||||
// Store the old filter state
|
||||
bool currStaticMode = staticMode;
|
||||
|
||||
// Limit reset rate to 5 Hz to allow the filter
|
||||
// to settle
|
||||
if (millis() - lastReset < 200) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (ekfDiverged) {
|
||||
ekfDiverged = false;
|
||||
}
|
||||
|
||||
int ret = 0;
|
||||
|
||||
// Check if we're on ground - this also sets static mode.
|
||||
OnGroundCheck();
|
||||
|
||||
// Reset the filter if the states went NaN
|
||||
if (StatesNaN(&last_ekf_error)) {
|
||||
if (StatesNaN()) {
|
||||
ekf_debug("re-initializing dynamic");
|
||||
|
||||
InitializeDynamic(velNED, magDeclination);
|
||||
// Reset and fill error report
|
||||
InitializeDynamic(velNED, magDeclination);
|
||||
|
||||
return 1;
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
// Reset the filter if the IMU data is too old
|
||||
if (dtIMU > 0.3f) {
|
||||
|
||||
current_ekf_state.imuTimeout = true;
|
||||
|
||||
// Fill error report
|
||||
GetFilterState(&last_ekf_error);
|
||||
|
||||
ResetVelocity();
|
||||
ResetPosition();
|
||||
ResetHeight();
|
||||
ResetStoredStates();
|
||||
|
||||
// that's all we can do here, return
|
||||
return 2;
|
||||
}
|
||||
// Timeout cleared with this reset
|
||||
current_ekf_state.imuTimeout = false;
|
||||
|
||||
// Check if we're on ground - this also sets static mode.
|
||||
OnGroundCheck();
|
||||
// that's all we can do here, return
|
||||
ret = 2;
|
||||
}
|
||||
|
||||
// Check if we switched between states
|
||||
if (currStaticMode != staticMode) {
|
||||
// Fill error report, but not setting error flag
|
||||
GetFilterState(&last_ekf_error);
|
||||
|
||||
ResetVelocity();
|
||||
ResetPosition();
|
||||
ResetHeight();
|
||||
ResetStoredStates();
|
||||
|
||||
return 3;
|
||||
ret = 3;
|
||||
}
|
||||
|
||||
return 0;
|
||||
// Reset the filter if gyro offsets are excessive
|
||||
if (GyroOffsetsDiverged()) {
|
||||
|
||||
// Reset and fill error report
|
||||
InitializeDynamic(velNED, magDeclination);
|
||||
|
||||
// that's all we can do here, return
|
||||
ret = 4;
|
||||
}
|
||||
|
||||
// Reset the filter if it diverges too far from GPS
|
||||
if (VelNEDDiverged()) {
|
||||
|
||||
// Reset and fill error report
|
||||
InitializeDynamic(velNED, magDeclination);
|
||||
|
||||
// that's all we can do here, return
|
||||
ret = 5;
|
||||
}
|
||||
|
||||
// The excessive covariance detection already
|
||||
// reset the filter. Just need to report here.
|
||||
if (last_ekf_error.covariancesExcessive) {
|
||||
ret = 6;
|
||||
}
|
||||
|
||||
if (ret) {
|
||||
ekfDiverged = true;
|
||||
lastReset = millis();
|
||||
|
||||
// This reads the last error and clears it
|
||||
GetLastErrorState(last_error);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
|
||||
|
|
@ -2456,6 +2464,30 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
|
|||
|
||||
void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
|
||||
{
|
||||
if (current_ekf_state.error) {
|
||||
GetFilterState(&last_ekf_error);
|
||||
}
|
||||
|
||||
ZeroVariables();
|
||||
|
||||
// Reset error states
|
||||
current_ekf_state.error = false;
|
||||
current_ekf_state.angNaN = false;
|
||||
current_ekf_state.summedDelVelNaN = false;
|
||||
current_ekf_state.KHNaN = false;
|
||||
current_ekf_state.KHPNaN = false;
|
||||
current_ekf_state.PNaN = false;
|
||||
current_ekf_state.covarianceNaN = false;
|
||||
current_ekf_state.kalmanGainsNaN = false;
|
||||
current_ekf_state.statesNaN = false;
|
||||
|
||||
current_ekf_state.velHealth = true;
|
||||
//current_ekf_state.posHealth = ?;
|
||||
//current_ekf_state.hgtHealth = ?;
|
||||
|
||||
current_ekf_state.velTimeout = false;
|
||||
//current_ekf_state.posTimeout = ?;
|
||||
//current_ekf_state.hgtTimeout = ?;
|
||||
|
||||
// Fill variables with valid data
|
||||
velNED[0] = initvelNED[0];
|
||||
|
|
@ -2494,7 +2526,11 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
|
|||
// write to state vector
|
||||
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
|
||||
for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
|
||||
for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel
|
||||
// positions:
|
||||
states[7] = posNE[0];
|
||||
states[8] = posNE[1];
|
||||
states[9] = -hgtMea;
|
||||
for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
|
||||
states[16] = initMagNED.x; // Magnetic Field North
|
||||
states[17] = initMagNED.y; // Magnetic Field East
|
||||
states[18] = initMagNED.z; // Magnetic Field Down
|
||||
|
|
@ -2525,14 +2561,16 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
|
|||
hgtRef = referenceHgt;
|
||||
refSet = true;
|
||||
|
||||
// we are at reference altitude, so measurement must be zero
|
||||
hgtMea = 0.0f;
|
||||
// we are at reference position, so measurement must be zero
|
||||
posNE[0] = 0.0f;
|
||||
posNE[1] = 0.0f;
|
||||
|
||||
// we are at an unknown, possibly non-zero altitude - so altitude
|
||||
// is not reset (hgtMea)
|
||||
|
||||
// the baro offset must be this difference now
|
||||
baroHgtOffset = baroHgt - referenceHgt;
|
||||
|
||||
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
|
||||
|
||||
InitializeDynamic(initvelNED, declination);
|
||||
}
|
||||
|
||||
|
|
@ -2540,27 +2578,8 @@ void AttPosEKF::ZeroVariables()
|
|||
{
|
||||
|
||||
// Initialize on-init initialized variables
|
||||
fusionModeGPS = 0;
|
||||
covSkipCount = 0;
|
||||
statesInitialised = false;
|
||||
fuseVelData = false;
|
||||
fusePosData = false;
|
||||
fuseHgtData = false;
|
||||
fuseMagData = false;
|
||||
fuseVtasData = false;
|
||||
onGround = true;
|
||||
staticMode = true;
|
||||
useAirspeed = true;
|
||||
useCompass = true;
|
||||
useRangeFinder = true;
|
||||
numericalProtection = true;
|
||||
refSet = false;
|
||||
|
||||
storeIndex = 0;
|
||||
gpsHgt = 0.0f;
|
||||
baroHgt = 0.0f;
|
||||
GPSstatus = 0;
|
||||
VtasMeas = 0.0f;
|
||||
magDeclination = 0.0f;
|
||||
|
||||
// Do the data structure init
|
||||
for (unsigned i = 0; i < n_states; i++) {
|
||||
|
|
@ -2577,9 +2596,7 @@ void AttPosEKF::ZeroVariables()
|
|||
correctedDelAng.zero();
|
||||
summedDelAng.zero();
|
||||
summedDelVel.zero();
|
||||
|
||||
dAngIMU.zero();
|
||||
dVelIMU.zero();
|
||||
lastGyroOffset.zero();
|
||||
|
||||
for (unsigned i = 0; i < data_buffer_size; i++) {
|
||||
|
||||
|
|
@ -2598,12 +2615,27 @@ void AttPosEKF::ZeroVariables()
|
|||
|
||||
}
|
||||
|
||||
void AttPosEKF::GetFilterState(struct ekf_status_report *state)
|
||||
void AttPosEKF::GetFilterState(struct ekf_status_report *err)
|
||||
{
|
||||
memcpy(state, ¤t_ekf_state, sizeof(*state));
|
||||
|
||||
// Copy states
|
||||
for (unsigned i = 0; i < n_states; i++) {
|
||||
current_ekf_state.states[i] = states[i];
|
||||
}
|
||||
current_ekf_state.n_states = n_states;
|
||||
|
||||
memcpy(err, ¤t_ekf_state, sizeof(*err));
|
||||
|
||||
// err->velHealth = current_ekf_state.velHealth;
|
||||
// err->posHealth = current_ekf_state.posHealth;
|
||||
// err->hgtHealth = current_ekf_state.hgtHealth;
|
||||
// err->velTimeout = current_ekf_state.velTimeout;
|
||||
// err->posTimeout = current_ekf_state.posTimeout;
|
||||
// err->hgtTimeout = current_ekf_state.hgtTimeout;
|
||||
}
|
||||
|
||||
void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
|
||||
{
|
||||
memcpy(last_error, &last_ekf_error, sizeof(*last_error));
|
||||
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
|
||||
}
|
||||
|
|
@ -1,76 +1,10 @@
|
|||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#pragma once
|
||||
|
||||
#define GRAVITY_MSS 9.80665f
|
||||
#define deg2rad 0.017453292f
|
||||
#define rad2deg 57.295780f
|
||||
#define pi 3.141592657f
|
||||
#define earthRate 0.000072921f
|
||||
#define earthRadius 6378145.0f
|
||||
#define earthRadiusInv 1.5678540e-7f
|
||||
|
||||
class Vector3f
|
||||
{
|
||||
private:
|
||||
public:
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
|
||||
float length(void) const;
|
||||
void zero(void);
|
||||
};
|
||||
|
||||
class Mat3f
|
||||
{
|
||||
private:
|
||||
public:
|
||||
Vector3f x;
|
||||
Vector3f y;
|
||||
Vector3f z;
|
||||
|
||||
Mat3f();
|
||||
|
||||
void identity();
|
||||
Mat3f transpose(void) const;
|
||||
};
|
||||
|
||||
Vector3f operator*(float sclIn1, Vector3f vecIn1);
|
||||
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
|
||||
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
|
||||
Vector3f operator*( Mat3f matIn, Vector3f vecIn);
|
||||
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
|
||||
Vector3f operator*(Vector3f vecIn1, float sclIn1);
|
||||
|
||||
void swap_var(float &d1, float &d2);
|
||||
#include "estimator_utilities.h"
|
||||
|
||||
const unsigned int n_states = 23;
|
||||
const unsigned int data_buffer_size = 50;
|
||||
|
||||
enum GPS_FIX {
|
||||
GPS_FIX_NOFIX = 0,
|
||||
GPS_FIX_2D = 2,
|
||||
GPS_FIX_3D = 3
|
||||
};
|
||||
|
||||
struct ekf_status_report {
|
||||
bool velHealth;
|
||||
bool posHealth;
|
||||
bool hgtHealth;
|
||||
bool velTimeout;
|
||||
bool posTimeout;
|
||||
bool hgtTimeout;
|
||||
uint32_t velFailTime;
|
||||
uint32_t posFailTime;
|
||||
uint32_t hgtFailTime;
|
||||
float states[n_states];
|
||||
bool statesNaN;
|
||||
bool covarianceNaN;
|
||||
bool kalmanGainsNaN;
|
||||
};
|
||||
|
||||
class AttPosEKF {
|
||||
|
||||
public:
|
||||
|
|
@ -141,7 +75,7 @@ public:
|
|||
accelProcessNoise = 0.5f;
|
||||
}
|
||||
|
||||
struct {
|
||||
struct mag_state_struct {
|
||||
unsigned obsIndex;
|
||||
float MagPred[3];
|
||||
float SH_MAG[9];
|
||||
|
|
@ -157,7 +91,12 @@ public:
|
|||
float magZbias;
|
||||
float R_MAG;
|
||||
Mat3f DCM;
|
||||
} magstate;
|
||||
};
|
||||
|
||||
struct mag_state_struct magstate;
|
||||
struct mag_state_struct resetMagState;
|
||||
|
||||
|
||||
|
||||
|
||||
// Global variables
|
||||
|
|
@ -166,6 +105,7 @@ public:
|
|||
float P[n_states][n_states]; // covariance matrix
|
||||
float Kfusion[n_states]; // Kalman gains
|
||||
float states[n_states]; // state matrix
|
||||
float resetStates[n_states];
|
||||
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
|
||||
|
|
@ -183,6 +123,8 @@ public:
|
|||
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
|
||||
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
|
||||
Vector3f lastGyroOffset; // Last gyro offset
|
||||
Vector3f delAngTotal;
|
||||
|
||||
Mat3f Tbn; // transformation matrix from body to NED coordinates
|
||||
Mat3f Tnb; // transformation amtrix from NED to body coordinates
|
||||
|
|
@ -196,11 +138,11 @@ public:
|
|||
float varInnovVelPos[6]; // innovation variance output
|
||||
|
||||
float velNED[3]; // North, East, Down velocity obs (m/s)
|
||||
float accelGPSNED[3]; // Acceleration predicted by GPS in earth frame
|
||||
float posNE[2]; // North, East position obs (m)
|
||||
float hgtMea; // measured height (m)
|
||||
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
|
||||
float rngMea; // Ground distance
|
||||
float posNED[3]; // North, East Down position (m)
|
||||
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
|
|
@ -243,6 +185,9 @@ public:
|
|||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
bool useRangeFinder; ///< true when rangefinder is being used
|
||||
|
||||
bool ekfDiverged;
|
||||
uint64_t lastReset;
|
||||
|
||||
struct ekf_status_report current_ekf_state;
|
||||
struct ekf_status_report last_ekf_error;
|
||||
|
||||
|
|
@ -299,9 +244,9 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]);
|
|||
|
||||
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
|
||||
|
||||
static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
|
||||
void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
|
||||
|
||||
static void calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef);
|
||||
static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
|
||||
|
||||
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
||||
|
||||
|
|
@ -321,7 +266,7 @@ void ConstrainStates();
|
|||
|
||||
void ForceSymmetry();
|
||||
|
||||
int CheckAndBound();
|
||||
int CheckAndBound(struct ekf_status_report *last_error);
|
||||
|
||||
void ResetPosition();
|
||||
|
||||
|
|
@ -333,8 +278,7 @@ void GetFilterState(struct ekf_status_report *state);
|
|||
|
||||
void GetLastErrorState(struct ekf_status_report *last_error);
|
||||
|
||||
bool StatesNaN(struct ekf_status_report *err_report);
|
||||
void FillErrorReport(struct ekf_status_report *err);
|
||||
bool StatesNaN();
|
||||
|
||||
void InitializeDynamic(float (&initvelNED)[3], float declination);
|
||||
|
||||
|
|
@ -342,6 +286,10 @@ protected:
|
|||
|
||||
bool FilterHealthy();
|
||||
|
||||
bool GyroOffsetsDiverged();
|
||||
|
||||
bool VelNEDDiverged();
|
||||
|
||||
void ResetHeight(void);
|
||||
|
||||
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
|
||||
|
|
@ -0,0 +1,139 @@
|
|||
|
||||
#include "estimator_utilities.h"
|
||||
|
||||
// Define EKF_DEBUG here to enable the debug print calls
|
||||
// if the macro is not set, these will be completely
|
||||
// optimized out by the compiler.
|
||||
//#define EKF_DEBUG
|
||||
|
||||
#ifdef EKF_DEBUG
|
||||
#include <stdio.h>
|
||||
|
||||
static void
|
||||
ekf_debug_print(const char *fmt, va_list args)
|
||||
{
|
||||
fprintf(stderr, "%s: ", "[ekf]");
|
||||
vfprintf(stderr, fmt, args);
|
||||
|
||||
fprintf(stderr, "\n");
|
||||
}
|
||||
|
||||
void
|
||||
ekf_debug(const char *fmt, ...)
|
||||
{
|
||||
va_list args;
|
||||
|
||||
va_start(args, fmt);
|
||||
ekf_debug_print(fmt, args);
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
void ekf_debug(const char *fmt, ...) { while(0){} }
|
||||
#endif
|
||||
|
||||
float Vector3f::length(void) const
|
||||
{
|
||||
return sqrt(x*x + y*y + z*z);
|
||||
}
|
||||
|
||||
void Vector3f::zero(void)
|
||||
{
|
||||
x = 0.0f;
|
||||
y = 0.0f;
|
||||
z = 0.0f;
|
||||
}
|
||||
|
||||
Mat3f::Mat3f() {
|
||||
identity();
|
||||
}
|
||||
|
||||
void Mat3f::identity() {
|
||||
x.x = 1.0f;
|
||||
x.y = 0.0f;
|
||||
x.z = 0.0f;
|
||||
|
||||
y.x = 0.0f;
|
||||
y.y = 1.0f;
|
||||
y.z = 0.0f;
|
||||
|
||||
z.x = 0.0f;
|
||||
z.y = 0.0f;
|
||||
z.z = 1.0f;
|
||||
}
|
||||
|
||||
Mat3f Mat3f::transpose(void) const
|
||||
{
|
||||
Mat3f ret = *this;
|
||||
swap_var(ret.x.y, ret.y.x);
|
||||
swap_var(ret.x.z, ret.z.x);
|
||||
swap_var(ret.y.z, ret.z.y);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// overload + operator to provide a vector addition
|
||||
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x + vecIn2.x;
|
||||
vecOut.y = vecIn1.y + vecIn2.y;
|
||||
vecOut.z = vecIn1.z + vecIn2.z;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload - operator to provide a vector subtraction
|
||||
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x - vecIn2.x;
|
||||
vecOut.y = vecIn1.y - vecIn2.y;
|
||||
vecOut.z = vecIn1.z - vecIn2.z;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a matrix vector product
|
||||
Vector3f operator*( Mat3f matIn, Vector3f vecIn)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
|
||||
vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
|
||||
vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload % operator to provide a vector cross product
|
||||
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
|
||||
vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
|
||||
vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a vector scaler product
|
||||
Vector3f operator*(Vector3f vecIn1, float sclIn1)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x * sclIn1;
|
||||
vecOut.y = vecIn1.y * sclIn1;
|
||||
vecOut.z = vecIn1.z * sclIn1;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a vector scaler product
|
||||
Vector3f operator*(float sclIn1, Vector3f vecIn1)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x * sclIn1;
|
||||
vecOut.y = vecIn1.y * sclIn1;
|
||||
vecOut.z = vecIn1.z * sclIn1;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
void swap_var(float &d1, float &d2)
|
||||
{
|
||||
float tmp = d1;
|
||||
d1 = d2;
|
||||
d2 = tmp;
|
||||
}
|
||||
|
|
@ -0,0 +1,82 @@
|
|||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#pragma once
|
||||
|
||||
#define GRAVITY_MSS 9.80665f
|
||||
#define deg2rad 0.017453292f
|
||||
#define rad2deg 57.295780f
|
||||
#define pi 3.141592657f
|
||||
#define earthRate 0.000072921f
|
||||
#define earthRadius 6378145.0
|
||||
#define earthRadiusInv 1.5678540e-7
|
||||
|
||||
class Vector3f
|
||||
{
|
||||
private:
|
||||
public:
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
|
||||
float length(void) const;
|
||||
void zero(void);
|
||||
};
|
||||
|
||||
class Mat3f
|
||||
{
|
||||
private:
|
||||
public:
|
||||
Vector3f x;
|
||||
Vector3f y;
|
||||
Vector3f z;
|
||||
|
||||
Mat3f();
|
||||
|
||||
void identity();
|
||||
Mat3f transpose(void) const;
|
||||
};
|
||||
|
||||
Vector3f operator*(float sclIn1, Vector3f vecIn1);
|
||||
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
|
||||
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
|
||||
Vector3f operator*( Mat3f matIn, Vector3f vecIn);
|
||||
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
|
||||
Vector3f operator*(Vector3f vecIn1, float sclIn1);
|
||||
|
||||
void swap_var(float &d1, float &d2);
|
||||
|
||||
enum GPS_FIX {
|
||||
GPS_FIX_NOFIX = 0,
|
||||
GPS_FIX_2D = 2,
|
||||
GPS_FIX_3D = 3
|
||||
};
|
||||
|
||||
struct ekf_status_report {
|
||||
bool error;
|
||||
bool velHealth;
|
||||
bool posHealth;
|
||||
bool hgtHealth;
|
||||
bool velTimeout;
|
||||
bool posTimeout;
|
||||
bool hgtTimeout;
|
||||
bool imuTimeout;
|
||||
uint32_t velFailTime;
|
||||
uint32_t posFailTime;
|
||||
uint32_t hgtFailTime;
|
||||
float states[32];
|
||||
unsigned n_states;
|
||||
bool angNaN;
|
||||
bool summedDelVelNaN;
|
||||
bool KHNaN;
|
||||
bool KHPNaN;
|
||||
bool PNaN;
|
||||
bool covarianceNaN;
|
||||
bool kalmanGainsNaN;
|
||||
bool statesNaN;
|
||||
bool gyroOffsetsExcessive;
|
||||
bool covariancesExcessive;
|
||||
bool velOffsetExcessive;
|
||||
};
|
||||
|
||||
void ekf_debug(const char *fmt, ...);
|
||||
|
|
@ -39,4 +39,5 @@ MODULE_COMMAND = ekf_att_pos_estimator
|
|||
|
||||
SRCS = ekf_att_pos_estimator_main.cpp \
|
||||
ekf_att_pos_estimator_params.c \
|
||||
estimator.cpp
|
||||
estimator_23states.cpp \
|
||||
estimator_utilities.cpp
|
||||
|
|
|
|||
|
|
@ -197,13 +197,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
|
|||
|
||||
if (buf_free < desired) {
|
||||
/* we don't want to send anything just in half, so return */
|
||||
instance->count_txerr();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t ret = write(uart, ch, desired);
|
||||
if (ret != desired) {
|
||||
// XXX overflow perf
|
||||
instance->count_txerr();
|
||||
} else {
|
||||
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
|
||||
}
|
||||
|
|
@ -249,7 +250,8 @@ Mavlink::Mavlink() :
|
|||
_param_use_hil_gps(0),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
|
||||
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
|
||||
{
|
||||
_wpm = &_wpm_s;
|
||||
mission.count = 0;
|
||||
|
|
@ -302,6 +304,7 @@ Mavlink::Mavlink() :
|
|||
Mavlink::~Mavlink()
|
||||
{
|
||||
perf_free(_loop_perf);
|
||||
perf_free(_txerr_perf);
|
||||
|
||||
if (_task_running) {
|
||||
/* task wakes up every 10ms or so at the longest */
|
||||
|
|
@ -326,6 +329,12 @@ Mavlink::~Mavlink()
|
|||
LL_DELETE(_mavlink_instances, this);
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::count_txerr()
|
||||
{
|
||||
perf_count(_txerr_perf);
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::set_mode(enum MAVLINK_MODE mode)
|
||||
{
|
||||
|
|
@ -2193,11 +2202,20 @@ int Mavlink::start_helper(int argc, char *argv[])
|
|||
/* create the instance in task context */
|
||||
Mavlink *instance = new Mavlink();
|
||||
|
||||
/* this will actually only return once MAVLink exits */
|
||||
int res = instance->task_main(argc, argv);
|
||||
int res;
|
||||
|
||||
/* delete instance on main thread end */
|
||||
delete instance;
|
||||
if (!instance) {
|
||||
|
||||
/* out of memory */
|
||||
res = -ENOMEM;
|
||||
warnx("OUT OF MEM");
|
||||
} else {
|
||||
/* this will actually only return once MAVLink exits */
|
||||
res = instance->task_main(argc, argv);
|
||||
|
||||
/* delete instance on main thread end */
|
||||
delete instance;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -215,10 +215,14 @@ public:
|
|||
|
||||
const mavlink_channel_t get_channel();
|
||||
|
||||
void configure_stream_threadsafe(const char *stream_name, const float rate);
|
||||
|
||||
bool _task_should_exit; /**< if true, mavlink task should exit */
|
||||
|
||||
int get_mavlink_fd() { return _mavlink_fd; }
|
||||
|
||||
MavlinkStream * get_streams() { return _streams; } const
|
||||
|
||||
|
||||
/* Functions for waiting to start transmission until message received. */
|
||||
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
|
||||
|
|
@ -232,6 +236,11 @@ public:
|
|||
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
|
||||
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
|
||||
|
||||
/**
|
||||
* Count a transmision error
|
||||
*/
|
||||
void count_txerr();
|
||||
|
||||
protected:
|
||||
Mavlink *next;
|
||||
|
||||
|
|
@ -303,6 +312,7 @@ private:
|
|||
pthread_mutex_t _message_buffer_mutex;
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
perf_counter_t _txerr_perf; /**< TX error counter */
|
||||
|
||||
bool _param_initialized;
|
||||
param_t _param_system_id;
|
||||
|
|
@ -371,7 +381,6 @@ private:
|
|||
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
|
||||
|
||||
int configure_stream(const char *stream_name, const float rate);
|
||||
void configure_stream_threadsafe(const char *stream_name, const float rate);
|
||||
|
||||
int message_buffer_init(int size);
|
||||
|
||||
|
|
|
|||
|
|
@ -232,6 +232,11 @@ public:
|
|||
return "HEARTBEAT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HEARTBEAT;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamHeartbeat();
|
||||
|
|
@ -292,6 +297,11 @@ public:
|
|||
return "SYS_STATUS";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_SYS_STATUS;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamSysStatus();
|
||||
|
|
@ -343,6 +353,11 @@ public:
|
|||
return "HIGHRES_IMU";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HIGHRES_IMU;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamHighresIMU();
|
||||
|
|
@ -428,6 +443,11 @@ public:
|
|||
return "ATTITUDE";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ATTITUDE;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamAttitude();
|
||||
|
|
@ -474,6 +494,11 @@ public:
|
|||
return "ATTITUDE_QUATERNION";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamAttitudeQuaternion();
|
||||
|
|
@ -526,6 +551,11 @@ public:
|
|||
return "VFR_HUD";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_VFR_HUD;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamVFRHUD();
|
||||
|
|
@ -609,6 +639,11 @@ public:
|
|||
return "GPS_RAW_INT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_GPS_RAW_INT;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamGPSRawInt();
|
||||
|
|
@ -662,6 +697,11 @@ public:
|
|||
return "GLOBAL_POSITION_INT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamGlobalPositionInt();
|
||||
|
|
@ -723,6 +763,11 @@ public:
|
|||
return "LOCAL_POSITION_NED";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamLocalPositionNED();
|
||||
|
|
@ -774,6 +819,11 @@ public:
|
|||
return "VICON_POSITION_ESTIMATE";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamViconPositionEstimate();
|
||||
|
|
@ -824,6 +874,11 @@ public:
|
|||
return "GPS_GLOBAL_ORIGIN";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamGPSGlobalOrigin();
|
||||
|
|
@ -864,6 +919,11 @@ public:
|
|||
return MavlinkStreamServoOutputRaw<N>::get_name_static();
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
|
||||
}
|
||||
|
||||
static const char *get_name_static()
|
||||
{
|
||||
switch (N) {
|
||||
|
|
@ -941,6 +1001,11 @@ public:
|
|||
return "HIL_CONTROLS";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HIL_CONTROLS;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamHILControls();
|
||||
|
|
@ -1078,6 +1143,11 @@ public:
|
|||
return "GLOBAL_POSITION_SETPOINT_INT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamGlobalPositionSetpointInt();
|
||||
|
|
@ -1121,6 +1191,11 @@ public:
|
|||
return "LOCAL_POSITION_SETPOINT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamLocalPositionSetpoint();
|
||||
|
|
@ -1169,6 +1244,11 @@ public:
|
|||
return "ROLL_PITCH_YAW_THRUST_SETPOINT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamRollPitchYawThrustSetpoint();
|
||||
|
|
@ -1217,6 +1297,11 @@ public:
|
|||
return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
|
||||
|
|
@ -1265,6 +1350,11 @@ public:
|
|||
return "RC_CHANNELS_RAW";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamRCChannelsRaw();
|
||||
|
|
@ -1349,6 +1439,11 @@ public:
|
|||
return "MANUAL_CONTROL";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_MANUAL_CONTROL;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamManualControl();
|
||||
|
|
@ -1398,6 +1493,11 @@ public:
|
|||
return "OPTICAL_FLOW";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_OPTICAL_FLOW;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamOpticalFlow();
|
||||
|
|
@ -1446,6 +1546,11 @@ public:
|
|||
return "ATTITUDE_CONTROLS";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamAttitudeControls();
|
||||
|
|
@ -1504,6 +1609,11 @@ public:
|
|||
return "NAMED_VALUE_FLOAT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamNamedValueFloat();
|
||||
|
|
@ -1552,6 +1662,11 @@ public:
|
|||
return "CAMERA_CAPTURE";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamCameraCapture();
|
||||
|
|
@ -1597,6 +1712,11 @@ public:
|
|||
return "DISTANCE_SENSOR";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_DISTANCE_SENSOR;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamDistanceSensor();
|
||||
|
|
|
|||
|
|
@ -159,6 +159,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
handle_message_heartbeat(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
|
||||
handle_message_request_data_stream(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
|
||||
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
|
||||
break;
|
||||
|
|
@ -498,6 +502,24 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_request_data_stream_t req;
|
||||
mavlink_msg_request_data_stream_decode(msg, &req);
|
||||
|
||||
if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
|
||||
float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
|
||||
|
||||
MavlinkStream *stream;
|
||||
LL_FOREACH(_mavlink->get_streams(), stream) {
|
||||
if (req.req_stream_id == stream->get_id()) {
|
||||
_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -115,6 +115,7 @@ private:
|
|||
void handle_message_radio_status(mavlink_message_t *msg);
|
||||
void handle_message_manual_control(mavlink_message_t *msg);
|
||||
void handle_message_heartbeat(mavlink_message_t *msg);
|
||||
void handle_message_request_data_stream(mavlink_message_t *msg);
|
||||
void handle_message_hil_sensor(mavlink_message_t *msg);
|
||||
void handle_message_hil_gps(mavlink_message_t *msg);
|
||||
void handle_message_hil_state_quaternion(mavlink_message_t *msg);
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ public:
|
|||
static const char *get_name_static();
|
||||
virtual void subscribe(Mavlink *mavlink) = 0;
|
||||
virtual const char *get_name() const = 0;
|
||||
virtual uint8_t get_id() = 0;
|
||||
|
||||
protected:
|
||||
mavlink_channel_t _channel;
|
||||
|
|
|
|||
|
|
@ -976,7 +976,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct log_BATT_s log_BATT;
|
||||
struct log_DIST_s log_DIST;
|
||||
struct log_TELE_s log_TELE;
|
||||
struct log_ESTM_s log_ESTM;
|
||||
struct log_EST0_s log_EST0;
|
||||
struct log_EST1_s log_EST1;
|
||||
struct log_PWR_s log_PWR;
|
||||
struct log_VICN_s log_VICN;
|
||||
struct log_GS0A_s log_GS0A;
|
||||
|
|
@ -1489,15 +1490,21 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
|
||||
/* --- ESTIMATOR STATUS --- */
|
||||
if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
|
||||
log_msg.msg_type = LOG_ESTM_MSG;
|
||||
unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s);
|
||||
memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s));
|
||||
memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy);
|
||||
log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states;
|
||||
log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan;
|
||||
log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan;
|
||||
log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan;
|
||||
LOGBUFFER_WRITE_AND_COUNT(ESTM);
|
||||
log_msg.msg_type = LOG_EST0_MSG;
|
||||
unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s);
|
||||
memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s));
|
||||
memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0);
|
||||
log_msg.body.log_EST0.n_states = buf.estimator_status.n_states;
|
||||
log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags;
|
||||
log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags;
|
||||
log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags;
|
||||
LOGBUFFER_WRITE_AND_COUNT(EST0);
|
||||
|
||||
log_msg.msg_type = LOG_EST1_MSG;
|
||||
unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s);
|
||||
memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s));
|
||||
memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1);
|
||||
LOGBUFFER_WRITE_AND_COUNT(EST1);
|
||||
}
|
||||
|
||||
/* --- TECS STATUS --- */
|
||||
|
|
|
|||
|
|
@ -288,15 +288,7 @@ struct log_TELE_s {
|
|||
uint8_t txbuf;
|
||||
};
|
||||
|
||||
/* --- ESTM - ESTIMATOR STATUS --- */
|
||||
#define LOG_ESTM_MSG 23
|
||||
struct log_ESTM_s {
|
||||
float s[10];
|
||||
uint8_t n_states;
|
||||
uint8_t states_nan;
|
||||
uint8_t covariance_nan;
|
||||
uint8_t kalman_gain_nan;
|
||||
};
|
||||
// ID 23 available
|
||||
|
||||
/* --- PWR - ONBOARD POWER SYSTEM --- */
|
||||
#define LOG_PWR_MSG 24
|
||||
|
|
@ -377,6 +369,22 @@ struct log_WIND_s {
|
|||
float cov_y;
|
||||
};
|
||||
|
||||
/* --- EST0 - ESTIMATOR STATUS --- */
|
||||
#define LOG_EST0_MSG 32
|
||||
struct log_EST0_s {
|
||||
float s[12];
|
||||
uint8_t n_states;
|
||||
uint8_t nan_flags;
|
||||
uint8_t health_flags;
|
||||
uint8_t timeout_flags;
|
||||
};
|
||||
|
||||
/* --- EST1 - ESTIMATOR STATUS --- */
|
||||
#define LOG_EST1_MSG 33
|
||||
struct log_EST1_s {
|
||||
float s[16];
|
||||
};
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
|
|
@ -425,7 +433,8 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
|
||||
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
|
||||
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
|
||||
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
|
||||
LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
|
||||
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
|
||||
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
|
||||
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
|
||||
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
|
|
|
|||
|
|
@ -64,9 +64,9 @@ struct estimator_status_report {
|
|||
uint64_t timestamp; /**< Timestamp in microseconds since boot */
|
||||
float states[32]; /**< Internal filter states */
|
||||
float n_states; /**< Number of states effectively used */
|
||||
bool states_nan; /**< If set to true, one of the states is NaN */
|
||||
bool covariance_nan; /**< If set to true, the covariance matrix went NaN */
|
||||
bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */
|
||||
uint8_t nan_flags; /**< Bitmask to indicate NaN states */
|
||||
uint8_t health_flags; /**< Bitmask to indicate sensor health states (vel, pos, hgt) */
|
||||
uint8_t timeout_flags; /**< Bitmask to indicate timeout flags (vel, pos, hgt) */
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue