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

Added landed agl correction for lpe.

Browse Source
This commit is contained in:
James Goppert
2016-10-14 07:00:13 -05:00
committed by James Goppert
parent 35bf165190
commit 3ffff212e1
5 changed files with 153 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+20
View File
@@ -18,12 +18,14 @@ static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
static const bool integrate = true; // use accel for integrating
static const float P_MAX = 1.0e6f; // max allowed value in state covariance
static const float LAND_RATE = 10.0f; // rate of land detector correction
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// this block has no parent, and has name LPE
SuperBlock(NULL, "LPE"),
// subscriptions, set rate, add to list
_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
_sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()),
_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
// set flow max update rate higher than expected to we don't lose packets
_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
@@ -86,6 +88,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
_flow_min_q(this, "FLW_QMIN"),
_land_z_stddev(this, "LAND_Z"),
_pn_p_noise_density(this, "PN_P"),
_pn_v_noise_density(this, "PN_V"),
_pn_b_noise_density(this, "PN_B"),
@@ -133,6 +136,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_time_init_sonar(0),
_time_last_vision_p(0),
_time_last_mocap(0),
_time_last_land(0),
// initialization flags
_receivedGps(false),
@@ -143,6 +147,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_flowInitialized(false),
_visionInitialized(false),
_mocapInitialized(false),
_landInitialized(false),
// reference altitudes
_altOrigin(0),
@@ -167,6 +172,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_sonarFault(FAULT_NONE),
_visionFault(FAULT_NONE),
_mocapFault(FAULT_NONE),
_landFault(FAULT_NONE),
// loop performance
_loop_perf(),
@@ -286,6 +292,11 @@ void BlockLocalPositionEstimator::update()
bool mocapUpdated = _sub_mocap.updated();
bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
bool landUpdated = (
(_sub_land.get().landed ||
((!_sub_armed.get().armed) && (!_sub_land.get().freefall)))
&& (!(_lidarInitialized || _mocapInitialized || _visionInitialized || _sonarInitialized))
&& ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE));
// get new data
updateSubscriptions();
@@ -493,6 +504,15 @@ void BlockLocalPositionEstimator::update()
}
}
if (landUpdated) {
if (!_landInitialized) {
landInit();
} else {
landCorrect();
}
}
if (_altOriginInitialized) {
// update all publications if possible
publishLocalPos();
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+21 -2
View File
@@ -11,6 +11,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -52,7 +53,8 @@ enum sensor_t {
SENSOR_FLOW,
SENSOR_SONAR,
SENSOR_VISION,
SENSOR_MOCAP
SENSOR_MOCAP,
SENSOR_LAND,
};
// change this to set when
@@ -113,12 +115,14 @@ class BlockLocalPositionEstimator : public control::SuperBlock
//
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
//
// lidar: px (actual measured d*cos(phi)*cos(theta))
// lidar: pz (actual measured d*cos(phi)*cos(theta))
//
// vision: px, py, pz, vx, vy, vz
//
// mocap: px, py, pz
//
// land (detects when landed)): pz (always measures agl = 0)
//
public:
// constants
@@ -131,6 +135,7 @@ public:
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 {Y_land_z = 0, n_y_land};
enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM, n_poll};
BlockLocalPositionEstimator();
@@ -199,6 +204,12 @@ private:
void mocapInit();
void mocapCheckTimeout();
// land
int landMeasure(Vector<float, n_y_land> &y);
void landCorrect();
void landInit();
void landCheckTimeout();
// timeouts
void checkTimeouts();
@@ -219,6 +230,7 @@ private:
// subscriptions
uORB::Subscription<actuator_armed_s> _sub_armed;
uORB::Subscription<vehicle_land_detected_s> _sub_land;
uORB::Subscription<vehicle_attitude_s> _sub_att;
uORB::Subscription<optical_flow_s> _sub_flow;
uORB::Subscription<sensor_combined_s> _sub_sensor;
@@ -291,6 +303,9 @@ private:
//BlockParamFloat _flow_board_y_offs;
BlockParamInt _flow_min_q;
// land parameters
BlockParamFloat _land_z_stddev;
// process noise
BlockParamFloat _pn_p_noise_density;
BlockParamFloat _pn_v_noise_density;
@@ -314,6 +329,7 @@ private:
BlockStats<float, n_y_vision> _visionStats;
BlockStats<float, n_y_mocap> _mocapStats;
BlockStats<double, n_y_gps> _gpsStats;
uint16_t _landCount;
// low pass
BlockLowPassVector<float, n_x> _xLowPass;
@@ -338,6 +354,7 @@ private:
uint64_t _time_init_sonar;
uint64_t _time_last_vision_p;
uint64_t _time_last_mocap;
uint64_t _time_last_land;
// initialization flags
bool _receivedGps;
@@ -348,6 +365,7 @@ private:
bool _flowInitialized;
bool _visionInitialized;
bool _mocapInitialized;
bool _landInitialized;
// reference altitudes
float _altOrigin;
@@ -372,6 +390,7 @@ private:
fault_t _sonarFault;
fault_t _visionFault;
fault_t _mocapFault;
fault_t _landFault;
// performance counters
perf_counter_t _loop_perf;
src/modules/local_position_estimator/CMakeLists.txt
+1
View File
@@ -46,6 +46,7 @@ px4_add_module(
sensors/baro.cpp
sensors/vision.cpp
sensors/mocap.cpp
sensors/land.cpp
DEPENDS
platforms__common
)
src/modules/local_position_estimator/params.c
+11
View File
@@ -402,3 +402,14 @@ PARAM_DEFINE_FLOAT(LPE_VXY_PUB, 0.3f);
* @decimal 1
*/
PARAM_DEFINE_FLOAT(LPE_Z_PUB, 1.0f);
/**
* Land detector z standard deviation
*
* @group Local Position Estimator
* @unit m
* @min 0.001
* @max 10.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_LAND_Z, 0.03f);
src/modules/local_position_estimator/sensors/land.cpp
+100
View File
@@ -0,0 +1,100 @@
#include "../BlockLocalPositionEstimator.hpp"
#include <systemlib/mavlink_log.h>
#include <matrix/math.hpp>
extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
//
static const uint32_t REQ_LAND_INIT_COUNT = 1;
static const uint32_t LAND_TIMEOUT = 1000000; // 1.0 s
void BlockLocalPositionEstimator::landInit()
{
// measure
Vector<float, n_y_land> y;
if (landMeasure(y) != OK) {
_landCount = 0;
}
// if finished
if (_landCount > REQ_LAND_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land init");
_landInitialized = true;
_landFault = FAULT_NONE;
}
}
int BlockLocalPositionEstimator::landMeasure(Vector<float, n_y_land> &y)
{
_time_last_land = _timeStamp;
y.setZero();
_landCount += 1;
return OK;
}
void BlockLocalPositionEstimator::landCorrect()
{
// measure land
Vector<float, n_y_land> y;
if (landMeasure(y) != OK) { return; }
// measurement matrix
Matrix<float, n_y_land, n_x> C;
C.setZero();
// y = -(z - tz)
// TODO could add trig to make this an EKF correction
C(Y_land_z, X_z) = -1; // measured altitude, negative down dir.
C(Y_land_z, X_tz) = 1; // measured altitude, negative down dir.
// use parameter covariance
SquareMatrix<float, n_y_land> R;
R.setZero();
R(0, 0) = _land_z_stddev.get() * _land_z_stddev.get();
// residual
Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * _P * C.transpose()) + R);
Vector<float, n_y_land> r = y - C * _x;
_pub_innov.get().hagl_innov = r(0);
_pub_innov.get().hagl_innov_var = R(0, 0);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_land]) {
if (_landFault < FAULT_MINOR) {
_landFault = FAULT_MINOR;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land fault, beta %5.2f", double(beta));
}
// abort correction
return;
} else if (_landFault) { // disable fault if ok
_landFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK");
}
// kalman filter correction if no fault
if (_landFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_land> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
correctionLogic(dx);
_x += dx;
_P -= K * C * _P;
}
}
void BlockLocalPositionEstimator::landCheckTimeout()
{
if (_timeStamp - _time_last_land > LAND_TIMEOUT) {
if (_landInitialized) {
_landInitialized = false;
_landCount = 0;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land timeout ");
}
}
}