mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
LPE land bug fix and switch to fusion bit mask.
This commit is contained in:
James Goppert
Lorenz Meier
parent
f263ea7f7e
commit
6ff85fb927
@ -23,7 +23,7 @@ param set SENS_BOARD_X_OFF 0.000001
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set NAV_DLL_ACT 2
|
||||
param set COM_DISARM_LAND 3
|
||||
param set NAV_ACC_RAD 12.0
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set RTL_DESCEND_ALT 10.0
|
||||
param set RTL_LAND_DELAY 0
|
||||
@ -41,14 +41,26 @@ param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_XY_VEL_P 0.15
|
||||
param set MPC_XY_VEL_I 0.2
|
||||
|
||||
# changes for LPE indoor flight
|
||||
# changes for optical flow navigation
|
||||
param set MC_PITCH_P 5.0
|
||||
param set MC_ROLL_P 5.0
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLLRATE_I 0.05
|
||||
param set MC_PITCHRATE_I 0.05
|
||||
param set LPE_GPS_ON 0
|
||||
param set MPC_ALT_MODE 1
|
||||
param set LPE_T_MAX_GRADE 0
|
||||
param set MPC_XY_VEL_MAX 2
|
||||
param set MPC_Z_VEL_MAX 2
|
||||
param set MPC_TILTMAX_AIR 10
|
||||
param set MPC_TILTMAX_LND 10
|
||||
param set MPC_XY_P 0.5
|
||||
param set MIS_TAKEOFF_ALT 2
|
||||
param set NAV_ACC_RAD 1.0
|
||||
param set CBRK_GPSFAIL 240024
|
||||
param set LPE_FUSION 246
|
||||
# 11110110 no vis (1 << 3) yaw and no gps (1 << 0)
|
||||
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
@ -81,6 +93,6 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 14556
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
|
||||
sdlog2 start -r 100 -e -t -a
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -3,7 +3,6 @@
|
||||
#include <px4_posix.h>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <matrix/Matrix.hpp>
|
||||
|
||||
@ -40,28 +39,6 @@ static const float BIAS_MAX = 1e-1f;
|
||||
static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP;
|
||||
static const size_t N_DIST_SUBS = 4;
|
||||
|
||||
enum fault_t {
|
||||
FAULT_NONE = 0,
|
||||
FAULT_MINOR,
|
||||
FAULT_SEVERE
|
||||
};
|
||||
|
||||
enum sensor_t {
|
||||
SENSOR_BARO = 0,
|
||||
SENSOR_GPS,
|
||||
SENSOR_LIDAR,
|
||||
SENSOR_FLOW,
|
||||
SENSOR_SONAR,
|
||||
SENSOR_VISION,
|
||||
SENSOR_MOCAP,
|
||||
SENSOR_LAND,
|
||||
};
|
||||
|
||||
// change this to set when
|
||||
// the system will abort correcting a measurement
|
||||
// given a fault has been detected
|
||||
static const fault_t fault_lvl_disable = FAULT_SEVERE;
|
||||
|
||||
// for fault detection
|
||||
// chi squared distribution, false alarm probability 0.0001
|
||||
// see fault_table.py
|
||||
@ -135,15 +112,39 @@ 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_vx, Y_land_vy, Y_land_agl = 0, n_y_land};
|
||||
enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM, n_poll};
|
||||
enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land};
|
||||
enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll};
|
||||
enum {
|
||||
FUSE_GPS = 1 << 0,
|
||||
FUSE_FLOW = 1 << 1,
|
||||
FUSE_VIS_POS = 1 << 2,
|
||||
FUSE_VIS_YAW = 1 << 3,
|
||||
FUSE_LAND = 1 << 4,
|
||||
FUSE_PUB_AGL_Z = 1 << 5,
|
||||
FUSE_FLOW_GYRO_COMP = 1 << 6,
|
||||
FUSE_BARO = 1 << 7,
|
||||
};
|
||||
|
||||
enum sensor_t {
|
||||
SENSOR_BARO = 1 << 0,
|
||||
SENSOR_GPS = 1 << 1,
|
||||
SENSOR_LIDAR = 1 << 2,
|
||||
SENSOR_FLOW = 1 << 3,
|
||||
SENSOR_SONAR = 1 << 4,
|
||||
SENSOR_VISION = 1 << 5,
|
||||
SENSOR_MOCAP = 1 << 6,
|
||||
SENSOR_LAND = 1 << 7,
|
||||
};
|
||||
|
||||
enum estimate_t {
|
||||
EST_XY = 1 << 0,
|
||||
EST_Z = 1 << 1,
|
||||
EST_TZ = 1 << 2,
|
||||
};
|
||||
|
||||
// public methods
|
||||
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:
|
||||
@ -153,6 +154,11 @@ private:
|
||||
|
||||
// methods
|
||||
// ----------------------------
|
||||
//
|
||||
Vector<float, n_x> dynamics(
|
||||
float t,
|
||||
const Vector<float, n_x> &x,
|
||||
const Vector<float, n_u> &u);
|
||||
void initP();
|
||||
void initSS();
|
||||
void updateSSStates();
|
||||
@ -189,7 +195,6 @@ private:
|
||||
int flowMeasure(Vector<float, n_y_flow> &y);
|
||||
void flowCorrect();
|
||||
void flowInit();
|
||||
void flowDeinit();
|
||||
void flowCheckTimeout();
|
||||
|
||||
// vision
|
||||
@ -214,10 +219,8 @@ private:
|
||||
void checkTimeouts();
|
||||
|
||||
// misc
|
||||
float agl();
|
||||
void correctionLogic(Vector<float, n_x> &dx);
|
||||
void covPropagationLogic(Matrix<float, n_x, n_x> &dP);
|
||||
void detectDistanceSensors();
|
||||
inline float agl() { return _x(X_tz) - _x(X_z); }
|
||||
bool landed();
|
||||
int getDelayPeriods(float delay, uint8_t *periods);
|
||||
|
||||
// publications
|
||||
@ -257,7 +260,7 @@ private:
|
||||
struct map_projection_reference_s _map_ref;
|
||||
|
||||
// general parameters
|
||||
BlockParamInt _pub_agl_z;
|
||||
BlockParamInt _fusion;
|
||||
BlockParamFloat _vxy_pub_thresh;
|
||||
BlockParamFloat _z_pub_thresh;
|
||||
|
||||
@ -277,7 +280,6 @@ private:
|
||||
BlockParamFloat _baro_stddev;
|
||||
|
||||
// gps parameters
|
||||
BlockParamInt _gps_on;
|
||||
BlockParamFloat _gps_delay;
|
||||
BlockParamFloat _gps_xy_stddev;
|
||||
BlockParamFloat _gps_z_stddev;
|
||||
@ -290,21 +292,22 @@ private:
|
||||
BlockParamFloat _vision_xy_stddev;
|
||||
BlockParamFloat _vision_z_stddev;
|
||||
BlockParamFloat _vision_delay;
|
||||
BlockParamInt _vision_on;
|
||||
|
||||
// mocap parameters
|
||||
BlockParamFloat _mocap_p_stddev;
|
||||
|
||||
// flow parameters
|
||||
BlockParamInt _flow_gyro_comp;
|
||||
BlockParamFloat _flow_z_offset;
|
||||
BlockParamFloat _flow_scale;
|
||||
//BlockParamFloat _flow_board_x_offs;
|
||||
//BlockParamFloat _flow_board_y_offs;
|
||||
BlockParamInt _flow_min_q;
|
||||
BlockParamFloat _flow_r;
|
||||
BlockParamFloat _flow_rr;
|
||||
|
||||
// land parameters
|
||||
BlockParamFloat _land_z_stddev;
|
||||
BlockParamFloat _land_vxy_stddev;
|
||||
|
||||
// process noise
|
||||
BlockParamFloat _pn_p_noise_density;
|
||||
@ -342,10 +345,8 @@ private:
|
||||
// misc
|
||||
px4_pollfd_struct_t _polls[3];
|
||||
uint64_t _timeStamp;
|
||||
uint64_t _timeStampLastBaro;
|
||||
uint64_t _time_last_hist;
|
||||
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;
|
||||
@ -356,17 +357,6 @@ private:
|
||||
uint64_t _time_last_mocap;
|
||||
uint64_t _time_last_land;
|
||||
|
||||
// initialization flags
|
||||
bool _receivedGps;
|
||||
bool _baroInitialized;
|
||||
bool _gpsInitialized;
|
||||
bool _lidarInitialized;
|
||||
bool _sonarInitialized;
|
||||
bool _flowInitialized;
|
||||
bool _visionInitialized;
|
||||
bool _mocapInitialized;
|
||||
bool _landInitialized;
|
||||
|
||||
// reference altitudes
|
||||
float _altOrigin;
|
||||
bool _altOriginInitialized;
|
||||
@ -374,28 +364,13 @@ private:
|
||||
float _gpsAltOrigin;
|
||||
|
||||
// status
|
||||
bool _validXY;
|
||||
bool _validZ;
|
||||
bool _validTZ;
|
||||
bool _xyTimeout;
|
||||
bool _zTimeout;
|
||||
bool _tzTimeout;
|
||||
bool _receivedGps;
|
||||
bool _lastArmedState;
|
||||
|
||||
// sensor faults
|
||||
fault_t _baroFault;
|
||||
fault_t _gpsFault;
|
||||
fault_t _lidarFault;
|
||||
fault_t _flowFault;
|
||||
fault_t _sonarFault;
|
||||
fault_t _visionFault;
|
||||
fault_t _mocapFault;
|
||||
fault_t _landFault;
|
||||
|
||||
// performance counters
|
||||
perf_counter_t _loop_perf;
|
||||
perf_counter_t _interval_perf;
|
||||
perf_counter_t _err_perf;
|
||||
// masks
|
||||
uint8_t _sensorTimeout;
|
||||
uint8_t _sensorFault;
|
||||
uint8_t _estimatorInitialized;
|
||||
|
||||
// state space
|
||||
Vector<float, n_x> _x; // state vector
|
||||
|
||||
@ -2,14 +2,6 @@
|
||||
|
||||
// 16 is max name length
|
||||
|
||||
/**
|
||||
* Publish AGL as Z
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_PUB_AGL_Z, 0);
|
||||
|
||||
/**
|
||||
* Optical flow z offset from center
|
||||
*
|
||||
@ -33,15 +25,26 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(LPE_FLW_SCALE, 1.3f);
|
||||
|
||||
/**
|
||||
* Optical flow gyro compensation
|
||||
* Optical flow rotation (roll/pitch) noise gain
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min -1
|
||||
* @max 1
|
||||
* @unit m/s / (rad)
|
||||
* @min 0.1
|
||||
* @max 10.0
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LPE_FLW_GYRO_CMP, 1);
|
||||
PARAM_DEFINE_FLOAT(LPE_FLW_R, 7.0f);
|
||||
|
||||
/**
|
||||
* Optical flow angular velocity noise gain
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m/s / (rad/s)
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_FLW_RR, 7.0f);
|
||||
|
||||
/**
|
||||
* Optical flow minimum quality threshold
|
||||
@ -131,19 +134,11 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.02f);
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 3
|
||||
* @max 100
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_BAR_Z, 3.0f);
|
||||
|
||||
/**
|
||||
* Enables GPS data, also forces alt init with GPS
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LPE_GPS_ON, 1);
|
||||
|
||||
/**
|
||||
* GPS delay compensaton
|
||||
*
|
||||
@ -251,19 +246,11 @@ PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.1f);
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 2
|
||||
* @max 100
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f);
|
||||
|
||||
/**
|
||||
* Vision correction
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LPE_VIS_ON, 1);
|
||||
|
||||
/**
|
||||
* Vicon position standard deviation.
|
||||
*
|
||||
@ -357,7 +344,7 @@ PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.001f);
|
||||
* @max 90
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
|
||||
PARAM_DEFINE_FLOAT(LPE_LAT, 47.397742f);
|
||||
|
||||
/**
|
||||
* Local origin longitude for nav w/o GPS
|
||||
@ -368,7 +355,7 @@ PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
|
||||
* @max 180
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
|
||||
PARAM_DEFINE_FLOAT(LPE_LON, 8.545594);
|
||||
|
||||
/**
|
||||
* Cut frequency for state publication
|
||||
@ -413,3 +400,43 @@ PARAM_DEFINE_FLOAT(LPE_Z_PUB, 1.0f);
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_LAND_Z, 0.03f);
|
||||
|
||||
/**
|
||||
* Land detector xy velocity standard deviation
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m/s
|
||||
* @min 0.01
|
||||
* @max 10.0
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_LAND_VXY, 0.05f);
|
||||
|
||||
/**
|
||||
* Integer bitmask controlling data fusion
|
||||
*
|
||||
* Set bits in the following positions to enable:
|
||||
* 0 : Set to true to fuse GPS data if available, also requires GPS for altitude init
|
||||
* 1 : Set to true to fuse optical flow data if available
|
||||
* 2 : Set to true to fuse vision position
|
||||
* 3 : Set to true to fuse vision yaw
|
||||
* 4 : Set to true to fuse land detector
|
||||
* 5 : Set to true to publish AGL as local position down component
|
||||
* 6 : Set to true to enable flow gyro compensation
|
||||
* 7 : Set to true to enable baro fusion
|
||||
*
|
||||
* default (247, no vision yaw)
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @min 0
|
||||
* @max 255
|
||||
* @bit 0 fuse GPS, requires GPS for alt. init
|
||||
* @bit 1 fuse optical flow
|
||||
* @bit 2 fuse vision position
|
||||
* @bit 3 fuse vision yaw
|
||||
* @bit 4 fuse land detector
|
||||
* @bit 5 pub agl as lpos down
|
||||
* @bit 6 flow gyro compensation
|
||||
* @bit 7 fuse baro
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LPE_FUSION, 247);
|
||||
|
||||
@ -26,8 +26,8 @@ void BlockLocalPositionEstimator::baroInit()
|
||||
"[lpe] baro init %d m std %d cm",
|
||||
(int)_baroStats.getMean()(0),
|
||||
(int)(100 * _baroStats.getStdDev()(0)));
|
||||
_baroInitialized = true;
|
||||
_baroFault = FAULT_NONE;
|
||||
_sensorTimeout &= ~SENSOR_BARO;
|
||||
_sensorFault &= ~SENSOR_BARO;
|
||||
|
||||
if (!_altOriginInitialized) {
|
||||
_altOriginInitialized = true;
|
||||
@ -74,25 +74,21 @@ void BlockLocalPositionEstimator::baroCorrect()
|
||||
float beta = (r.transpose() * (S_I * r))(0, 0);
|
||||
|
||||
if (beta > BETA_TABLE[n_y_baro]) {
|
||||
if (_baroFault < FAULT_MINOR) {
|
||||
if (beta > 2.0f * BETA_TABLE[n_y_baro]) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
|
||||
double(r(0)), double(beta));
|
||||
}
|
||||
|
||||
_baroFault = FAULT_MINOR;
|
||||
if (!(_sensorFault & SENSOR_BARO)) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
|
||||
double(r(0)), double(beta));
|
||||
_sensorFault |= SENSOR_BARO;
|
||||
}
|
||||
|
||||
} else if (_baroFault) {
|
||||
_baroFault = FAULT_NONE;
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK");
|
||||
} else if (_sensorFault & SENSOR_BARO) {
|
||||
_sensorFault &= ~SENSOR_BARO;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK");
|
||||
}
|
||||
|
||||
// kalman filter correction if no fault
|
||||
if (_baroFault < fault_lvl_disable) {
|
||||
if (!(_sensorFault & SENSOR_BARO)) {
|
||||
Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
@ -101,8 +97,8 @@ void BlockLocalPositionEstimator::baroCorrect()
|
||||
void BlockLocalPositionEstimator::baroCheckTimeout()
|
||||
{
|
||||
if (_timeStamp - _time_last_baro > BARO_TIMEOUT) {
|
||||
if (_baroInitialized) {
|
||||
_baroInitialized = false;
|
||||
if (!(_sensorTimeout & SENSOR_BARO)) {
|
||||
_sensorTimeout |= SENSOR_BARO;
|
||||
_baroStats.reset();
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro timeout ");
|
||||
}
|
||||
|
||||
@ -29,17 +29,11 @@ void BlockLocalPositionEstimator::flowInit()
|
||||
"quality %d std %d",
|
||||
int(_flowQStats.getMean()(0)),
|
||||
int(_flowQStats.getStdDev()(0)));
|
||||
_flowInitialized = true;
|
||||
_flowFault = FAULT_NONE;
|
||||
_sensorTimeout &= ~SENSOR_FLOW;
|
||||
_sensorFault &= ~SENSOR_FLOW;
|
||||
}
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::flowDeinit()
|
||||
{
|
||||
_flowInitialized = false;
|
||||
_flowQStats.reset();
|
||||
}
|
||||
|
||||
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
|
||||
{
|
||||
// check for sane pitch/roll
|
||||
@ -60,7 +54,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
|
||||
}
|
||||
|
||||
// calculate range to center of image for flow
|
||||
if (!_validTZ) {
|
||||
if (!(_estimatorInitialized & EST_TZ)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -82,7 +76,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
|
||||
float gyro_x_rad = 0;
|
||||
float gyro_y_rad = 0;
|
||||
|
||||
if (_flow_gyro_comp.get()) {
|
||||
if (_fusion.get() & FUSE_FLOW_GYRO_COMP) {
|
||||
gyro_x_rad = _flow_gyro_x_high_pass.update(
|
||||
_sub_flow.get().gyro_x_rate_integral);
|
||||
gyro_y_rad = _flow_gyro_y_high_pass.update(
|
||||
@ -161,7 +155,15 @@ void BlockLocalPositionEstimator::flowCorrect()
|
||||
// compute polynomial value
|
||||
float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h;
|
||||
|
||||
R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev;
|
||||
float rotrate_sq = _sub_att.get().rollspeed * _sub_att.get().rollspeed
|
||||
+ _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
|
||||
+ _sub_att.get().yawspeed * _sub_att.get().yawspeed;
|
||||
|
||||
float rot_sq = _eul(0) * _eul(0) + _eul(1) * _eul(1);
|
||||
|
||||
R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev +
|
||||
_flow_r.get() * _flow_r.get() * rot_sq +
|
||||
_flow_rr.get() * _flow_rr.get() * rotrate_sq;
|
||||
R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);
|
||||
|
||||
// residual
|
||||
@ -179,21 +181,20 @@ void BlockLocalPositionEstimator::flowCorrect()
|
||||
float beta = (r.transpose() * (S_I * r))(0, 0);
|
||||
|
||||
if (beta > BETA_TABLE[n_y_flow]) {
|
||||
if (_flowFault < FAULT_MINOR) {
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
|
||||
_flowFault = FAULT_MINOR;
|
||||
if (!(_sensorFault & SENSOR_FLOW)) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
|
||||
_sensorFault |= SENSOR_FLOW;
|
||||
}
|
||||
|
||||
} else if (_flowFault) {
|
||||
_flowFault = FAULT_NONE;
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
|
||||
} else if (_sensorFault & SENSOR_FLOW) {
|
||||
_sensorFault &= ~SENSOR_FLOW;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
|
||||
}
|
||||
|
||||
if (_flowFault < fault_lvl_disable) {
|
||||
if (!(_sensorFault & SENSOR_FLOW)) {
|
||||
Matrix<float, n_x, n_y_flow> K =
|
||||
_P * C.transpose() * S_I;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
|
||||
@ -204,8 +205,9 @@ void BlockLocalPositionEstimator::flowCorrect()
|
||||
void BlockLocalPositionEstimator::flowCheckTimeout()
|
||||
{
|
||||
if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
|
||||
if (_flowInitialized) {
|
||||
flowDeinit();
|
||||
if (!(_sensorTimeout & SENSOR_FLOW)) {
|
||||
_sensorTimeout |= SENSOR_FLOW;
|
||||
_flowQStats.reset();
|
||||
mavlink_log_critical(&mavlink_log_pub, "[lpe] flow timeout ");
|
||||
}
|
||||
}
|
||||
|
||||
@ -42,8 +42,8 @@ void BlockLocalPositionEstimator::gpsInit()
|
||||
double gpsLon = _gpsStats.getMean()(1);
|
||||
float gpsAlt = _gpsStats.getMean()(2);
|
||||
|
||||
_gpsInitialized = true;
|
||||
_gpsFault = FAULT_NONE;
|
||||
_sensorTimeout &= ~SENSOR_GPS;
|
||||
_sensorFault &= ~SENSOR_GPS;
|
||||
_gpsStats.reset();
|
||||
|
||||
if (!_receivedGps) {
|
||||
@ -55,25 +55,29 @@ void BlockLocalPositionEstimator::gpsInit()
|
||||
_gpsAltOrigin = gpsAlt + _x(X_z);
|
||||
|
||||
// find lat, lon of current origin by subtracting x and y
|
||||
double gpsLatOrigin = 0;
|
||||
double gpsLonOrigin = 0;
|
||||
// reproject at current coordinates
|
||||
map_projection_init(&_map_ref, gpsLat, gpsLon);
|
||||
// find origin
|
||||
map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
|
||||
// reinit origin
|
||||
map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
|
||||
// if not using vision position since vision will
|
||||
// have it's own origin, not necessarily where vehicle starts
|
||||
if (!(_fusion.get() & FUSE_VIS_POS)) {
|
||||
double gpsLatOrigin = 0;
|
||||
double gpsLonOrigin = 0;
|
||||
// reproject at current coordinates
|
||||
map_projection_init(&_map_ref, gpsLat, gpsLon);
|
||||
// find origin
|
||||
map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
|
||||
// reinit origin
|
||||
map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
|
||||
|
||||
// always override alt origin on first GPS to fix
|
||||
// possible baro offset in global altitude at init
|
||||
_altOrigin = _gpsAltOrigin;
|
||||
_altOriginInitialized = true;
|
||||
// always override alt origin on first GPS to fix
|
||||
// possible baro offset in global altitude at init
|
||||
_altOrigin = _gpsAltOrigin;
|
||||
_altOriginInitialized = true;
|
||||
}
|
||||
|
||||
PX4_INFO("[lpe] gps init "
|
||||
"lat %6.2f lon %6.2f alt %5.1f m",
|
||||
gpsLatOrigin,
|
||||
gpsLonOrigin,
|
||||
double(_gpsAltOrigin));
|
||||
gpsLat,
|
||||
gpsLon,
|
||||
double(gpsAlt));
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -187,26 +191,22 @@ void BlockLocalPositionEstimator::gpsCorrect()
|
||||
float beta = (r.transpose() * (S_I * r))(0, 0);
|
||||
|
||||
if (beta > BETA_TABLE[n_y_gps]) {
|
||||
if (_gpsFault < FAULT_MINOR) {
|
||||
if (beta > 3.0f * BETA_TABLE[n_y_gps]) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
|
||||
double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
|
||||
double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
|
||||
}
|
||||
|
||||
_gpsFault = FAULT_MINOR;
|
||||
if (!(_sensorFault & SENSOR_GPS)) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
|
||||
double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
|
||||
double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
|
||||
_sensorFault |= SENSOR_GPS;
|
||||
}
|
||||
|
||||
} else if (_gpsFault) {
|
||||
_gpsFault = FAULT_NONE;
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
|
||||
} else if (_sensorFault & SENSOR_GPS) {
|
||||
_sensorFault &= ~SENSOR_GPS;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
|
||||
}
|
||||
|
||||
// kalman filter correction if no hard fault
|
||||
if (_gpsFault < fault_lvl_disable) {
|
||||
if (!(_sensorFault & SENSOR_GPS)) {
|
||||
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
@ -215,8 +215,8 @@ void BlockLocalPositionEstimator::gpsCorrect()
|
||||
void BlockLocalPositionEstimator::gpsCheckTimeout()
|
||||
{
|
||||
if (_timeStamp - _time_last_gps > GPS_TIMEOUT) {
|
||||
if (_gpsInitialized) {
|
||||
_gpsInitialized = false;
|
||||
if (!(_sensorTimeout & SENSOR_GPS)) {
|
||||
_sensorTimeout |= SENSOR_GPS;
|
||||
_gpsStats.reset();
|
||||
mavlink_log_critical(&mavlink_log_pub, "[lpe] GPS timeout ");
|
||||
}
|
||||
|
||||
@ -22,8 +22,8 @@ void BlockLocalPositionEstimator::landInit()
|
||||
// if finished
|
||||
if (_landCount > REQ_LAND_INIT_COUNT) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land init");
|
||||
_landInitialized = true;
|
||||
_landFault = FAULT_NONE;
|
||||
_sensorTimeout &= ~SENSOR_LAND;
|
||||
_sensorFault &= ~SENSOR_LAND;
|
||||
}
|
||||
}
|
||||
|
||||
@ -54,8 +54,8 @@ void BlockLocalPositionEstimator::landCorrect()
|
||||
// use parameter covariance
|
||||
SquareMatrix<float, n_y_land> R;
|
||||
R.setZero();
|
||||
R(Y_land_vx, Y_land_vx) = 0.1;
|
||||
R(Y_land_vy, Y_land_vy) = 0.1;
|
||||
R(Y_land_vx, Y_land_vx) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
|
||||
R(Y_land_vy, Y_land_vy) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
|
||||
R(Y_land_agl, Y_land_agl) = _land_z_stddev.get() * _land_z_stddev.get();
|
||||
|
||||
// residual
|
||||
@ -68,24 +68,23 @@ void BlockLocalPositionEstimator::landCorrect()
|
||||
float beta = (r.transpose() * (S_I * r))(0, 0);
|
||||
|
||||
if (beta > BETA_TABLE[n_y_land]) {
|
||||
if (_landFault < FAULT_MINOR) {
|
||||
_landFault = FAULT_MINOR;
|
||||
if (!(_sensorFault & SENSOR_LAND)) {
|
||||
_sensorFault |= SENSOR_LAND;
|
||||
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");
|
||||
} else if (_sensorFault & SENSOR_LAND) {
|
||||
_sensorFault &= ~SENSOR_LAND;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK");
|
||||
}
|
||||
|
||||
// kalman filter correction if no fault
|
||||
if (_landFault < fault_lvl_disable) {
|
||||
if (!(_sensorFault & SENSOR_LAND)) {
|
||||
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;
|
||||
}
|
||||
@ -94,10 +93,11 @@ void BlockLocalPositionEstimator::landCorrect()
|
||||
void BlockLocalPositionEstimator::landCheckTimeout()
|
||||
{
|
||||
if (_timeStamp - _time_last_land > LAND_TIMEOUT) {
|
||||
if (_landInitialized) {
|
||||
_landInitialized = false;
|
||||
if (!(_sensorTimeout & SENSOR_LAND)) {
|
||||
_sensorTimeout |= SENSOR_LAND;
|
||||
_landCount = 0;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land timeout ");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -25,8 +25,8 @@ void BlockLocalPositionEstimator::lidarInit()
|
||||
"mean %d cm stddev %d cm",
|
||||
int(100 * _lidarStats.getMean()(0)),
|
||||
int(100 * _lidarStats.getStdDev()(0)));
|
||||
_lidarInitialized = true;
|
||||
_lidarFault = FAULT_NONE;
|
||||
_sensorTimeout &= ~SENSOR_LIDAR;
|
||||
_sensorFault &= ~SENSOR_LIDAR;
|
||||
}
|
||||
}
|
||||
|
||||
@ -100,24 +100,23 @@ void BlockLocalPositionEstimator::lidarCorrect()
|
||||
float beta = (r.transpose() * (S_I * r))(0, 0);
|
||||
|
||||
if (beta > BETA_TABLE[n_y_lidar]) {
|
||||
if (_lidarFault < FAULT_MINOR) {
|
||||
_lidarFault = FAULT_MINOR;
|
||||
if (!(_sensorFault & SENSOR_LIDAR)) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar fault, beta %5.2f", double(beta));
|
||||
_sensorFault |= SENSOR_LIDAR;
|
||||
}
|
||||
|
||||
// abort correction
|
||||
return;
|
||||
|
||||
} else if (_lidarFault) { // disable fault if ok
|
||||
_lidarFault = FAULT_NONE;
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK");
|
||||
} else if (_sensorFault & SENSOR_LIDAR) {
|
||||
_sensorFault &= ~SENSOR_LIDAR;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK");
|
||||
}
|
||||
|
||||
// kalman filter correction if no fault
|
||||
if (_lidarFault < fault_lvl_disable) {
|
||||
if (!(_sensorFault & SENSOR_LIDAR)) {
|
||||
Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
@ -126,8 +125,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
|
||||
void BlockLocalPositionEstimator::lidarCheckTimeout()
|
||||
{
|
||||
if (_timeStamp - _time_last_lidar > LIDAR_TIMEOUT) {
|
||||
if (_lidarInitialized) {
|
||||
_lidarInitialized = false;
|
||||
if (!(_sensorTimeout & SENSOR_LIDAR)) {
|
||||
_sensorTimeout |= SENSOR_LIDAR;
|
||||
_lidarStats.reset();
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar timeout ");
|
||||
}
|
||||
|
||||
@ -29,8 +29,8 @@ void BlockLocalPositionEstimator::mocapInit()
|
||||
double(_mocapStats.getStdDev()(0)),
|
||||
double(_mocapStats.getStdDev()(1)),
|
||||
double(_mocapStats.getStdDev()(2)));
|
||||
_mocapInitialized = true;
|
||||
_mocapFault = FAULT_NONE;
|
||||
_sensorTimeout &= ~SENSOR_MOCAP;
|
||||
_sensorFault &= ~SENSOR_MOCAP;
|
||||
|
||||
if (!_altOriginInitialized) {
|
||||
_altOriginInitialized = true;
|
||||
@ -81,21 +81,20 @@ void BlockLocalPositionEstimator::mocapCorrect()
|
||||
float beta = (r.transpose() * (S_I * r))(0, 0);
|
||||
|
||||
if (beta > BETA_TABLE[n_y_mocap]) {
|
||||
if (_mocapFault < FAULT_MINOR) {
|
||||
if (!(_sensorFault & SENSOR_MOCAP)) {
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
|
||||
_mocapFault = FAULT_MINOR;
|
||||
_sensorFault |= SENSOR_MOCAP;
|
||||
}
|
||||
|
||||
} else if (_mocapFault) {
|
||||
_mocapFault = FAULT_NONE;
|
||||
} else if (_sensorFault & SENSOR_MOCAP) {
|
||||
_sensorFault &= ~SENSOR_MOCAP;
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap OK");
|
||||
}
|
||||
|
||||
// kalman filter correction if no fault
|
||||
if (_mocapFault < fault_lvl_disable) {
|
||||
if (!(_sensorFault & SENSOR_MOCAP)) {
|
||||
Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
@ -104,8 +103,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
|
||||
void BlockLocalPositionEstimator::mocapCheckTimeout()
|
||||
{
|
||||
if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) {
|
||||
if (_mocapInitialized) {
|
||||
_mocapInitialized = false;
|
||||
if (!(_sensorTimeout & SENSOR_MOCAP)) {
|
||||
_sensorTimeout |= SENSOR_MOCAP;
|
||||
_mocapStats.reset();
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap timeout ");
|
||||
}
|
||||
|
||||
@ -38,8 +38,8 @@ void BlockLocalPositionEstimator::sonarInit()
|
||||
"mean %d cm std %d cm",
|
||||
int(100 * _sonarStats.getMean()(0)),
|
||||
int(100 * _sonarStats.getStdDev()(0)));
|
||||
_sonarInitialized = true;
|
||||
_sonarFault = FAULT_NONE;
|
||||
_sensorTimeout &= ~SENSOR_SONAR;
|
||||
_sensorFault &= ~SENSOR_SONAR;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -116,25 +116,24 @@ void BlockLocalPositionEstimator::sonarCorrect()
|
||||
float beta = (r.transpose() * (S_I * r))(0, 0);
|
||||
|
||||
if (beta > BETA_TABLE[n_y_sonar]) {
|
||||
if (_sonarFault < FAULT_MINOR) {
|
||||
_sonarFault = FAULT_MINOR;
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta));
|
||||
if (!(_sensorFault & SENSOR_SONAR)) {
|
||||
_sensorFault |= SENSOR_SONAR;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta));
|
||||
}
|
||||
|
||||
// abort correction
|
||||
return;
|
||||
|
||||
} else if (_sonarFault) {
|
||||
_sonarFault = FAULT_NONE;
|
||||
} else if (_sensorFault & SENSOR_SONAR) {
|
||||
_sensorFault &= ~SENSOR_SONAR;
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar OK");
|
||||
}
|
||||
|
||||
// kalman filter correction if no fault
|
||||
if (_sonarFault < fault_lvl_disable) {
|
||||
if (!(_sensorFault & SENSOR_SONAR)) {
|
||||
Matrix<float, n_x, n_y_sonar> K =
|
||||
_P * C.transpose() * S_I;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
@ -144,8 +143,8 @@ void BlockLocalPositionEstimator::sonarCorrect()
|
||||
void BlockLocalPositionEstimator::sonarCheckTimeout()
|
||||
{
|
||||
if (_timeStamp - _time_last_sonar > SONAR_TIMEOUT) {
|
||||
if (_sonarInitialized) {
|
||||
_sonarInitialized = false;
|
||||
if (!(_sensorTimeout & SENSOR_SONAR)) {
|
||||
_sensorTimeout |= SENSOR_SONAR;
|
||||
_sonarStats.reset();
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar timeout ");
|
||||
}
|
||||
|
||||
@ -34,8 +34,8 @@ void BlockLocalPositionEstimator::visionInit()
|
||||
double(_visionStats.getStdDev()(0)),
|
||||
double(_visionStats.getStdDev()(1)),
|
||||
double(_visionStats.getStdDev()(2)));
|
||||
_visionInitialized = true;
|
||||
_visionFault = FAULT_NONE;
|
||||
_sensorTimeout &= ~SENSOR_VISION;
|
||||
_sensorFault &= ~SENSOR_VISION;
|
||||
|
||||
if (!_altOriginInitialized) {
|
||||
_altOriginInitialized = true;
|
||||
@ -91,21 +91,20 @@ void BlockLocalPositionEstimator::visionCorrect()
|
||||
float beta = (r.transpose() * (S_I * r))(0, 0);
|
||||
|
||||
if (beta > BETA_TABLE[n_y_vision]) {
|
||||
if (_visionFault < FAULT_MINOR) {
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
|
||||
_visionFault = FAULT_MINOR;
|
||||
if (!(_sensorFault & SENSOR_VISION)) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
|
||||
_sensorFault |= SENSOR_VISION;
|
||||
}
|
||||
|
||||
} else if (_visionFault) {
|
||||
_visionFault = FAULT_NONE;
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK");
|
||||
} else if (_sensorFault & SENSOR_VISION) {
|
||||
_sensorFault &= ~SENSOR_VISION;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK");
|
||||
}
|
||||
|
||||
// kalman filter correction if no fault
|
||||
if (_visionFault < fault_lvl_disable) {
|
||||
if (!(_sensorFault & SENSOR_VISION)) {
|
||||
Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
@ -114,8 +113,8 @@ void BlockLocalPositionEstimator::visionCorrect()
|
||||
void BlockLocalPositionEstimator::visionCheckTimeout()
|
||||
{
|
||||
if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) {
|
||||
if (_visionInitialized) {
|
||||
_visionInitialized = false;
|
||||
if (!(_sensorTimeout & SENSOR_VISION)) {
|
||||
_sensorTimeout |= SENSOR_VISION;
|
||||
_visionStats.reset();
|
||||
mavlink_log_critical(&mavlink_log_pub, "[lpe] vision position timeout ");
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user