CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/ArduPlane/VTOL_Assist.cpp
Views: 1798
1
#include "Plane.h"
2
3
#if HAL_QUADPLANE_ENABLED
4
5
// Assistance hysteresis helpers
6
7
// Reset state
8
void VTOL_Assist::Assist_Hysteresis::reset()
9
{
10
start_ms = 0;
11
last_ms = 0;
12
active = false;
13
}
14
15
// Update state, return true when first triggered
16
bool VTOL_Assist::Assist_Hysteresis::update(const bool trigger, const uint32_t &now_ms, const uint32_t &trigger_delay_ms, const uint32_t &clear_delay_ms)
17
{
18
bool ret = false;
19
20
if (trigger) {
21
last_ms = now_ms;
22
if (start_ms == 0) {
23
start_ms = now_ms;
24
}
25
if ((now_ms - start_ms) > trigger_delay_ms) {
26
// trigger delay has elapsed
27
if (!active) {
28
// return true on first trigger
29
ret = true;
30
}
31
active = true;
32
}
33
34
} else if (active) {
35
if ((last_ms == 0) || ((now_ms - last_ms) > clear_delay_ms)) {
36
// Clear delay passed
37
reset();
38
}
39
40
} else {
41
reset();
42
}
43
44
return ret;
45
}
46
47
// Assistance not needed, reset any state
48
void VTOL_Assist::reset()
49
{
50
force_assist = false;
51
speed_assist = false;
52
angle_error.reset();
53
alt_error.reset();
54
}
55
56
/*
57
return true if the quadplane should provide stability assistance
58
*/
59
bool VTOL_Assist::should_assist(float aspeed, bool have_airspeed)
60
{
61
if (!plane.arming.is_armed_and_safety_off() || (state == STATE::ASSIST_DISABLED) || quadplane.tailsitter.is_control_surface_tailsitter()) {
62
// disarmed or disabled by aux switch or because a control surface tailsitter
63
reset();
64
return false;
65
}
66
67
if (!quadplane.tailsitter.enabled() && !( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed)
68
|| is_positive(plane.get_throttle_input())
69
|| plane.is_flying() ) ) {
70
// not in a flight mode and condition where it would be safe to turn on vertial lift motors
71
// skip this check for tailsitters because the forward and vertial motors are the same and are controled directly by throttle imput unlike other quadplanes
72
reset();
73
return false;
74
}
75
76
if (plane.flare_mode != Plane::FlareMode::FLARE_DISABLED) {
77
// Never active in fixed wing flare
78
reset();
79
return false;
80
}
81
82
force_assist = state == STATE::FORCE_ENABLED;
83
84
if (speed <= 0) {
85
// all checks disabled via speed threshold, still allow force enabled
86
speed_assist = false;
87
alt_error.reset();
88
angle_error.reset();
89
return force_assist;
90
}
91
92
// assistance due to Q_ASSIST_SPEED
93
// if option bit is enabled only allow assist with real airspeed sensor
94
speed_assist = (have_airspeed && aspeed < speed) &&
95
(!quadplane.option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || plane.ahrs.using_airspeed_sensor());
96
97
const uint32_t now_ms = AP_HAL::millis();
98
const uint32_t tigger_delay_ms = delay * 1000;
99
const uint32_t clear_delay_ms = tigger_delay_ms * 2;
100
101
/*
102
optional assistance when altitude is too close to the ground
103
*/
104
if (alt <= 0) {
105
// Alt assist disabled
106
alt_error.reset();
107
108
} else {
109
const float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
110
if (alt_error.update(height_above_ground < alt, now_ms, tigger_delay_ms, clear_delay_ms)) {
111
gcs().send_text(MAV_SEVERITY_WARNING, "Alt assist %.1fm", height_above_ground);
112
}
113
}
114
115
if (angle <= 0) {
116
// Angle assist disabled
117
angle_error.reset();
118
119
} else {
120
121
/*
122
now check if we should provide assistance due to attitude error
123
*/
124
const uint16_t allowed_envelope_error_cd = 500U;
125
const bool inside_envelope = (labs(plane.ahrs.roll_sensor) <= (plane.aparm.roll_limit*100 + allowed_envelope_error_cd)) &&
126
(plane.ahrs.pitch_sensor < (plane.aparm.pitch_limit_max*100 + allowed_envelope_error_cd)) &&
127
(plane.ahrs.pitch_sensor > (plane.aparm.pitch_limit_min*100 - allowed_envelope_error_cd));
128
129
const int32_t max_angle_cd = 100U*angle;
130
const bool inside_angle_error = (labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) < max_angle_cd) &&
131
(labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) < max_angle_cd);
132
133
if (angle_error.update(!inside_envelope && !inside_angle_error, now_ms, tigger_delay_ms, clear_delay_ms)) {
134
gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d",
135
(int)(plane.ahrs.roll_sensor/100),
136
(int)(plane.ahrs.pitch_sensor/100));
137
}
138
}
139
140
return force_assist || speed_assist || alt_error.is_active() || angle_error.is_active();
141
}
142
143
#endif // HAL_QUADPLANE_ENABLED
144
145