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

Work on LPE GPS delay.

Browse Source
This commit is contained in:
James Goppert
2016-03-08 09:38:31 -05:00
parent ac66050cd6
commit d37ddb6102
5 changed files with 56 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/controllib/blocks.cpp
+1 -1
View File
@@ -581,7 +581,7 @@ int blockDelayTest()
{
printf("Test BlockDelay\t\t\t: ");
using namespace matrix;
BlockDelay<float, 2, 3> delay(NULL, "TEST");
BlockDelay<float, 2, 1, 3> delay(NULL, "TEST");
Vector2f u1(1, 2);
Vector2f y1 = delay.update(u1);
ASSERT_CL(equal(y1(0), u1(0)));
src/modules/controllib/blocks.hpp
+5 -5
View File
@@ -601,7 +601,7 @@ private:
int __EXPORT blockStatsTest();
template<class Type, size_t M, size_t LEN>
template<class Type, size_t M, size_t N, size_t LEN>
class __EXPORT BlockDelay: public Block
{
public:
@@ -615,7 +615,7 @@ public:
{
};
virtual ~BlockDelay() {};
matrix::Vector<Type, M> update(const matrix::Vector<Type, M> &u)
matrix::Matrix<Type, M, N> update(const matrix::Matrix<Type, M, N> &u)
{
// store current value
_h[_index] = u;
@@ -644,9 +644,9 @@ public:
// get delayed value
return _h[j];
}
matrix::Vector<Type, M> get()
matrix::Matrix<Type, M, N> get(size_t delay)
{
int j = _index - _delay;
int j = _index - delay;
if (j < 0) { j += LEN; }
@@ -654,7 +654,7 @@ public:
}
private:
// attributes
matrix::Vector<Type, M> _h[LEN];
matrix::Matrix<Type, M, N> _h[LEN];
size_t _index;
int _delay;
};
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+44 -41
View File
@@ -105,11 +105,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_gpsStats(this, ""),
// stats
_gpsDelay(this, ""),
_xDelay(this, ""),
_PDelay(this, ""),
_tDelay(this, ""),
// misc
_polls(),
_timeStamp(hrt_absolute_time()),
_time_last_hist(0),
_time_last_xy(0),
_time_last_z(0),
_time_last_tz(0),
@@ -589,17 +592,6 @@ void BlockLocalPositionEstimator::initGps()
return;
}
// gps measurement
Vector<double, 6> h;
h.setZero();
h(0) = _sub_gps.get().lat;
h(1) = _sub_gps.get().lon;
h(2) = _sub_gps.get().alt;
h(3) = _sub_gps.get().vel_n_m_s;
h(4) = _sub_gps.get().vel_e_m_s;
h(5) = _sub_gps.get().vel_d_m_s;
_gpsDelay.update(h);
// collect gps data
Vector3<double> p(
_sub_gps.get().lat * 1e-7,
@@ -1040,6 +1032,13 @@ void BlockLocalPositionEstimator::predict()
_x += dx;
_P += (A * _P + _P * A.transpose() +
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));
}
}
void BlockLocalPositionEstimator::correctFlow()
@@ -1048,7 +1047,7 @@ void BlockLocalPositionEstimator::correctFlow()
float qual = _sub_flow.get().quality;
if (qual < _flow_min_q.get()) {
if (!_flowFault) {
if (_flowFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] low flow quality %d", int(qual));
warnx("[lpe] low flow quality %d", int(qual));
_flowFault = FAULT_SEVERE;
@@ -1063,11 +1062,11 @@ void BlockLocalPositionEstimator::correctFlow()
// calculate range to center of image for flow
float d = 0;
if (_lidarInitialized && !_lidarFault) {
if (_lidarInitialized && _lidarFault < FAULT_SEVERE) {
d = _sub_lidar->get().current_distance
+ (_lidar_z_offset.get() - _flow_z_offset.get());
} else if (_sonarInitialized && !_sonarFault) {
} else if (_sonarInitialized && _sonarFault < FAULT_SEVERE) {
d = _sub_sonar->get().current_distance
+ (_sonar_z_offset.get() - _flow_z_offset.get());
@@ -1151,7 +1150,7 @@ void BlockLocalPositionEstimator::correctFlow()
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_flow]) {
if (!_flowFault) {
if (_flowFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] flow fault, beta %5.2f", double(beta));
warnx("[lpe] flow fault, beta %5.2f", double(beta));
_flowFault = FAULT_MINOR;
@@ -1207,7 +1206,7 @@ void BlockLocalPositionEstimator::correctSonar()
_time_last_sonar = _timeStamp;
// do not use sonar if lidar is active
if (_lidarInitialized && !_lidarFault) { return; }
if (_lidarInitialized && _lidarFault < FAULT_SEVERE) { return; }
// calculate covariance
float cov = _sub_sonar->get().covariance;
@@ -1303,7 +1302,7 @@ void BlockLocalPositionEstimator::correctBaro()
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_baro]) {
if (!_baroFault) {
if (_baroFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] baro fault, beta %5.2f", double(beta));
warnx("[lpe] baro fault, beta %5.2f", double(beta));
_baroFault = FAULT_MINOR;
@@ -1345,7 +1344,7 @@ void BlockLocalPositionEstimator::correctLidar()
// if out of range, this is an error
if (d < min_dist || d > max_dist) {
if (!_lidarFault) {
if (_lidarFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range");
warnx("[lpe] lidar out of range");
_lidarFault = FAULT_SEVERE;
@@ -1389,7 +1388,7 @@ void BlockLocalPositionEstimator::correctLidar()
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_lidar]) {
if (!_lidarFault) {
if (_lidarFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, beta %5.2f", double(beta));
warnx("[lpe] lidar fault, beta %5.2f", double(beta));
_lidarFault = FAULT_MINOR;
@@ -1434,22 +1433,11 @@ void BlockLocalPositionEstimator::correctGps()
return;
}
// gps measurement
Vector<double, 6> h0;
h0.setZero();
h0(0) = _sub_gps.get().lat;
h0(1) = _sub_gps.get().lon;
h0(2) = _sub_gps.get().alt;
h0(3) = _sub_gps.get().vel_n_m_s;
h0(4) = _sub_gps.get().vel_e_m_s;
h0(5) = _sub_gps.get().vel_d_m_s;
Vector<double, 6> h = _gpsDelay.update(h0);
// gps measurement in local frame
_time_last_gps = _timeStamp;
double lat = h(0) * 1.0e-7;
double lon = h(1) * 1.0e-7;
float alt = h(2) * 1.0e-3;
double lat = _sub_gps.get().lat * 1.0e-7;
double lon = _sub_gps.get().lon * 1.0e-7;
float alt = _sub_gps.get().alt * 1.0e-3;
float px = 0;
float py = 0;
float pz = -(alt - _gpsAltHome);
@@ -1459,9 +1447,9 @@ void BlockLocalPositionEstimator::correctGps()
y(0) = px;
y(1) = py;
y(2) = pz;
y(3) = h(3);
y(4) = h(4);
y(5) = h(5);
y(3) = _sub_gps.get().vel_n_m_s;
y(4) = _sub_gps.get().vel_e_m_s;
y(5) = _sub_gps.get().vel_d_m_s;
// gps measurement matrix, measures position and velocity
Matrix<float, n_y_gps, n_x> C;
@@ -1499,9 +1487,24 @@ void BlockLocalPositionEstimator::correctGps()
R(4, 4) = var_vxy;
R(5, 5) = var_vz;
// get delayed x and P
uint64_t t_delay = 0;
int i = 0;
for (i = 0; i < HIST_LEN; i++) {
t_delay += _tDelay.get(i)(0, 0);
if (t_delay > 2000) {
break;
}
}
Vector<float, n_x> x0 = _xDelay.get(i);
Matrix<float, n_x, n_x> P0 = _PDelay.get(i);
// residual
Vector<float, 6> r = y - C * _x;
Matrix<float, 6, 6> S_I = inv<float, 6>(C * _P * C.transpose() + R);
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);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
@@ -1533,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 = _P * C.transpose() * S_I;
Matrix<float, n_x, n_y_gps> K = P0 * C.transpose() * S_I;
_x += K * r;
_P -= K * C * _P;
_P -= K * C * P0;
}
}
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+5 -2
View File
@@ -41,7 +41,7 @@ using namespace Eigen;
using namespace control;
static const size_t GPS_HIST_LEN = 2; // delay, at 10 hz, (200 ms)
static const size_t HIST_LEN = 2; // each step is 100 ms, gps has 200 ms delay
enum fault_t {
FAULT_NONE = 0,
@@ -257,11 +257,14 @@ private:
BlockStats<double, 3> _gpsStats;
// delay blocks
BlockDelay<double, n_y_gps, GPS_HIST_LEN> _gpsDelay;
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_xy;
uint64_t _time_last_z;
uint64_t _time_last_tz;
src/modules/local_position_estimator/CMakeLists.txt
+1 -1
View File
@@ -31,7 +31,7 @@
#
############################################################################
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500)
list(APPEND MODULE_CFLAGS -Wframe-larger-than=7000)
elseif(${OS} STREQUAL "posix")
list(APPEND MODULE_CFLAGS -Wno-error)
endif()