Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

move vehicle at rest detection ekf2 -> land_detector

Browse Source 
- move vehicle at reset detection ekf2 -> land_detector
 - ekf_unit: reduce init period
   - Fake fusion is when at rest is quite strong and makes the variance reduce rapidly. Reduce the amount of time we wait before checking if the variances are still large enough.
 - ekf_unit: reduce minimum vel/pos variance required after init
   - Fake pos fusion has a low observation noise, making the vel/pos variances reduce quickly.

Co-authored-by:: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2022-02-01 17:50:19 -05:00
committed by GitHub
parent e387f302f9
commit f3e2a197ad
19 changed files with 408 additions and 441 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+1 -60
View File
@@ -160,7 +160,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_pcoef_yp(_params->static_pressure_coef_yp),
_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
_param_ekf2_move_test(_params->is_moving_scaler),
_param_ekf2_mag_check(_params->check_mag_strength),
_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
@@ -385,8 +384,6 @@ void EKF2::Run()
_gyro_calibration_count = imu.gyro_calibration_count;
} else {
bool reset_actioned = false;
if ((imu.accel_calibration_count != _accel_calibration_count)
|| (imu.accel_device_id != _device_id_accel)) {
@@ -398,8 +395,6 @@ void EKF2::Run()
// reset bias learning
_accel_cal = {};
reset_actioned = true;
}
if ((imu.gyro_calibration_count != _gyro_calibration_count)
@@ -413,12 +408,6 @@ void EKF2::Run()
// reset bias learning
_gyro_cal = {};
reset_actioned = true;
}
if (reset_actioned) {
SelectImuStatus();
}
}
}
@@ -460,8 +449,6 @@ void EKF2::Run()
// reset bias learning
_accel_cal = {};
SelectImuStatus();
}
if (_device_id_gyro != sensor_selection.gyro_device_id) {
@@ -472,8 +459,6 @@ void EKF2::Run()
// reset bias learning
_gyro_cal = {};
SelectImuStatus();
}
}
}
@@ -482,8 +467,6 @@ void EKF2::Run()
if (imu_updated) {
const hrt_abstime now = imu_sample_new.time_us;
UpdateImuStatus();
// push imu data into estimator
_ekf.setIMUData(imu_sample_new);
PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)
@@ -534,6 +517,7 @@ void EKF2::Run()
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
const bool was_in_air = _ekf.control_status_flags().in_air;
_ekf.set_in_air_status(!vehicle_land_detected.landed);
_ekf.set_vehicle_at_rest(vehicle_land_detected.at_rest);
if (_armed && (_param_ekf2_gnd_eff_dz.get() > 0.f)) {
if (!_had_valid_terrain) {
@@ -1194,7 +1178,6 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
_ekf.get_ekf_soln_status(&status.solution_status_flags);
_ekf.getImuVibrationMetrics().copyTo(status.vibe);
// reset counters
status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter;
@@ -1401,48 +1384,6 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
return amsl_hgt + _wgs84_hgt_offset;
}
void EKF2::SelectImuStatus()
{
for (uint8_t imu_instance = 0; imu_instance < MAX_NUM_IMUS; imu_instance++) {
uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_instance};
vehicle_imu_status_s imu_status{};
imu_status_sub.copy(&imu_status);
if (imu_status.accel_device_id == _device_id_accel) {
_vehicle_imu_status_sub.ChangeInstance(imu_instance);
return;
}
}
PX4_WARN("%d - IMU status not found for accel %" PRId32 ", gyro %" PRId32, _instance, _device_id_accel,
_device_id_gyro);
}
void EKF2::UpdateImuStatus()
{
vehicle_imu_status_s imu_status;
if (_vehicle_imu_status_sub.update(&imu_status)) {
if (imu_status.accel_device_id != _device_id_accel) {
SelectImuStatus();
return;
}
const float dt = _ekf.get_dt_imu_avg();
// accel -> delta velocity
_ekf.setDeltaVelocityHighFrequencyVibrationMetric(imu_status.accel_vibration_metric * dt);
// gyro -> delta angle
_ekf.setDeltaAngleHighFrequencyVibrationMetric(imu_status.gyro_vibration_metric * dt);
// note: the coning metric uses the cross product of two consecutive angular velocities
// this is why the conversion to delta angle requires dt^2
_ekf.setDeltaAngleConingMetric(imu_status.gyro_coning_vibration * dt * dt);
}
}
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF airspeed sample