Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

Updated lpe.

Browse Source
This commit is contained in:
James Goppert
2016-03-08 03:30:36 -05:00
parent e33fd3d815
commit baed2c0255
4 changed files with 771 additions and 422 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cmake/configs/nuttx_px4fmu-v2_lpe.cmake
+4
View File
@@ -1,5 +1,9 @@
include(cmake/configs/nuttx_px4fmu-v2_default.cmake)
list(REMOVE_ITEM config_module_list
modules/ekf_att_pos_estimator
)
list(APPEND config_module_list
modules/local_position_estimator
)
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+591 -336
View File
File diff suppressed because it is too large Load Diff
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+61 -47
View File
@@ -34,7 +34,7 @@ using namespace Eigen;
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/filtered_bottom_flow.h>
//#include <uORB/topics/filtered_bottom_flow.h>
#include <uORB/topics/estimator_status.h>
#define CBRK_NO_VISION_KEY 328754
@@ -108,6 +108,19 @@ class BlockLocalPositionEstimator : public control::SuperBlock
// mocap: px, py, pz
//
public:
// constants
enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz, X_tz, n_x};
enum {U_ax = 0, U_ay, U_az, n_u};
enum {Y_baro_z = 0, n_y_baro};
enum {Y_lidar_z = 0, n_y_lidar};
enum {Y_flow_x = 0, Y_flow_y, n_y_flow};
enum {Y_sonar_z = 0, n_y_sonar};
enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM, n_poll};
BlockLocalPositionEstimator();
void update();
virtual ~BlockLocalPositionEstimator();
@@ -117,27 +130,6 @@ private:
BlockLocalPositionEstimator(const BlockLocalPositionEstimator &);
BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &);
// constants
static const uint8_t n_x = 9;
static const uint8_t n_u = 3; // 3 accelerations
static const uint8_t n_y_flow = 2;
static const uint8_t n_y_sonar = 1;
static const uint8_t n_y_baro = 1;
static const uint8_t n_y_lidar = 1;
static const uint8_t n_y_gps = 6;
static const uint8_t n_y_vision = 3;
static const uint8_t n_y_mocap = 3;
enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz};
enum {U_ax = 0, U_ay, U_az};
enum {Y_baro_z = 0};
enum {Y_lidar_z = 0};
enum {Y_flow_x = 0, Y_flow_y};
enum {Y_sonar_z = 0};
enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz};
enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz};
enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z};
enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM};
// methods
// ----------------------------
void initP();
@@ -152,7 +144,10 @@ private:
void correctFlow();
void correctSonar();
void correctVision();
void correctmocap();
void correctMocap();
// sensor timeout checks
void checkTimeouts();
// sensor initialization
void updateHome();
@@ -162,12 +157,12 @@ private:
void initSonar();
void initFlow();
void initVision();
void initmocap();
void initMocap();
// publications
void publishLocalPos();
void publishGlobalPos();
void publishFilteredFlow();
//void publishFilteredFlow();
void publishEstimatorStatus();
// attributes
@@ -181,60 +176,90 @@ private:
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<distance_sensor_s> _sub_distance;
uORB::Subscription<parameter_update_s> _sub_param_update;
uORB::Subscription<manual_control_setpoint_s> _sub_manual;
uORB::Subscription<home_position_s> _sub_home;
uORB::Subscription<vehicle_gps_position_s> _sub_gps;
uORB::Subscription<vision_position_estimate_s> _sub_vision_pos;
uORB::Subscription<att_pos_mocap_s> _sub_mocap;
uORB::Subscription<distance_sensor_s> *_distance_subs[ORB_MULTI_MAX_INSTANCES];
uORB::Subscription<distance_sensor_s> *_sub_lidar;
uORB::Subscription<distance_sensor_s> *_sub_sonar;
// publications
uORB::Publication<vehicle_local_position_s> _pub_lpos;
uORB::Publication<vehicle_global_position_s> _pub_gpos;
uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow;
//uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow;
uORB::Publication<estimator_status_s> _pub_est_status;
// map projection
struct map_projection_reference_s _map_ref;
// parameters
// general parameters
BlockParamInt _integrate;
BlockParamFloat _flow_xy_stddev;
// sonar parameters
BlockParamFloat _sonar_z_stddev;
BlockParamFloat _sonar_z_offset;
// lidar parameters
BlockParamFloat _lidar_z_stddev;
BlockParamFloat _lidar_z_offset;
// accel parameters
BlockParamFloat _accel_xy_stddev;
BlockParamFloat _accel_z_stddev;
// baro parameters
BlockParamFloat _baro_stddev;
// gps parameters
BlockParamFloat _gps_xy_stddev;
BlockParamFloat _gps_z_stddev;
BlockParamFloat _gps_vxy_stddev;
BlockParamFloat _gps_vz_stddev;
BlockParamFloat _gps_eph_max;
// vision parameters
BlockParamFloat _vision_xy_stddev;
BlockParamFloat _vision_z_stddev;
BlockParamInt _no_vision;
BlockParamFloat _beta_max;
// mocap parameters
BlockParamFloat _mocap_p_stddev;
// flow parameters
BlockParamFloat _flow_z_offset;
BlockParamFloat _flow_xy_stddev;
//BlockParamFloat _flow_board_x_offs;
//BlockParamFloat _flow_board_y_offs;
BlockParamInt _flow_min_q;
// process noise
BlockParamFloat _pn_p_noise_power;
BlockParamFloat _pn_v_noise_power;
BlockParamFloat _pn_b_noise_power;
BlockParamFloat _pn_t_noise_power;
// flow gyro filter
BlockHighPass _flow_gyro_x_high_pass;
BlockHighPass _flow_gyro_y_high_pass;
// stats
BlockStats<float, 1> _baroStats;
BlockStats<float, 1> _sonarStats;
BlockStats<float, 1> _lidarStats;
BlockStats<float, 1> _flowQStats;
BlockStats<float, 3> _visionStats;
BlockStats<float, 3> _mocapStats;
BlockStats<double, 3> _gpsStats;
// misc
struct pollfd _polls[3];
uint64_t _timeStamp;
uint64_t _time_last_xy;
uint64_t _time_last_z;
uint64_t _time_last_tz;
uint64_t _time_last_flow;
uint64_t _time_last_baro;
uint64_t _time_last_gps;
@@ -253,23 +278,11 @@ private:
bool _visionInitialized;
bool _mocapInitialized;
// init counts
int _baroInitCount;
int _gpsInitCount;
int _lidarInitCount;
int _sonarInitCount;
int _flowInitCount;
int _visionInitCount;
int _mocapInitCount;
// reference altitudes
float _altHome;
bool _altHomeInitialized;
float _baroAltHome;
float _gpsAltHome;
float _lidarAltHome;
float _sonarAltHome;
float _flowAltHome;
Vector3f _visionHome;
Vector3f _mocapHome;
@@ -285,7 +298,10 @@ private:
// status
bool _canEstimateXY;
bool _canEstimateZ;
bool _canEstimateT;
bool _xyTimeout;
bool _zTimeout;
bool _tzTimeout;
// sensor faults
fault_t _baroFault;
@@ -296,9 +312,7 @@ private:
fault_t _visionFault;
fault_t _mocapFault;
bool _visionTimeout;
bool _mocapTimeout;
// performance counters
perf_counter_t _loop_perf;
perf_counter_t _interval_perf;
perf_counter_t _err_perf;
src/modules/local_position_estimator/params.c
+85 -9
View File
@@ -7,6 +7,9 @@
* Enable local position estimator.
*
* @group Local Position Estimator
* @min 0
* @max 1
* @decimal 0
*/
PARAM_DEFINE_INT32(LPE_ENABLED, 1);
@@ -14,9 +17,23 @@ PARAM_DEFINE_INT32(LPE_ENABLED, 1);
* Enable accelerometer integration for prediction.
*
* @group Local Position Estimator
* @min 0
* @max 1
* @decimal 0
*/
PARAM_DEFINE_INT32(LPE_INTEGRATE, 1);
/**
* Optical flow z offset from center
*
* @group Local Position Estimator
* @unit m
* @min -1
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
/**
* Optical flow xy standard deviation.
*
@@ -24,9 +41,20 @@ PARAM_DEFINE_INT32(LPE_INTEGRATE, 1);
* @unit m
* @min 0.01
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
/**
* Optical flow minimum quality threshold
*
* @group Local Position Estimator
* @min 0
* @max 255
* @decimal 0
*/
PARAM_DEFINE_INT32(LPE_FLW_QMIN, 75);
/**
* Sonar z standard deviation.
*
@@ -34,8 +62,20 @@ PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
* @unit m
* @min 0.01
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f);
PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.05f);
/**
* Sonar z offset from center of vehicle +down
*
* @group Local Position Estimator
* @unit m
* @min -1
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_SNR_OFF_Z, 0.00f);
/**
* Lidar z standard deviation.
@@ -44,9 +84,21 @@ PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f);
* @unit m
* @min 0.01
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
/**
* Lidar z offset from center of vehicle +down
*
* @group Local Position Estimator
* @unit m
* @min -1
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);
/**
* Accelerometer xy standard deviation
*
@@ -60,6 +112,7 @@ PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
* @unit m/s^2
* @min 0.00001
* @max 2
* @decimal 4
*/
PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
@@ -72,6 +125,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
* @unit m/s^2
* @min 0.00001
* @max 2
* @decimal 4
*/
PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
@@ -82,6 +136,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
* @unit m
* @min 0.01
* @max 3
* @decimal 2
*/
PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
@@ -92,6 +147,7 @@ PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
* @unit m
* @min 0.01
* @max 5
* @decimal 2
*/
PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
@@ -102,6 +158,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
* @unit m
* @min 0.01
* @max 20
* @decimal 2
*/
PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f);
@@ -112,6 +169,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f);
* @unit m/s
* @min 0.01
* @max 2
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f);
@@ -122,6 +180,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f);
* @unit m/s
* @min 0.01
* @max 2
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f);
@@ -132,6 +191,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f);
* @unit m
* @min 1.0
* @max 5.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
@@ -144,6 +204,7 @@ PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
* @unit m
* @min 0.01
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
@@ -154,6 +215,7 @@ PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
* @unit m
* @min 0.01
* @max 2
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f);
@@ -165,6 +227,7 @@ PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f);
* @group Local Position Estimator
* @min 0
* @max 1
* @decimal 0
*/
PARAM_DEFINE_INT32(LPE_NO_VISION, 0);
@@ -175,6 +238,7 @@ PARAM_DEFINE_INT32(LPE_NO_VISION, 0);
* @unit m
* @min 0.01
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
@@ -185,6 +249,7 @@ PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
* @unit (m/s^2)-s
* @min 0
* @max 1
* @decimal 8
*/
PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
@@ -195,6 +260,7 @@ PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
* @unit (m/s)-s
* @min 0
* @max 5
* @decimal 8
*/
PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
@@ -205,18 +271,28 @@ PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
* @unit (m/s)-s
* @min 0
* @max 1
* @decimal 8
*/
PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f);
/**
* Fault detection threshold, for chi-squared dist.
*
* TODO add separate params for 1 dof, 3 dof, and 6 dof beta
* or false alarm rate in false alarms/hr
* Terrain random walk noise power (variance*sampling rate).
*
* @group Local Position Estimator
* @unit
* @min 3
* @max 1000
* @unit m-s
* @min 0
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_BETA_MAX, 1000.0f);
PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-3f);
/**
* Flow gyro high pass filter cut off frequency
*
* @group Local Position Estimator
* @unit Hz
* @min 0
* @max 2
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);