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

LPE Variance Dependent Publication (#4914)

Browse Source 
* Use variance to control publishing for LPE.

* Don't stop publishing if we have gps/ baro.

* LPE tuning and cleanup.

* Added bias saturation to LPE.

* Added vector enabled low pass filter block.

* Added rk4 integration and pub lowpass to LPE.

* Fix std::abs issue on mac/ reset lowpass on state reset.

* Don't estimate gyro bias when rotating at high speed  att_est_q.

* Lowered low pass on position to 5 Hz for LPE.

* Streamline state space update for LPE.

* Added health flags to est2 log.

* Revert to old tuning, more conservative, less faults.

* Formatting.

* Fix for fault message on LPE.

* Added subscription throttling to LPE.

* Formatting.
This commit is contained in:
James Goppert
2016-07-01 11:43:09 -04:00
committed by GitHub
parent 2a395c3fec
commit 00dfc99e08
17 changed files with 417 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+21 -8
View File
@@ -33,8 +33,9 @@
using namespace matrix;
using namespace control;
static const float GPS_DELAY_MAX = 0.5; // seconds
static const float HIST_STEP = 0.05; // 20 hz
static const float GPS_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; // GPS_DELAY_MAX / HIST_STEP;
static const size_t N_DIST_SUBS = 4;
@@ -134,6 +135,10 @@ public:
BlockLocalPositionEstimator();
void update();
Vector<float, n_x> dynamics(
float t,
const Vector<float, n_x> &x,
const Vector<float, n_u> &u);
virtual ~BlockLocalPositionEstimator();
private:
@@ -144,6 +149,9 @@ private:
// methods
// ----------------------------
void initP();
void initSS();
void updateSSStates();
void updateSSParams();
// predict the next state
void predict();
@@ -195,6 +203,7 @@ private:
// misc
float agl();
void correctionLogic(Vector<float, n_x> &dx);
void detectDistanceSensors();
void updateHome();
@@ -207,11 +216,8 @@ private:
// ----------------------------
// subscriptions
uORB::Subscription<vehicle_status_s> _sub_status;
uORB::Subscription<actuator_armed_s> _sub_armed;
uORB::Subscription<vehicle_control_mode_s> _sub_control_mode;
uORB::Subscription<vehicle_attitude_s> _sub_att;
uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp;
uORB::Subscription<optical_flow_s> _sub_flow;
uORB::Subscription<sensor_combined_s> _sub_sensor;
uORB::Subscription<parameter_update_s> _sub_param_update;
@@ -302,6 +308,9 @@ private:
BlockStats<float, n_y_mocap> _mocapStats;
BlockStats<double, n_y_gps> _gpsStats;
// low pass
BlockLowPassVector<float, n_x> _xLowPass;
// delay blocks
BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
@@ -346,9 +355,9 @@ private:
float _flowMeanQual;
// status
bool _canEstimateXY;
bool _canEstimateZ;
bool _canEstimateT;
bool _validXY;
bool _validZ;
bool _validTZ;
bool _xyTimeout;
bool _zTimeout;
bool _tzTimeout;
@@ -372,4 +381,8 @@ private:
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<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
};