fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 07:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

move geo and geo_lookup from PX4 Firmware to ECL

Browse Source
This commit is contained in:
Daniel Agar
2018-03-26 21:52:11 -04:00
parent 1bb4c17c0e
commit cd12f049fe
25 changed files with 392 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+4 -4
View File
@@ -40,10 +40,11 @@
*
*/
#include "../ecl.h"
#include "ekf.h"
#include <ecl.h>
#include <math.h>
#include "mathlib.h"
#include <mathlib/mathlib.h>
void Ekf::initialiseCovariance()
{
@@ -753,8 +754,7 @@ void Ekf::fixCovarianceErrors()
float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar,
sq(_gravity_mss * _dt_ekf_avg));
P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(CONSTANTS_ONE_G * _dt_ekf_avg));
}
// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero