PX4模块设计之三十:Hysteresis类
1. Hysteresis类简介
根据当前状态、期望状态、状态之间迁徙时间,通过迁徙时间和期望状态更新的迟滞算法来计算当前状态的类,能够过滤及稳定状态输出,同时带来一定的时间上的迟滞。
49 class Hysteresis
50 {
51 public:
52 explicit Hysteresis(bool init_state) :
53 _state(init_state),
54 _requested_state(init_state)
55 {}
56 Hysteresis() = delete; // no default constructor
57
58 ~Hysteresis() = default;
59
60 bool get_state() const { return _state; }
61
62 void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us);
63
64 void set_state_and_update(const bool new_state, const hrt_abstime &now_us);
65
66 void update(const hrt_abstime &now_us);
67
68 private:
69
70 hrt_abstime _last_time_to_change_state{0};
71
72 hrt_abstime _time_from_true_us{0};
73 hrt_abstime _time_from_false_us{0};
74
75 bool _state;
76 bool _requested_state;
77 };
2. Hysteresis类成员变量介绍
- hrt_abstime _last_time_to_change_state: 上一次状态变更时间
- hrt_abstime _time_from_true_us: true --> false 状态变更迟滞时间
- hrt_abstime _time_from_false_us:false --> true 状态变更迟滞时间
- bool _state: 当前状态
- bool _requested_state: 期望状态
3. Hysteresis类迟滞逻辑
- 期望状态与当前状态一致,则继续维持该状态
- 期望状态与当前状态不一致:若期望状态与上次期望状态一致,则表示期望状态稳定,维持当前期望状态,等待超时状态变更
- 期望状态与当前状态不一致:若期望状态与上次期望状态不一致,则表示期望状态不稳定,更新最新的期望状态,并更新期望状态更新时间,等待状态稳定,超时变更
4. Hysteresis类重要方法
4.1 Hysteresis(bool init_state)
将状态初始化为设置的init_state(当前状态+期望状态)
Hysteresis(bool init_state)
├──> _state = init_state
└──> _requested_state = init_state
4.2 void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
设置状态迁徙需要的时间,单位us
set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
├──> <from_state>
│ └──> _time_from_true_us = new_hysteresis_time_us // true -> false 状态迁徙需要的时间
└──> <else>
└──> _time_from_false_us = new_hysteresis_time_us // false -> true 状态迁徙需要的时间
4.3 void set_state_and_update(const bool new_state, const hrt_abstime &now_us)
设置期望状态,并根据状态迁徙时间进行更新。
set_state_and_update(const bool new_state, const hrt_abstime &now_us)
├──> <new_state != _state><new_state != _requested_state>
│ ├──> _requested_state = new_state
│ └──> _last_time_to_change_state = now_us
├──> <else>
│ └──> _requested_state = _state
└──> update(now_us)
4.4 void update(const hrt_abstime &now_us)
根据迁徙时间及期望状态,更新当前状态。
update(const hrt_abstime &now_us)
└──> <_requested_state != _state>
├──> const hrt_abstime elapsed = now_us - _last_time_to_change_state
├──> <_state && !_requested_state><elapsed >= _time_from_true_us> // true -> false
│ └──> _state = false;
└──> <!_state && _requested_state><elapsed >= _time_from_false_us> // false -> true
└──> _state = true;
5. 总结
Hysteresis类,通过不同状态迁徙时间设置,期望状态调整,过滤了不稳定二值状态,同时带来了一定时间上的迟滞。