fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 05:13:34 +08:00
Code Issues 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
co-authored by : bresch
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/land_detector/LandDetector.cpp
+78 -2
View File
@@ -48,7 +48,18 @@ namespace land_detector
LandDetector::LandDetector() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{}
{
_land_detected.ground_contact = true;
_land_detected.maybe_landed = true;
_land_detected.landed = true;
_land_detected.in_ground_effect = true;
_land_detected.in_descend = false;
_land_detected.has_low_throttle = false;
_land_detected.vertical_movement = false;
_land_detected.horizontal_movement = false;
_land_detected.close_to_ground_or_skipped_check = true;
_land_detected.at_rest = true;
}
LandDetector::~LandDetector()
{
@@ -91,6 +102,18 @@ void LandDetector::Run()
_acceleration = matrix::Vector3f{vehicle_acceleration.xyz};
}
vehicle_angular_velocity_s vehicle_angular_velocity{};
if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) {
_angular_velocity = matrix::Vector3f{vehicle_angular_velocity.xyz};
static constexpr float GYRO_NORM_MAX = math::radians(3.f); // 3 degrees/second
if (_angular_velocity.norm() > GYRO_NORM_MAX) {
_time_last_move_detect_us = vehicle_angular_velocity.timestamp_sample;
}
}
_vehicle_local_position_sub.update(&_vehicle_local_position);
_vehicle_status_sub.update(&_vehicle_status);
@@ -124,13 +147,18 @@ void LandDetector::Run()
const bool landDetected = _landed_hysteresis.get_state();
const bool in_ground_effect = _ground_effect_hysteresis.get_state();
UpdateVehicleAtRest();
const bool at_rest = landDetected && _at_rest;
// publish at 1 Hz, very first time, or when the result has changed
if ((hrt_elapsed_time(&_land_detected.timestamp) >= 1_s) ||
(_land_detected.landed != landDetected) ||
(_land_detected.freefall != freefallDetected) ||
(_land_detected.maybe_landed != maybe_landedDetected) ||
(_land_detected.ground_contact != ground_contactDetected) ||
(_land_detected.in_ground_effect != in_ground_effect)) {
(_land_detected.in_ground_effect != in_ground_effect) ||
(_land_detected.at_rest != at_rest)) {
if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */
// We did take off
@@ -147,6 +175,7 @@ void LandDetector::Run()
_land_detected.horizontal_movement = _get_horizontal_movement();
_land_detected.vertical_movement = _get_vertical_movement();
_land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check();
_land_detected.at_rest = at_rest;
_land_detected.timestamp = hrt_absolute_time();
_vehicle_land_detected_pub.publish(_land_detected);
}
@@ -178,4 +207,51 @@ void LandDetector::Run()
}
}
void LandDetector::UpdateVehicleAtRest()
{
if (_sensor_selection_sub.updated()) {
sensor_selection_s sensor_selection{};
_sensor_selection_sub.copy(&sensor_selection);
if (sensor_selection.gyro_device_id != _device_id_gyro) {
bool gyro_status_found = false;
// find corresponding vehicle_imu_status instance
for (uint8_t imu_instance = 0; imu_instance < 4; 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.gyro_device_id != 0) && (imu_status.gyro_device_id == sensor_selection.gyro_device_id)) {
_vehicle_imu_status_sub.ChangeInstance(imu_instance);
_device_id_gyro = sensor_selection.gyro_device_id;
gyro_status_found = true;
break;
}
}
if (!gyro_status_found) {
PX4_WARN("IMU status not found for gyro %" PRId32, sensor_selection.gyro_device_id);
}
}
}
vehicle_imu_status_s imu_status;
if (_vehicle_imu_status_sub.update(&imu_status)) {
static constexpr float GYRO_VIBE_METRIC_MAX = 0.02f; // gyro_vibration_metric * dt * 4.0e4f > is_moving_scaler)
static constexpr float ACCEL_VIBE_METRIC_MAX = 1.2f; // accel_vibration_metric * dt * 2.1e2f > is_moving_scaler
if ((imu_status.gyro_vibration_metric > GYRO_VIBE_METRIC_MAX)
|| (imu_status.accel_vibration_metric > ACCEL_VIBE_METRIC_MAX)) {
_time_last_move_detect_us = imu_status.timestamp;
}
}
_at_rest = (hrt_elapsed_time(&_time_last_move_detect_us) > 1_s);
}
} // namespace land_detector
src/modules/land_detector/LandDetector.h
+15 -7
View File
@@ -59,7 +59,10 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
@@ -148,6 +151,7 @@ protected:
vehicle_status_s _vehicle_status{};
matrix::Vector3f _acceleration{};
matrix::Vector3f _angular_velocity{};
bool _armed{false};
bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state
@@ -156,17 +160,14 @@ protected:
private:
void Run() override;
vehicle_land_detected_s _land_detected = {
.timestamp = 0,
.freefall = false,
.ground_contact = true,
.maybe_landed = true,
.landed = true,
};
void UpdateVehicleAtRest();
vehicle_land_detected_s _land_detected{};
hrt_abstime _takeoff_time{0};
hrt_abstime _total_flight_time{0}; ///< total vehicle flight time in microseconds
hrt_abstime _time_last_move_detect_us{0}; // timestamp of last movement detection event in microseconds
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
uORB::Publication<vehicle_land_detected_s> _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)};
@@ -174,11 +175,18 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
uint32_t _device_id_gyro{0};
bool _at_rest{true};
DEFINE_PARAMETERS_CUSTOM_PARENT(
ModuleParams,
(ParamInt<px4::params::LND_FLIGHT_T_HI>) _param_total_flight_time_high,
src/modules/land_detector/MulticopterLandDetector.cpp
+2 -3
View File
@@ -285,11 +285,10 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
}
// Next look if all rotation angles are not moving.
vehicle_angular_velocity_s vehicle_angular_velocity{};
_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
const Vector2f angular_velocity{vehicle_angular_velocity.xyz[0], vehicle_angular_velocity.xyz[1]};
const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
matrix::Vector2f angular_velocity{_angular_velocity(0), _angular_velocity(1)};
if (angular_velocity.norm() > max_rotation_scaled) {
return false;
}
src/modules/land_detector/MulticopterLandDetector.h
+1 -2
View File
@@ -44,7 +44,6 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/takeoff_status.h>
@@ -110,7 +109,7 @@ private:
uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};