PX4模块设计之二十七：LandDetector模块

1. LandDetector模块简介

支持多种机型：

1. 固定翼：fixedwing
2. 多旋翼：multicopter
3. 垂直起降：vtol
4. 车辆：rover
5. 飞艇：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的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类进行区分：

1. 固定翼：fixedwing ───> FixedwingLandDetector
2. 多旋翼：multicopter ───> MulticopterLandDetector
3. 垂直起降：vtol ───> VtolLandDetector
4. 车辆：rover ───> RoverLandDetector
5. 飞艇：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_sub{this, 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(&param_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::Vector3f{vehicle_acceleration.xyz}
 ├──> <_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
 │   └──> <_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
【7】PX4模块设计之三十：Hysteresis类