PX4模块设计之二十七：LandDetector模块
Posted lida2003
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了PX4模块设计之二十七：LandDetector模块相关的知识，希望对你有一定的参考价值。
PX4模块设计之二十七：LandDetector模块
1. LandDetector模块简介
支持多种机型：
- 固定翼：fixedwing
- 多旋翼：multicopter
- 垂直起降：vtol
- 车辆：rover
- 飞艇：airship
注：模块仅支持start/stop/status命令。
### Description
Module to detect the freefall and landed state of the vehicle, and publishing the `vehicle_land_detected` topic.
Each vehicle type (multirotor, fixedwing, vtol, ...) provides its own algorithm, taking into account various
states, such as commanded thrust, arming state and vehicle motion.
### Implementation
Every type is implemented in its own class with a common base class. The base class maintains a state (landed,
maybe_landed, ground_contact). Each possible state is implemented in the derived classes. A hysteresis and a fixed
priority of each internal state determines the actual land_detector state.
#### Multicopter Land Detector
**ground_contact**: thrust setpoint and velocity in z-direction must be below a defined threshold for time
GROUND_CONTACT_TRIGGER_TIME_US. When ground_contact is detected, the position controller turns off the thrust setpoint
in body x and y.
**maybe_landed**: it requires ground_contact together with a tighter thrust setpoint threshold and no velocity in the
horizontal direction. The trigger time is defined by MAYBE_LAND_TRIGGER_TIME. When maybe_landed is detected, the
position controller sets the thrust setpoint to zero.
**landed**: it requires maybe_landed to be true for time LAND_DETECTOR_TRIGGER_TIME_US.
The module runs periodically on the HP work queue.
land_detector <command> [arguments...]
Commands:
start Start the background task
fixedwing|multicopter|vtol|rover|airship Select vehicle type
stop
status print status info
注：print_usage函数是具体对应实现。
class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::ScheduledWorkItem
注：LandDetector模块采用了ModuleBase和WorkQueue设计。
2. 模块入口函数
2.1 主入口land_detector_main
同样继承了ModuleBase，由ModuleBase(https://blog.csdn.net/lida2003/article/details/126173134)的main来完成模块入口。
land_detector_main
└──> return LandDetector::main(argc, argv)
2.2 自定义子命令custom_command
自定义命令仅支持start/stop/status命令。
LandDetector::custom_command
└──> return print_usage("unknown command");
3. LandDetector模块重要函数
3.1 task_spawn
不同的机型使用不同的继承LandDetector类进行区分：
- 固定翼：fixedwing ───> FixedwingLandDetector
- 多旋翼：multicopter ───> MulticopterLandDetector
- 垂直起降：vtol ───> VtolLandDetector
- 车辆：rover ───> RoverLandDetector
- 飞艇：airship ───> AirshipLandDetector
模块使用启动函数LandDetector::start来启动模块。
LandDetector::task_spawn
├──> <fixedwing>
│ └──> obj = new FixedwingLandDetector()
├──> <multicopter>
│ └──> obj = new MulticopterLandDetector()
├──> <vtol>
│ └──> obj = new VtolLandDetector()
├──> <rover>
│ └──> obj = new RoverLandDetector()
├──> <airship>
│ └──> obj = new AirshipLandDetector()
├──> <else>
│ ├──> print_usage("unknown mode")
│ └──> return PX4_ERROR
├──> <obj == nullptr>
│ ├──> PX4_ERR("alloc failed")
│ └──> return PX4_ERROR
├──> strncpy(_currentMode, argv[1], sizeof(_currentMode) - 1);_currentMode[sizeof(_currentMode) - 1] = '\\0'; // Remember current active mode
├──> _object.store(obj)
├──> _task_id = task_id_is_work_queue
├──> obj->start()
└──> return PX4_OK
注：不同机型的LandDetector继承类，分别重载了各自对应的实现方法。从LandDetector业务框架流程的角度还是在LandDetector::Run中实现。
3.1.1 FixedwingLandDetector
class FixedwingLandDetector final : public LandDetector
public:
FixedwingLandDetector();
~FixedwingLandDetector() override = default;
protected:
bool _get_landed_state() override;
void _set_hysteresis_factor(const int factor) override ;
private:
... //略
3.1.2 MulticopterLandDetector
class MulticopterLandDetector : public LandDetector
public:
MulticopterLandDetector();
~MulticopterLandDetector() override = default;
protected:
void _update_params() override;
void _update_topics() override;
bool _get_landed_state() override;
bool _get_ground_contact_state() override;
bool _get_maybe_landed_state() override;
bool _get_freefall_state() override;
bool _get_ground_effect_state() override;
bool _get_in_descend() override return _in_descend;
bool _get_has_low_throttle() override return _has_low_throttle;
bool _get_horizontal_movement() override return _horizontal_movement;
bool _get_vertical_movement() override return _vertical_movement;
bool _get_close_to_ground_or_skipped_check() override return _close_to_ground_or_skipped_check;
void _set_hysteresis_factor(const int factor) override;
private:
... //略
3.1.3 VtolLandDetector
class VtolLandDetector : public MulticopterLandDetector
public:
VtolLandDetector() = default;
~VtolLandDetector() override = default;
protected:
void _update_topics() override;
bool _get_landed_state() override;
bool _get_maybe_landed_state() override;
bool _get_freefall_state() override;
private:
... //略
3.1.4 RoverLandDetector
class RoverLandDetector : public LandDetector
public:
RoverLandDetector() = default;
~RoverLandDetector() override = default;
protected:
bool _get_ground_contact_state() override;
bool _get_landed_state() override;
void _set_hysteresis_factor(const int factor) override ;
3.1.5 AirshipLandDetector
class AirshipLandDetector : public LandDetector
public:
AirshipLandDetector() = default;
~AirshipLandDetector() override = default;
protected:
bool _get_ground_contact_state() override;
bool _get_landed_state() override;
void _set_hysteresis_factor(const int factor) override ;
;
3.2 instantiate
注：鉴于该模块不采用任务(线程)，所以ModuleBase::run_trampoline无需执行，所以可以不实现。
3.3 start
LandDetector::start
├──> ScheduleDelayed(50_ms) // 默认50ms进行一次ScheduleNow
└──> _vehicle_local_position_sub.registerCallback() //飞行器位置消息发布时，回调一次ScheduleNow
注：位置发生改变直接回调ScheduleNow，从而触发LandDetector::Run。
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_subthis, ORB_ID(vehicle_local_position);
3.4 Run
LandDetector业务监测框架代码（飞行器位置发生改变或者50ms定时间隔轮询）实现逻辑流程如下所示：
LandDetector::Run
├──> ScheduleDelayed(50_ms) // push backup schedule, 定时延期（当没有其他消息回调时）
├──> perf_begin(_cycle_perf)
│
├──> <_parameter_update_sub.updated() || (_land_detected.timestamp == 0)>
│ ├──> _parameter_update_sub.copy(¶m_update)
│ ├──> updateParams()
│ ├──> 【可重载】_update_params()
│ ├──> _total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32
│ └──> _total_flight_time |= static_cast<uint32_t>(_param_total_flight_time_low.get())
│
├──> <_actuator_armed_sub.update(&actuator_armed)>
│ └──> _armed = actuator_armed.armed
├──> <_vehicle_acceleration_sub.update(&vehicle_acceleration)>
│ └──> _acceleration = matrix::Vector3fvehicle_acceleration.xyz
├──> <_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)>
│ ├──> _angular_velocity = matrix::Vector3fvehicle_angular_velocity.xyz
│ ├──> static constexpr float GYRO_NORM_MAX = math::radians(3.f); // 3 degrees/second
│ └──> <_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)
├──> 【可重载】_update_topics()
├──> <!_dist_bottom_is_observable>
│ └──> _dist_bottom_is_observable = _vehicle_local_position.dist_bottom_sensor_bitfield & vehicle_local_position_s::DIST_BOTTOM_SENSOR_RANGE; // we consider the distance to the ground observable if the system is using a range sensor
├──> <_dist_bottom_is_observable && !_vehicle_local_position.dist_bottom_valid>
│ └──> _set_hysteresis_factor(3)
├──> <else>
│ └──> _set_hysteresis_factor(1)
│ 【开始LandDetector状态判断】
├──> const hrt_abstime now_us = hrt_absolute_time();
├──> _freefall_hysteresis.set_state_and_update(【可重载】_get_freefall_state(), now_us);
├──> _ground_contact_hysteresis.set_state_and_update(【可重载】_get_ground_contact_state(), now_us);
├──> _maybe_landed_hysteresis.set_state_and_update(【可重载】_get_maybe_landed_state(), now_us);
├──> _landed_hysteresis.set_state_and_update(【可重载】_get_landed_state(), now_us);
├──> _ground_effect_hysteresis.set_state_and_update(【可重载】_get_ground_effect_state(), now_us);
│ 【获取LandDetector状态】
├──> const bool freefallDetected = _freefall_hysteresis.get_state();
├──> const bool ground_contactDetected = _ground_contact_hysteresis.get_state();
├──> const bool maybe_landedDetected = _maybe_landed_hysteresis.get_state();
├──> 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;
│
├──> <(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.at_rest != at_rest)> // publish at 1 Hz, very first time, or when the result has changed
│ ├──> <!landDetected && _land_detected.landed && _takeoff_time == 0>
│ │ └──> _takeoff_time = now_us// only set take off time once, until disarming
│ ├──> _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_descend = 【可重载】_get_in_descend();
│ ├──> _land_detected.has_low_throttle = 【可重载】_get_has_low_throttle();
│ ├──> _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);
├──> <_takeoff_time != 0 && !_armed && _previous_armed_state>
│ ├──> _total_flight_time += now_us - _takeoff_time;
│ ├──> _takeoff_time = 0;
│ ├──> uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
│ ├──> _param_total_flight_time_high.set(flight_time);
│ ├──> _param_total_flight_time_high.commit_no_notification();
│ ├──> flight_time = _total_flight_time & 0xffffffff;
│ ├──> _param_total_flight_time_low.set(flight_time);
│ └──> _param_total_flight_time_low.commit_no_notification();
├──> _previous_armed_state = _armed
├──> perf_end(_cycle_perf)
└──> <should_exit()>
├──> ScheduleClear()
└──> exit_and_cleanup()
注：这里用到了systemlib::Hysteresis类，有一定的迟滞作用。具体详见hysteresis.h/hysteresis.cpp
4. 总结
该模块最重要的任务就是更新发布vehicle_land_detected消息。
struct vehicle_land_detected_s
uint64_t timestamp;
bool freefall;
bool ground_contact;
bool maybe_landed;
bool landed;
bool in_ground_effect;
bool in_descend;
bool has_low_throttle;
bool vertical_movement;
bool horizontal_movement;
bool close_to_ground_or_skipped_check;
bool at_rest;
uint8_t _padding0[5]; // required for logger
鉴于不同机型判断逻辑不同，设计了统一的LandDetector业务监测框架，将具体判断逻辑放到LandDetector继承类的重载函数实现，详见3.1章节。
5. 参考资料
【1】PX4开源软件框架简明简介
【2】PX4模块设计之十一：Built-In框架
【3】PX4模块设计之十二：High Resolution Timer设计
【4】PX4模块设计之十三：WorkQueue设计
【5】PX4模块设计之十七：ModuleBase模块
【6】PX4 modules_main
以上是关于PX4模块设计之二十七：LandDetector模块的主要内容，如果未能解决你的问题，请参考以下文章