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

lpe: style fix

Browse Source
This commit is contained in:
TSC21
2017-11-03 23:45:43 +01:00
committed by Lorenz Meier
parent 051822eee1
commit 483f0d55e0
10 changed files with 96 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+28 -25
View File
@@ -32,10 +32,10 @@
using namespace matrix;
using namespace control;
static const float DELAY_MAX = 0.5f; // seconds
static const float HIST_STEP = 0.05f; // 20 hz
static const float DELAY_MAX = 0.5f; // seconds
static const float HIST_STEP = 0.05f; // 20 hz
static const float BIAS_MAX = 1e-1f;
static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP;
static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP;
static const size_t N_DIST_SUBS = 4;
// for fault detection
@@ -73,31 +73,31 @@ class BlockLocalPositionEstimator : public control::SuperBlock
//
//
// input:
// ax, ay, az (acceleration NED)
// ax, ay, az (acceleration NED)
//
// states:
// px, py, pz , ( position NED, m)
// vx, vy, vz ( vel NED, m/s),
// bx, by, bz ( accel bias, m/s^2)
// tz (terrain altitude, ASL, m)
// px, py, pz , ( position NED, m)
// vx, vy, vz ( vel NED, m/s),
// bx, by, bz ( accel bias, m/s^2)
// tz (terrain altitude, ASL, m)
//
// measurements:
//
// sonar: pz (measured d*cos(phi)*cos(theta))
// sonar: pz (measured d*cos(phi)*cos(theta))
//
// baro: pz
// baro: pz
//
// flow: vx, vy (flow is in body x, y frame)
// flow: vx, vy (flow is in body x, y frame)
//
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
//
// lidar: pz (actual measured d*cos(phi)*cos(theta))
// lidar: pz (actual measured d*cos(phi)*cos(theta))
//
// vision: px, py, pz, vx, vy, vz
// vision: px, py, pz, vx, vy, vz
//
// mocap: px, py, pz
// mocap: px, py, pz
//
// land (detects when landed)): pz (always measures agl = 0)
// land (detects when landed)): pz (always measures agl = 0)
//
public:
@@ -220,7 +220,10 @@ private:
void checkTimeouts();
// misc
inline float agl() { return _x(X_tz) - _x(X_z); }
inline float agl()
{
return _x(X_tz) - _x(X_z);
}
bool landed();
int getDelayPeriods(float delay, uint8_t *periods);
@@ -317,7 +320,7 @@ private:
BlockParamFloat _t_max_grade;
// init origin
BlockParamInt _fake_origin;
BlockParamInt _fake_origin;
BlockParamFloat _init_origin_lat;
BlockParamFloat _init_origin_lon;
@@ -375,15 +378,15 @@ private:
uint8_t _estimatorInitialized;
// state space
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
matrix::Dcm<float> _R_att;
Vector3f _eul;
Matrix<float, n_x, n_x> _A; // dynamics matrix
Matrix<float, n_x, n_u> _B; // input matrix
Matrix<float, n_u, n_u> _R; // input covariance
Matrix<float, n_x, n_x> _Q; // process noise covariance
Matrix<float, n_x, n_x> _A; // dynamics matrix
Matrix<float, n_x, n_u> _B; // input matrix
Matrix<float, n_u, n_u> _R; // input covariance
Matrix<float, n_x, n_x> _Q; // process noise covariance
};