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

Added check for valid attitude data.

Browse Source
This commit is contained in:
James Goppert
2013-01-15 12:37:12 -05:00
parent 022c30ea4f
commit edf0a6bae7
2 changed files with 19 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/kalman_demo/KalmanNav.cpp
+12 -3
View File
@@ -177,7 +177,10 @@ void KalmanNav::update()
if (!(sensorsUpdate || gpsUpdate)) return; if (!(sensorsUpdate || gpsUpdate)) return;
// if received gps for first time, reset position to gps // if received gps for first time, reset position to gps
if (!_positionInitialized && gpsUpdate && _gps.fix_type > 2 && _gps.counter_pos_valid > 5) { if (!_positionInitialized &&
gpsUpdate &&
_gps.fix_type > 2 &&
_gps.counter_pos_valid > 10) {
vN = _gps.vel_n; vN = _gps.vel_n;
vE = _gps.vel_e; vE = _gps.vel_e;
vD = _gps.vel_d; vD = _gps.vel_d;
@@ -455,6 +458,12 @@ void KalmanNav::correctAtt()
{ {
using namespace math; using namespace math;
// check for valid data, return if not ready
if (_sensors.accelerometer_counter < 10 ||
_sensors.gyro_counter < 10 ||
_sensors.magnetometer_counter < 10)
return;
// trig // trig
float cosPhi = cosf(phi); float cosPhi = cosf(phi);
float cosTheta = cosf(theta); float cosTheta = cosf(theta);
@@ -566,7 +575,7 @@ void KalmanNav::correctAtt()
if (isnan(val) || isinf(val)) { if (isnan(val) || isinf(val)) {
// abort correction and return // abort correction and return
printf("[kalman_demo] numerical failure in att correction\n"); printf("[kalman_demo] numerical failure in att correction\n");
// reset P matrix to identity // reset P matrix to P0
P = P0; P = P0;
return; return;
} }
@@ -649,7 +658,7 @@ void KalmanNav::correctPos()
setLatDegE7(_gps.lat); setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon); setLonDegE7(_gps.lon);
setAltE3(_gps.alt); setAltE3(_gps.alt);
// reset P matrix to identity // reset P matrix to P0
P = P0; P = P0;
return; return;
} }
apps/examples/kalman_demo/params.c
+2 -2
View File
@@ -1,8 +1,8 @@
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f); PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);