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

LPE: Don't use home as local origin (#5067)

Browse Source
This commit is contained in:
James Goppert 2016-07-17 10:16:17 -04:00 committed by GitHub
parent 981353f439
commit 9fcf121380
8 changed files with 65 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -32,7 +32,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// status updates 2 hz
_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
_sub_manual(ORB_ID(manual_control_setpoint), 1000 / 2, 0, &getSubscriptions()),
_sub_home(ORB_ID(home_position), 1000 / 2, 0, &getSubscriptions()),
// gps 10 hz
_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
// vision 5 hz
@ -87,9 +86,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_pn_b_noise_density(this, "PN_B"),
_t_max_grade(this, "T_MAX_GRADE"),
// init home
_init_home_lat(this, "LAT"),
_init_home_lon(this, "LON"),
// init origin
_init_origin_lat(this, "LAT"),
_init_origin_lon(this, "LON"),
// flow gyro
_flow_gyro_x_high_pass(this, "FGYRO_HP"),
@ -140,12 +139,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_mocapInitialized(false),
// reference altitudes
_altHome(0),
_altHomeInitialized(false),
_baroAltHome(0),
_gpsAltHome(0),
_visionHome(),
_mocapHome(),
_altOrigin(0),
_altOriginInitialized(false),
_baroAltOrigin(0),
_gpsAltOrigin(0),
_visionOrigin(),
_mocapOrigin(),
// flow integration
_flowX(0),
@ -256,12 +255,10 @@ void BlockLocalPositionEstimator::update()
// reset pos, vel, and terrain on arming
if (!_lastArmedState && armedState) {
// we just armed, we are at home position on the ground
// we just armed, we are at origin on the ground
_x(X_x) = 0;
_x(X_y) = 0;
// the pressure altitude of home may have drifted, so we don't
// reset z to zero
_x(X_z) = 0;
// reset flow integral
_flowX = 0;
@ -287,7 +284,6 @@ void BlockLocalPositionEstimator::update()
bool paramsUpdated = _sub_param_update.updated();
bool baroUpdated = _sub_sensor.updated();
bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
bool homeUpdated = _sub_home.updated();
bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
bool mocapUpdated = _sub_mocap.updated();
bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
@ -302,11 +298,6 @@ void BlockLocalPositionEstimator::update()
updateSSParams();
}
// update home position projection
if (homeUpdated) {
updateHome();
}
// is xy valid?
bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();
@ -370,8 +361,8 @@ void BlockLocalPositionEstimator::update()
// if we have no lat, lon initialize projection at 0,0
if (_validXY && !_map_ref.init_done) {
map_projection_init(&_map_ref,
_init_home_lat.get(),
_init_home_lon.get());
_init_origin_lat.get(),
_init_origin_lon.get());
}
// reinitialize x if necessary
@ -484,7 +475,7 @@ void BlockLocalPositionEstimator::update()
}
}
if (_altHomeInitialized) {
if (_altOriginInitialized) {
// update all publications if possible
publishLocalPos();
publishEstimatorStatus();
@ -570,6 +561,7 @@ void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
dx(X_bz) = 0;
}
// if xy not valid, stop estimating
if (!_validXY) {
dx(X_x) = 0;
dx(X_y) = 0;
@ -579,12 +571,14 @@ void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
dx(X_by) = 0;
}
// if z not valid, stop estimating
if (!_validZ) {
dx(X_z) = 0;
dx(X_vz) = 0;
dx(X_bz) = 0;
}
// if terrain not valid, stop estimating
if (!_validTZ) {
dx(X_tz) = 0;
}
@ -629,28 +623,6 @@ void BlockLocalPositionEstimator::detectDistanceSensors()
}
}
void BlockLocalPositionEstimator::updateHome()
{
double lat = _sub_home.get().lat;
double lon = _sub_home.get().lon;
float alt = _sub_home.get().alt;
// updating home causes absolute measurements
// like gps and baro to be off, need to allow it
// to reset by resetting covariance
initP();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] home "
"lat %6.2f lon %6.2f alt %5.1f m",
lat, lon, double(alt));
map_projection_init(&_map_ref, lat, lon);
float delta_alt = alt - _altHome;
_altHomeInitialized = true;
_altHome = alt;
_gpsAltHome += delta_alt;
_baroAltHome += delta_alt;
_visionHome(2) += delta_alt;
_mocapHome(2) += delta_alt;
}
void BlockLocalPositionEstimator::publishLocalPos()
{
@ -672,12 +644,12 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().vy = xLP(X_vy); // east
_pub_lpos.get().vz = xLP(X_vz); // down
_pub_lpos.get().yaw = _sub_att.get().yaw;
_pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference
_pub_lpos.get().xy_global = _validXY;
_pub_lpos.get().z_global = _baroInitialized;
_pub_lpos.get().ref_timestamp = _sub_home.get().timestamp;
_pub_lpos.get().ref_timestamp = _timeStamp;
_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
_pub_lpos.get().ref_alt = _sub_home.get().alt;
_pub_lpos.get().ref_alt = _altOrigin;
_pub_lpos.get().dist_bottom = _aglLowPass.getState();
_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
@ -725,7 +697,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
double lon = 0;
const Vector<float, n_x> &xLP = _xLowPass.getState();
map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
float alt = -xLP(X_z) + _altHome;
float alt = -xLP(X_z) + _altOrigin;
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
@ -741,7 +713,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
_pub_gpos.get().yaw = _sub_att.get().yaw;
_pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
_pub_gpos.get().epv = sqrtf(_P(X_z, X_z));
_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
_pub_gpos.get().terrain_alt_valid = _validTZ;
_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;

21
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@ -19,7 +19,6 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
@ -205,7 +204,6 @@ private:
float agl();
void correctionLogic(Vector<float, n_x> &dx);
void detectDistanceSensors();
void updateHome();
// publications
void publishLocalPos();
@ -222,7 +220,6 @@ private:
uORB::Subscription<sensor_combined_s> _sub_sensor;
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;
@ -292,9 +289,9 @@ private:
BlockParamFloat _pn_b_noise_density;
BlockParamFloat _t_max_grade;
// init home
BlockParamFloat _init_home_lat;
BlockParamFloat _init_home_lon;
// init origin
BlockParamFloat _init_origin_lat;
BlockParamFloat _init_origin_lon;
// flow gyro filter
BlockHighPass _flow_gyro_x_high_pass;
@ -344,12 +341,12 @@ private:
bool _mocapInitialized;
// reference altitudes
float _altHome;
bool _altHomeInitialized;
float _baroAltHome;
float _gpsAltHome;
Vector3f _visionHome;
Vector3f _mocapHome;
float _altOrigin;
bool _altOriginInitialized;
float _baroAltOrigin;
float _gpsAltOrigin;
Vector3f _visionOrigin;
Vector3f _mocapOrigin;
// flow integration
float _flowX;

4
src/modules/local_position_estimator/params.c
View File

@ -308,7 +308,7 @@ PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f);
PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);
/**
* Home latitude for nav w/o GPS
* Local origin latitude for nav w/o GPS
*
* @group Local Position Estimator
* @unit deg
@ -319,7 +319,7 @@ PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);
PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
/**
* Home longitude for nav w/o GPS
* Local origin longitude for nav w/o GPS
*
* @group Local Position Estimator
* @unit deg

14
src/modules/local_position_estimator/sensors/baro.cpp
View File

@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::baroInit()
// if finished
if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
_baroAltHome = _baroStats.getMean()(0);
_baroAltOrigin = _baroStats.getMean()(0);
mavlink_and_console_log_info(&mavlink_log_pub,
"[lpe] baro init %d m std %d cm",
(int)_baroStats.getMean()(0),
@ -29,10 +29,10 @@ void BlockLocalPositionEstimator::baroInit()
_baroInitialized = true;
_baroFault = FAULT_NONE;
// only initialize alt home with baro if gps is disabled
if (!_altHomeInitialized && !_gps_on.get()) {
_altHomeInitialized = true;
_altHome = _baroAltHome;
// only initialize alt origin with baro and no gps
if (!_altOriginInitialized && !_gps_on.get()) {
_altOriginInitialized = true;
_altOrigin = _baroAltOrigin;
}
}
}
@ -54,8 +54,8 @@ void BlockLocalPositionEstimator::baroCorrect()
if (baroMeasure(y) != OK) { return; }
// subtract baro home alt
y -= _baroAltHome;
// subtract baro origin alt
y -= _baroAltOrigin;
// baro measurement matrix
Matrix<float, n_y_baro, n_x> C;

22
src/modules/local_position_estimator/sensors/gps.cpp
View File

@ -37,27 +37,27 @@ void BlockLocalPositionEstimator::gpsInit()
// if finished
if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
double gpsLatHome = _gpsStats.getMean()(0);
double gpsLonHome = _gpsStats.getMean()(1);
double gpsLatOrigin = _gpsStats.getMean()(0);
double gpsLonOrigin = _gpsStats.getMean()(1);
if (!_receivedGps) {
_receivedGps = true;
map_projection_init(&_map_ref, gpsLatHome, gpsLonHome);
map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
}
_gpsAltHome = _gpsStats.getMean()(2);
_gpsAltOrigin = _gpsStats.getMean()(2);
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
gpsLatHome,
gpsLonHome,
double(_gpsAltHome));
gpsLatOrigin,
gpsLonOrigin,
double(_gpsAltOrigin));
_gpsInitialized = true;
_gpsFault = FAULT_NONE;
_gpsStats.reset();
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _gpsAltHome;
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _gpsAltOrigin;
}
}
}
@ -92,7 +92,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
float alt = y_global(2);
float px = 0;
float py = 0;
float pz = -(alt - _gpsAltHome);
float pz = -(alt - _gpsAltOrigin);
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector<float, 6> y;
y.setZero();

12
src/modules/local_position_estimator/sensors/mocap.cpp
View File

@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::mocapInit()
// if finished
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
_mocapHome = _mocapStats.getMean();
_mocapOrigin = _mocapStats.getMean();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)),
@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::mocapInit()
_mocapInitialized = true;
_mocapFault = FAULT_NONE;
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _mocapHome(2);
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _mocapOrigin(2);
}
}
}
@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
if (mocapMeasure(y) != OK) { return; }
// make measurement relative to home
y -= _mocapHome;
// make measurement relative to origin
y -= _mocapOrigin;
// mocap measurement matrix, measures position
Matrix<float, n_y_mocap, n_x> C;

12
src/modules/local_position_estimator/sensors/vision.cpp
View File

@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::visionInit()
// increament sums for mean
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
_visionHome = _visionStats.getMean();
_visionOrigin = _visionStats.getMean();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: "
"%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
double(_visionStats.getMean()(0)),
@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::visionInit()
_visionInitialized = true;
_visionFault = FAULT_NONE;
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _visionHome(2);
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _visionOrigin(2);
}
}
}
@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::visionCorrect()
if (visionMeasure(y) != OK) { return; }
// make measurement relative to home
y -= _visionHome;
// make measurement relative to origin
y -= _visionOrigin;
// vision measurement matrix, measures position
Matrix<float, n_y_vision, n_x> C;

2
src/modules/uORB/Subscription.cpp
View File

@ -63,6 +63,7 @@
#include "topics/att_pos_mocap.h"
#include "topics/vision_position_estimate.h"
#include "topics/control_state.h"
#include "topics/vehicle_land_detected.h"
#include <px4_defines.h>
@ -179,5 +180,6 @@ template class __EXPORT Subscription<distance_sensor_s>;
template class __EXPORT Subscription<att_pos_mocap_s>;
template class __EXPORT Subscription<vision_position_estimate_s>;
template class __EXPORT Subscription<control_state_s>;
template class __EXPORT Subscription<vehicle_land_detected_s>;
} // namespace uORB