mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-17 03:37:35 +08:00
LPE land bug fix and switch to fusion bit mask.
This commit is contained in:
James Goppert
committed by
Lorenz Meier
Lorenz Meier
parent
f263ea7f7e
commit
6ff85fb927
@@ -15,11 +15,12 @@ static const uint32_t EST_SRC_TIMEOUT = 10000; // 0.01 s
|
||||
static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m
|
||||
static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m
|
||||
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
|
||||
|
||||
static const char *msg_label = "[lpe] "; // rate of land detector correction
|
||||
|
||||
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
||||
// this block has no parent, and has name LPE
|
||||
SuperBlock(NULL, "LPE"),
|
||||
@@ -59,7 +60,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
||||
_map_ref(),
|
||||
|
||||
// block parameters
|
||||
_pub_agl_z(this, "PUB_AGL_Z"),
|
||||
_fusion(this, "FUSION"),
|
||||
_vxy_pub_thresh(this, "VXY_PUB"),
|
||||
_z_pub_thresh(this, "Z_PUB"),
|
||||
_sonar_z_stddev(this, "SNR_Z"),
|
||||
@@ -69,7 +70,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
||||
_accel_xy_stddev(this, "ACC_XY"),
|
||||
_accel_z_stddev(this, "ACC_Z"),
|
||||
_baro_stddev(this, "BAR_Z"),
|
||||
_gps_on(this, "GPS_ON"),
|
||||
_gps_delay(this, "GPS_DELAY"),
|
||||
_gps_xy_stddev(this, "GPS_XY"),
|
||||
_gps_z_stddev(this, "GPS_Z"),
|
||||
@@ -80,15 +80,16 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
||||
_vision_xy_stddev(this, "VIS_XY"),
|
||||
_vision_z_stddev(this, "VIS_Z"),
|
||||
_vision_delay(this, "VIS_DELAY"),
|
||||
_vision_on(this, "VIS_ON"),
|
||||
_mocap_p_stddev(this, "VIC_P"),
|
||||
_flow_gyro_comp(this, "FLW_GYRO_CMP"),
|
||||
_flow_z_offset(this, "FLW_OFF_Z"),
|
||||
_flow_scale(this, "FLW_SCALE"),
|
||||
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
|
||||
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
|
||||
_flow_min_q(this, "FLW_QMIN"),
|
||||
_flow_r(this, "FLW_R"),
|
||||
_flow_rr(this, "FLW_RR"),
|
||||
_land_z_stddev(this, "LAND_Z"),
|
||||
_land_vxy_stddev(this, "LAND_VXY"),
|
||||
_pn_p_noise_density(this, "PN_P"),
|
||||
_pn_v_noise_density(this, "PN_V"),
|
||||
_pn_b_noise_density(this, "PN_B"),
|
||||
@@ -124,10 +125,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
||||
// misc
|
||||
_polls(),
|
||||
_timeStamp(hrt_absolute_time()),
|
||||
_timeStampLastBaro(hrt_absolute_time()),
|
||||
_time_last_hist(0),
|
||||
_time_last_xy(0),
|
||||
_time_last_z(0),
|
||||
_time_last_tz(0),
|
||||
_time_last_flow(0),
|
||||
_time_last_baro(0),
|
||||
_time_last_gps(0),
|
||||
@@ -138,17 +137,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
||||
_time_last_mocap(0),
|
||||
_time_last_land(0),
|
||||
|
||||
// initialization flags
|
||||
_receivedGps(false),
|
||||
_baroInitialized(false),
|
||||
_gpsInitialized(false),
|
||||
_lidarInitialized(false),
|
||||
_sonarInitialized(false),
|
||||
_flowInitialized(false),
|
||||
_visionInitialized(false),
|
||||
_mocapInitialized(false),
|
||||
_landInitialized(false),
|
||||
|
||||
// reference altitudes
|
||||
_altOrigin(0),
|
||||
_altOriginInitialized(false),
|
||||
@@ -156,28 +144,13 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
||||
_gpsAltOrigin(0),
|
||||
|
||||
// status
|
||||
_validXY(false),
|
||||
_validZ(false),
|
||||
_validTZ(false),
|
||||
_xyTimeout(true),
|
||||
_zTimeout(true),
|
||||
_tzTimeout(true),
|
||||
_receivedGps(false),
|
||||
_lastArmedState(false),
|
||||
|
||||
// faults
|
||||
_baroFault(FAULT_NONE),
|
||||
_gpsFault(FAULT_NONE),
|
||||
_lidarFault(FAULT_NONE),
|
||||
_flowFault(FAULT_NONE),
|
||||
_sonarFault(FAULT_NONE),
|
||||
_visionFault(FAULT_NONE),
|
||||
_mocapFault(FAULT_NONE),
|
||||
_landFault(FAULT_NONE),
|
||||
|
||||
// loop performance
|
||||
_loop_perf(),
|
||||
_interval_perf(),
|
||||
_err_perf(),
|
||||
// masks
|
||||
_sensorTimeout(255),
|
||||
_sensorFault(0),
|
||||
_estimatorInitialized(0),
|
||||
|
||||
// kf matrices
|
||||
_x(), _u(), _P(), _R_att(), _eul()
|
||||
@@ -203,18 +176,22 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
||||
_u.setZero();
|
||||
initSS();
|
||||
|
||||
// perf counters
|
||||
_loop_perf = perf_alloc(PC_ELAPSED,
|
||||
"local_position_estimator_runtime");
|
||||
//_interval_perf = perf_alloc(PC_INTERVAL,
|
||||
//"local_position_estimator_interval");
|
||||
_err_perf = perf_alloc(PC_COUNT, "local_position_estimator_err");
|
||||
|
||||
// map
|
||||
_map_ref.init_done = false;
|
||||
|
||||
// intialize parameter dependent matrices
|
||||
updateParams();
|
||||
|
||||
// print fusion settings to console
|
||||
printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
|
||||
"vis_yaw: %d, land: %d, pub_agl_z: %d, flow_gyro: %d\n",
|
||||
(_fusion.get() & FUSE_GPS) != 0,
|
||||
(_fusion.get() & FUSE_FLOW) != 0,
|
||||
(_fusion.get() & FUSE_VIS_POS) != 0,
|
||||
(_fusion.get() & FUSE_VIS_YAW) != 0,
|
||||
(_fusion.get() & FUSE_LAND) != 0,
|
||||
(_fusion.get() & FUSE_PUB_AGL_Z) != 0,
|
||||
(_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0);
|
||||
}
|
||||
|
||||
BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
|
||||
@@ -236,8 +213,6 @@ void BlockLocalPositionEstimator::update()
|
||||
int ret = px4_poll(_polls, 3, 100);
|
||||
|
||||
if (ret < 0) {
|
||||
/* poll error, count it in perf */
|
||||
perf_count(_err_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -252,7 +227,32 @@ void BlockLocalPositionEstimator::update()
|
||||
bool armedState = _sub_armed.get().armed;
|
||||
|
||||
if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
|
||||
detectDistanceSensors();
|
||||
|
||||
// detect distance sensors
|
||||
for (int i = 0; i < N_DIST_SUBS; i++) {
|
||||
uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];
|
||||
|
||||
if (s == _sub_lidar || s == _sub_sonar) { continue; }
|
||||
|
||||
if (s->updated()) {
|
||||
s->update();
|
||||
|
||||
if (s->get().timestamp == 0) { continue; }
|
||||
|
||||
if (s->get().type == \
|
||||
distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
|
||||
_sub_lidar == NULL) {
|
||||
_sub_lidar = s;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "%sLidar detected with ID %i", msg_label, i);
|
||||
|
||||
} else if (s->get().type == \
|
||||
distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
|
||||
_sub_sonar == NULL) {
|
||||
_sub_sonar = s;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "%sSonar detected with ID %i", msg_label, i);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// reset pos, vel, and terrain on arming
|
||||
@@ -286,17 +286,29 @@ void BlockLocalPositionEstimator::update()
|
||||
// see which updates are available
|
||||
bool flowUpdated = _sub_flow.updated();
|
||||
bool paramsUpdated = _sub_param_update.updated();
|
||||
bool baroUpdated = _sub_sensor.updated();
|
||||
bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
|
||||
bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
|
||||
bool baroUpdated = false;
|
||||
|
||||
if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated()) {
|
||||
int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;
|
||||
|
||||
if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) {
|
||||
uint64_t baro_timestamp = _sub_sensor.get().timestamp + \
|
||||
_sub_sensor.get().baro_timestamp_relative;
|
||||
|
||||
if (baro_timestamp != _timeStampLastBaro) {
|
||||
baroUpdated = true;
|
||||
_timeStampLastBaro = baro_timestamp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
|
||||
bool visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated();
|
||||
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));
|
||||
bool landUpdated = landed()
|
||||
&& ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE); // throttle rate
|
||||
|
||||
// get new data
|
||||
updateSubscriptions();
|
||||
@@ -314,16 +326,21 @@ void BlockLocalPositionEstimator::update()
|
||||
vxy_stddev_ok = true;
|
||||
}
|
||||
|
||||
if (_validXY) {
|
||||
if (_estimatorInitialized & EST_XY) {
|
||||
// if valid and gps has timed out, set to not valid
|
||||
if (!vxy_stddev_ok && !_gpsInitialized) {
|
||||
_validXY = false;
|
||||
if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS)) {
|
||||
_estimatorInitialized &= ~EST_XY;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (vxy_stddev_ok) {
|
||||
if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
|
||||
_validXY = true;
|
||||
if (!(_sensorTimeout & SENSOR_GPS)
|
||||
|| !(_sensorTimeout & SENSOR_FLOW)
|
||||
|| !(_sensorTimeout & SENSOR_VISION)
|
||||
|| !(_sensorTimeout & SENSOR_MOCAP)
|
||||
|| !(_sensorTimeout & SENSOR_LAND)
|
||||
) {
|
||||
_estimatorInitialized |= EST_XY;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -331,50 +348,37 @@ void BlockLocalPositionEstimator::update()
|
||||
// is z valid?
|
||||
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
|
||||
|
||||
if (_validZ) {
|
||||
if (_estimatorInitialized & EST_Z) {
|
||||
// if valid and baro has timed out, set to not valid
|
||||
if (!z_stddev_ok && !_baroInitialized) {
|
||||
_validZ = false;
|
||||
if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO)) {
|
||||
_estimatorInitialized &= ~EST_Z;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (z_stddev_ok) {
|
||||
_validZ = true;
|
||||
_estimatorInitialized |= EST_Z;
|
||||
}
|
||||
}
|
||||
|
||||
// is terrain valid?
|
||||
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
|
||||
|
||||
if (_validTZ) {
|
||||
if (_estimatorInitialized & EST_TZ) {
|
||||
if (!tz_stddev_ok) {
|
||||
_validTZ = false;
|
||||
_estimatorInitialized &= ~EST_TZ;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (tz_stddev_ok) {
|
||||
_validTZ = true;
|
||||
_estimatorInitialized |= EST_TZ;
|
||||
}
|
||||
}
|
||||
|
||||
// timeouts
|
||||
if (_validXY) {
|
||||
_time_last_xy = _timeStamp;
|
||||
}
|
||||
|
||||
if (_validZ) {
|
||||
_time_last_z = _timeStamp;
|
||||
}
|
||||
|
||||
if (_validTZ) {
|
||||
_time_last_tz = _timeStamp;
|
||||
}
|
||||
|
||||
// check timeouts
|
||||
checkTimeouts();
|
||||
|
||||
// if we have no lat, lon initialize projection at 0,0
|
||||
if (_validXY && !_map_ref.init_done) {
|
||||
if ((_estimatorInitialized & EST_XY) && !_map_ref.init_done) {
|
||||
map_projection_init(&_map_ref,
|
||||
_init_origin_lat.get(),
|
||||
_init_origin_lon.get());
|
||||
@@ -389,7 +393,7 @@ void BlockLocalPositionEstimator::update()
|
||||
// don't want it to take too long
|
||||
if (!PX4_ISFINITE(_x(i))) {
|
||||
reinit_x = true;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i);
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%d) not finite", msg_label, i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -399,7 +403,7 @@ void BlockLocalPositionEstimator::update()
|
||||
_x(i) = 0;
|
||||
}
|
||||
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
|
||||
}
|
||||
|
||||
// force P symmetry and reinitialize P if necessary
|
||||
@@ -409,7 +413,7 @@ void BlockLocalPositionEstimator::update()
|
||||
for (int j = 0; j <= i; j++) {
|
||||
if (!PX4_ISFINITE(_P(i, j))) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub,
|
||||
"[lpe] reinit P (%d, %d) not finite", i, j);
|
||||
"%sreinit P (%d, %d) not finite", msg_label, i, j);
|
||||
reinit_P = true;
|
||||
}
|
||||
|
||||
@@ -417,7 +421,7 @@ void BlockLocalPositionEstimator::update()
|
||||
// make sure diagonal elements are positive
|
||||
if (_P(i, i) <= 0) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub,
|
||||
"[lpe] reinit P (%d, %d) negative", i, j);
|
||||
"%sreinit P (%d, %d) negative", msg_label, i, j);
|
||||
reinit_P = true;
|
||||
}
|
||||
|
||||
@@ -442,7 +446,7 @@ void BlockLocalPositionEstimator::update()
|
||||
|
||||
// sensor corrections/ initializations
|
||||
if (gpsUpdated) {
|
||||
if (!_gpsInitialized) {
|
||||
if (_sensorTimeout & SENSOR_GPS) {
|
||||
gpsInit();
|
||||
|
||||
} else {
|
||||
@@ -451,7 +455,7 @@ void BlockLocalPositionEstimator::update()
|
||||
}
|
||||
|
||||
if (baroUpdated) {
|
||||
if (!_baroInitialized) {
|
||||
if (_sensorTimeout & SENSOR_BARO) {
|
||||
baroInit();
|
||||
|
||||
} else {
|
||||
@@ -460,7 +464,7 @@ void BlockLocalPositionEstimator::update()
|
||||
}
|
||||
|
||||
if (lidarUpdated) {
|
||||
if (!_lidarInitialized) {
|
||||
if (_sensorTimeout & SENSOR_LIDAR) {
|
||||
lidarInit();
|
||||
|
||||
} else {
|
||||
@@ -469,7 +473,7 @@ void BlockLocalPositionEstimator::update()
|
||||
}
|
||||
|
||||
if (sonarUpdated) {
|
||||
if (!_sonarInitialized) {
|
||||
if (_sensorTimeout & SENSOR_SONAR) {
|
||||
sonarInit();
|
||||
|
||||
} else {
|
||||
@@ -478,19 +482,16 @@ void BlockLocalPositionEstimator::update()
|
||||
}
|
||||
|
||||
if (flowUpdated) {
|
||||
if (!_flowInitialized) {
|
||||
if (_sensorTimeout & SENSOR_FLOW) {
|
||||
flowInit();
|
||||
|
||||
} else {
|
||||
perf_begin(_loop_perf);// TODO
|
||||
flowCorrect();
|
||||
//perf_count(_interval_perf);
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
}
|
||||
|
||||
if (visionUpdated) {
|
||||
if (!_visionInitialized) {
|
||||
if (_sensorTimeout & SENSOR_VISION) {
|
||||
visionInit();
|
||||
|
||||
} else {
|
||||
@@ -499,7 +500,7 @@ void BlockLocalPositionEstimator::update()
|
||||
}
|
||||
|
||||
if (mocapUpdated) {
|
||||
if (!_mocapInitialized) {
|
||||
if (_sensorTimeout & SENSOR_MOCAP) {
|
||||
mocapInit();
|
||||
|
||||
} else {
|
||||
@@ -508,7 +509,7 @@ void BlockLocalPositionEstimator::update()
|
||||
}
|
||||
|
||||
if (landUpdated) {
|
||||
if (!_landInitialized) {
|
||||
if (_sensorTimeout & SENSOR_LAND) {
|
||||
landInit();
|
||||
|
||||
} else {
|
||||
@@ -522,7 +523,7 @@ void BlockLocalPositionEstimator::update()
|
||||
publishEstimatorStatus();
|
||||
_pub_innov.update();
|
||||
|
||||
if (_validXY) {
|
||||
if ((_estimatorInitialized & EST_XY)) {
|
||||
publishGlobalPos();
|
||||
}
|
||||
}
|
||||
@@ -542,155 +543,31 @@ void BlockLocalPositionEstimator::update()
|
||||
|
||||
void BlockLocalPositionEstimator::checkTimeouts()
|
||||
{
|
||||
if (_timeStamp - _time_last_xy > EST_SRC_TIMEOUT) {
|
||||
if (!_xyTimeout) {
|
||||
_xyTimeout = true;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] xy timeout ");
|
||||
}
|
||||
|
||||
} else if (_xyTimeout) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] xy resume ");
|
||||
_xyTimeout = false;
|
||||
}
|
||||
|
||||
if (_timeStamp - _time_last_z > EST_SRC_TIMEOUT) {
|
||||
if (!_zTimeout) {
|
||||
_zTimeout = true;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] z timeout ");
|
||||
}
|
||||
|
||||
} else if (_zTimeout) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] z resume ");
|
||||
_zTimeout = false;
|
||||
}
|
||||
|
||||
if (_timeStamp - _time_last_tz > EST_SRC_TIMEOUT) {
|
||||
if (!_tzTimeout) {
|
||||
_tzTimeout = true;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] tz timeout ");
|
||||
}
|
||||
|
||||
} else if (_tzTimeout) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] tz resume ");
|
||||
_tzTimeout = false;
|
||||
}
|
||||
|
||||
lidarCheckTimeout();
|
||||
sonarCheckTimeout();
|
||||
baroCheckTimeout();
|
||||
gpsCheckTimeout();
|
||||
lidarCheckTimeout();
|
||||
flowCheckTimeout();
|
||||
sonarCheckTimeout();
|
||||
visionCheckTimeout();
|
||||
mocapCheckTimeout();
|
||||
landCheckTimeout();
|
||||
}
|
||||
|
||||
float BlockLocalPositionEstimator::agl()
|
||||
bool BlockLocalPositionEstimator::landed()
|
||||
{
|
||||
return _x(X_tz) - _x(X_z);
|
||||
if (!(_fusion.get() & FUSE_LAND)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool disarmed_not_falling = (!_sub_armed.get().armed) && (!_sub_land.get().freefall);
|
||||
|
||||
if (!(_sub_land.get().landed || disarmed_not_falling)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
|
||||
{
|
||||
// don't correct bias when rotating rapidly
|
||||
float ang_speed = sqrtf(
|
||||
_sub_att.get().rollspeed * _sub_att.get().rollspeed +
|
||||
_sub_att.get().pitchspeed * _sub_att.get().pitchspeed +
|
||||
_sub_att.get().yawspeed * _sub_att.get().yawspeed);
|
||||
|
||||
if (ang_speed > 1) {
|
||||
dx(X_bx) = 0;
|
||||
dx(X_by) = 0;
|
||||
dx(X_bz) = 0;
|
||||
}
|
||||
|
||||
// if xy not valid, stop estimating
|
||||
if (!_validXY) {
|
||||
dx(X_x) = 0;
|
||||
dx(X_y) = 0;
|
||||
dx(X_vx) = 0;
|
||||
dx(X_vy) = 0;
|
||||
dx(X_bx) = 0;
|
||||
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;
|
||||
}
|
||||
|
||||
// saturate bias
|
||||
float bx = dx(X_bx) + _x(X_bx);
|
||||
float by = dx(X_by) + _x(X_by);
|
||||
float bz = dx(X_bz) + _x(X_bz);
|
||||
|
||||
if (std::abs(bx) > BIAS_MAX) {
|
||||
bx = BIAS_MAX * bx / std::abs(bx);
|
||||
dx(X_bx) = bx - _x(X_bx);
|
||||
}
|
||||
|
||||
if (std::abs(by) > BIAS_MAX) {
|
||||
by = BIAS_MAX * by / std::abs(by);
|
||||
dx(X_by) = by - _x(X_by);
|
||||
}
|
||||
|
||||
if (std::abs(bz) > BIAS_MAX) {
|
||||
bz = BIAS_MAX * bz / std::abs(bz);
|
||||
dx(X_bz) = bz - _x(X_bz);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void BlockLocalPositionEstimator::covPropagationLogic(Matrix<float, n_x, n_x> &dP)
|
||||
{
|
||||
for (int i = 0; i < n_x; i++) {
|
||||
if (_P(i, i) > P_MAX) {
|
||||
// if diagonal element greater than max, stop propagating
|
||||
dP(i, i) = 0;
|
||||
|
||||
for (int j = 0; j < n_x; j++) {
|
||||
dP(i, j) = 0;
|
||||
dP(j, i) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::detectDistanceSensors()
|
||||
{
|
||||
for (int i = 0; i < N_DIST_SUBS; i++) {
|
||||
uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];
|
||||
|
||||
if (s == _sub_lidar || s == _sub_sonar) { continue; }
|
||||
|
||||
if (s->updated()) {
|
||||
s->update();
|
||||
|
||||
if (s->get().timestamp == 0) { continue; }
|
||||
|
||||
if (s->get().type == \
|
||||
distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
|
||||
_sub_lidar == NULL) {
|
||||
_sub_lidar = s;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] Lidar detected with ID %i", i);
|
||||
|
||||
} else if (s->get().type == \
|
||||
distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
|
||||
_sub_sonar == NULL) {
|
||||
_sub_sonar = s;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] Sonar detected with ID %i", i);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void BlockLocalPositionEstimator::publishLocalPos()
|
||||
{
|
||||
const Vector<float, n_x> &xLP = _xLowPass.getState();
|
||||
@@ -717,14 +594,14 @@ void BlockLocalPositionEstimator::publishLocalPos()
|
||||
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
|
||||
&& PX4_ISFINITE(_x(X_vz))) {
|
||||
_pub_lpos.get().timestamp = _timeStamp;
|
||||
_pub_lpos.get().xy_valid = _validXY;
|
||||
_pub_lpos.get().z_valid = _validZ;
|
||||
_pub_lpos.get().v_xy_valid = _validXY;
|
||||
_pub_lpos.get().v_z_valid = _validZ;
|
||||
_pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY;
|
||||
_pub_lpos.get().z_valid = _estimatorInitialized & EST_Z;
|
||||
_pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY;
|
||||
_pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z;
|
||||
_pub_lpos.get().x = xLP(X_x); // north
|
||||
_pub_lpos.get().y = xLP(X_y); // east
|
||||
|
||||
if (_pub_agl_z.get()) {
|
||||
if (_fusion.get() & FUSE_PUB_AGL_Z) {
|
||||
_pub_lpos.get().z = -_aglLowPass.getState(); // agl
|
||||
|
||||
} else {
|
||||
@@ -736,8 +613,8 @@ void BlockLocalPositionEstimator::publishLocalPos()
|
||||
_pub_lpos.get().vz = xLP(X_vz); // down
|
||||
|
||||
_pub_lpos.get().yaw = _eul(2);
|
||||
_pub_lpos.get().xy_global = _validXY;
|
||||
_pub_lpos.get().z_global = _baroInitialized;
|
||||
_pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;
|
||||
_pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO);
|
||||
_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;
|
||||
@@ -749,7 +626,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
|
||||
// if you are in terrain following mode this is important
|
||||
// so that if terrain estimation fails there isn't a
|
||||
// sudden altitude jump
|
||||
_pub_lpos.get().dist_bottom_valid = _validZ;
|
||||
_pub_lpos.get().dist_bottom_valid = _estimatorInitialized & EST_Z;
|
||||
_pub_lpos.get().eph = eph;
|
||||
_pub_lpos.get().epv = epv;
|
||||
_pub_lpos.update();
|
||||
@@ -767,22 +644,8 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
|
||||
|
||||
_pub_est_status.get().n_states = n_x;
|
||||
_pub_est_status.get().nan_flags = 0;
|
||||
_pub_est_status.get().health_flags =
|
||||
((_baroFault > FAULT_NONE) << SENSOR_BARO)
|
||||
+ ((_gpsFault > FAULT_NONE) << SENSOR_GPS)
|
||||
+ ((_lidarFault > FAULT_NONE) << SENSOR_LIDAR)
|
||||
+ ((_flowFault > FAULT_NONE) << SENSOR_FLOW)
|
||||
+ ((_sonarFault > FAULT_NONE) << SENSOR_SONAR)
|
||||
+ ((_visionFault > FAULT_NONE) << SENSOR_VISION)
|
||||
+ ((_mocapFault > FAULT_NONE) << SENSOR_MOCAP);
|
||||
_pub_est_status.get().timeout_flags =
|
||||
(_baroInitialized << SENSOR_BARO)
|
||||
+ (_gpsInitialized << SENSOR_GPS)
|
||||
+ (_flowInitialized << SENSOR_FLOW)
|
||||
+ (_lidarInitialized << SENSOR_LIDAR)
|
||||
+ (_sonarInitialized << SENSOR_SONAR)
|
||||
+ (_visionInitialized << SENSOR_VISION)
|
||||
+ (_mocapInitialized << SENSOR_MOCAP);
|
||||
_pub_est_status.get().health_flags = _sensorFault;
|
||||
_pub_est_status.get().timeout_flags = _sensorTimeout;
|
||||
_pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph;
|
||||
_pub_est_status.get().pos_vert_accuracy = _pub_gpos.get().epv;
|
||||
|
||||
@@ -830,8 +693,8 @@ void BlockLocalPositionEstimator::publishGlobalPos()
|
||||
_pub_gpos.get().eph = eph;
|
||||
_pub_gpos.get().epv = epv;
|
||||
_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().terrain_alt_valid = _estimatorInitialized & EST_TZ;
|
||||
_pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);
|
||||
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;
|
||||
_pub_gpos.update();
|
||||
}
|
||||
@@ -931,22 +794,14 @@ void BlockLocalPositionEstimator::updateSSParams()
|
||||
|
||||
void BlockLocalPositionEstimator::predict()
|
||||
{
|
||||
// if can't update anything, don't propagate
|
||||
// state or covariance
|
||||
if (!_validXY && !_validZ) { return; }
|
||||
|
||||
if (integrate) {
|
||||
matrix::Quaternion<float> q(&_sub_att.get().q[0]);
|
||||
_eul = matrix::Euler<float>(q);
|
||||
_R_att = matrix::Dcm<float>(q);
|
||||
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
|
||||
// note, bias is removed in dynamics function
|
||||
_u = _R_att * a;
|
||||
_u(U_az) += 9.81f; // add g
|
||||
|
||||
} else {
|
||||
_u = Vector3f(0, 0, 0);
|
||||
}
|
||||
// get acceleration
|
||||
matrix::Quaternion<float> q(&_sub_att.get().q[0]);
|
||||
_eul = matrix::Euler<float>(q);
|
||||
_R_att = matrix::Dcm<float>(q);
|
||||
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
|
||||
// note, bias is removed in dynamics function
|
||||
_u = _R_att * a;
|
||||
_u(U_az) += 9.81f; // add g
|
||||
|
||||
// update state space based on new states
|
||||
updateSSStates();
|
||||
@@ -963,12 +818,62 @@ void BlockLocalPositionEstimator::predict()
|
||||
k4 = dynamics(h, _x + k3 * h, _u);
|
||||
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
|
||||
|
||||
// don't integrate position if no valid xy data
|
||||
if (!(_estimatorInitialized & EST_XY)) {
|
||||
dx(X_x) = 0;
|
||||
dx(X_vx) = 0;
|
||||
dx(X_y) = 0;
|
||||
dx(X_vy) = 0;
|
||||
}
|
||||
|
||||
// don't integrate z if no valid z data
|
||||
if (!(_estimatorInitialized & EST_Z)) {
|
||||
dx(X_z) = 0;
|
||||
}
|
||||
|
||||
// don't integrate tz if no valid tz data
|
||||
if (!(_estimatorInitialized & EST_TZ)) {
|
||||
dx(X_tz) = 0;
|
||||
}
|
||||
|
||||
// saturate bias
|
||||
float bx = dx(X_bx) + _x(X_bx);
|
||||
float by = dx(X_by) + _x(X_by);
|
||||
float bz = dx(X_bz) + _x(X_bz);
|
||||
|
||||
if (std::abs(bx) > BIAS_MAX) {
|
||||
bx = BIAS_MAX * bx / std::abs(bx);
|
||||
dx(X_bx) = bx - _x(X_bx);
|
||||
}
|
||||
|
||||
if (std::abs(by) > BIAS_MAX) {
|
||||
by = BIAS_MAX * by / std::abs(by);
|
||||
dx(X_by) = by - _x(X_by);
|
||||
}
|
||||
|
||||
if (std::abs(bz) > BIAS_MAX) {
|
||||
bz = BIAS_MAX * bz / std::abs(bz);
|
||||
dx(X_bz) = bz - _x(X_bz);
|
||||
}
|
||||
|
||||
// propagate
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
|
||||
_B * _R * _B.transpose() + _Q) * getDt();
|
||||
covPropagationLogic(dP);
|
||||
|
||||
// covariance propagation logic
|
||||
for (int i = 0; i < n_x; i++) {
|
||||
if (_P(i, i) > P_MAX) {
|
||||
// if diagonal element greater than max, stop propagating
|
||||
dP(i, i) = 0;
|
||||
|
||||
for (int j = 0; j < n_x; j++) {
|
||||
dP(i, j) = 0;
|
||||
dP(j, i) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_P += dP;
|
||||
|
||||
_xLowPass.update(_x);
|
||||
@@ -991,7 +896,7 @@ int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
|
||||
*periods = i_hist;
|
||||
|
||||
if (t_delay > DELAY_MAX) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] delayed data too old: %8.4f", double(t_delay));
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay));
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user