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

LPE: comment out gps delay handling, too much memory required.

Browse Source
This commit is contained in:
James Goppert 2016-03-08 11:48:20 -05:00
parent d37ddb6102
commit 613ec40d86
3 changed files with 29 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -105,14 +105,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_gpsStats(this, ""),
// stats
_xDelay(this, ""),
_PDelay(this, ""),
_tDelay(this, ""),
//_xDelay(this, ""),
//_PDelay(this, ""),
//_tDelay(this, ""),
// misc
_polls(),
_timeStamp(hrt_absolute_time()),
_time_last_hist(0),
//_time_last_hist(0),
_time_last_xy(0),
_time_last_z(0),
_time_last_tz(0),
@ -1034,11 +1034,11 @@ void BlockLocalPositionEstimator::predict()
B * R * B.transpose() + Q) * getDt();
// propagate delayed state
if (_time_last_hist == 0 || _time_last_hist - _timeStamp > 1000) {
_xDelay.update(_x);
_PDelay.update(_P);
_tDelay.update(Scalar<uint64_t>(_timeStamp));
}
//if (_time_last_hist == 0 || _time_last_hist - _timeStamp > 1000) {
//_xDelay.update(_x);
//_PDelay.update(_P);
//_tDelay.update(Scalar<uint64_t>(_timeStamp));
//}
}
void BlockLocalPositionEstimator::correctFlow()
@ -1488,23 +1488,23 @@ void BlockLocalPositionEstimator::correctGps()
R(5, 5) = var_vz;
// get delayed x and P
uint64_t t_delay = 0;
int i = 0;
//uint64_t t_delay = 0;
//int i = 0;
for (i = 0; i < HIST_LEN; i++) {
t_delay += _tDelay.get(i)(0, 0);
//for (i = 0; i < HIST_LEN; i++) {
//t_delay += _tDelay.get(i)(0, 0);
if (t_delay > 2000) {
break;
}
}
//if (t_delay > 2000) {
//break;
//}
//}
Vector<float, n_x> x0 = _xDelay.get(i);
Matrix<float, n_x, n_x> P0 = _PDelay.get(i);
//Vector<float, n_x> x0 = _xDelay.get(i);
//Matrix<float, n_x, n_x> P0 = _PDelay.get(i);
// residual
Vector<float, n_y_gps> r = y - C * x0;
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * P0 * C.transpose() + R);
Vector<float, n_y_gps> r = y - C * _x;
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * _P * C.transpose() + R);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
@ -1536,9 +1536,9 @@ void BlockLocalPositionEstimator::correctGps()
// kalman filter correction if no hard fault
if (_gpsFault < FAULT_SEVERE) {
Matrix<float, n_x, n_y_gps> K = P0 * C.transpose() * S_I;
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
_x += K * r;
_P -= K * C * P0;
_P -= K * C * _P;
}
}

10
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@ -41,7 +41,7 @@ using namespace Eigen;
using namespace control;
static const size_t HIST_LEN = 2; // each step is 100 ms, gps has 200 ms delay
//static const size_t HIST_LEN = 2; // each step is 100 ms, gps has 200 ms delay
enum fault_t {
FAULT_NONE = 0,
@ -257,14 +257,14 @@ private:
BlockStats<double, 3> _gpsStats;
// delay blocks
BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
BlockDelay<float, n_x, n_x, HIST_LEN> _PDelay;
BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
//BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
//BlockDelay<float, n_x, n_x, HIST_LEN> _PDelay;
//BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
// misc
struct pollfd _polls[3];
uint64_t _timeStamp;
uint64_t _time_last_hist;
//uint64_t _time_last_hist;
uint64_t _time_last_xy;
uint64_t _time_last_z;
uint64_t _time_last_tz;

3
src/modules/local_position_estimator/local_position_estimator_main.cpp
View File

@ -152,11 +152,10 @@ int local_position_estimator_thread_main(int argc, char *argv[])
using namespace control;
BlockLocalPositionEstimator est;
thread_running = true;
BlockLocalPositionEstimator est;
while (!thread_should_exit) {
est.update();
}