forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathpullup.cpp
239 lines (215 loc) · 8.8 KB
/
pullup.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
#include "Plane.h"
/*
support for pullup after an alitude wait. Used for high altitude gliders
*/
#if AP_PLANE_GLIDER_PULLUP_ENABLED
// Pullup control parameters
const AP_Param::GroupInfo GliderPullup::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable pullup after altitude wait
// @Description: Enable pullup after altitude wait
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, GliderPullup, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: ELEV_OFS
// @DisplayName: Elevator deflection used before starting pullup
// @Description: Elevator deflection offset from -1 to 1 while waiting for airspeed to rise before starting close loop control of the pullup.
// @Range: -1.0 1.0
// @User: Advanced
AP_GROUPINFO("ELEV_OFS", 2, GliderPullup, elev_offset, 0.1f),
// @Param: NG_LIM
// @DisplayName: Maximum normal load factor during pullup
// @Description: This is the nominal maximum value of normal load factor used during the closed loop pitch rate control of the pullup.
// @Range: 1.0 4.0
// @User: Advanced
AP_GROUPINFO("NG_LIM", 3, GliderPullup, ng_limit, 2.0f),
// @Param: NG_JERK_LIM
// @DisplayName: Maximum normal load factor rate of change during pullup
// @Description: The normal load factor used for closed loop pitch rate control of the pullup will be ramped up to the value set by PUP_NG_LIM at the rate of change set by this parameter. The parameter value specified will be scaled internally by 1/EAS2TAS.
// @Units: 1/s
// @Range: 0.1 10.0
// @User: Advanced
AP_GROUPINFO("NG_JERK_LIM", 4, GliderPullup, ng_jerk_limit, 4.0f),
// @Param: PITCH
// @DisplayName: Target pitch angle during pullup
// @Description: The vehicle will attempt achieve this pitch angle during the pull-up maneouvre.
// @Units: deg
// @Range: -5 15
// @User: Advanced
AP_GROUPINFO("PITCH", 5, GliderPullup, pitch_dem, 3),
// @Param: ARSPD_START
// @DisplayName: Pullup target airspeed
// @Description: Target airspeed for initial airspeed wait
// @Units: m/s
// @Range: 0 100
// @User: Advanced
AP_GROUPINFO("ARSPD_START", 6, GliderPullup, airspeed_start, 30),
// @Param: PITCH_START
// @DisplayName: Pullup target pitch
// @Description: Target pitch for initial pullup
// @Units: deg
// @Range: -80 0
// @User: Advanced
AP_GROUPINFO("PITCH_START", 7, GliderPullup, pitch_start, -60),
AP_GROUPEND
};
// constructor
GliderPullup::GliderPullup(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
/*
return true if in a pullup manoeuvre at the end of NAV_ALTITUDE_WAIT
*/
bool GliderPullup::in_pullup(void) const
{
return plane.control_mode == &plane.mode_auto &&
plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT &&
stage != Stage::NONE;
}
/*
start a pullup maneouvre, called when NAV_ALTITUDE_WAIT has reached
altitude or exceeded descent rate
*/
bool GliderPullup::pullup_start(void)
{
if (enable != 1) {
return false;
}
// release balloon
SRV_Channels::set_output_scaled(SRV_Channel::k_lift_release, 100);
stage = Stage::WAIT_AIRSPEED;
plane.auto_state.idle_mode = false;
float aspeed;
if (!plane.ahrs.airspeed_estimate(aspeed)) {
aspeed = -1;
}
gcs().send_text(MAV_SEVERITY_INFO, "Start pullup airspeed %.1fm/s at %.1fm AMSL", aspeed, plane.current_loc.alt*0.01);
return true;
}
/*
first stage pullup from balloon release, verify completion
*/
bool GliderPullup::verify_pullup(void)
{
const auto &ahrs = plane.ahrs;
const auto ¤t_loc = plane.current_loc;
const auto &aparm = plane.aparm;
switch (stage) {
case Stage::WAIT_AIRSPEED: {
float aspeed;
if (ahrs.airspeed_estimate(aspeed) && (aspeed > airspeed_start || ahrs.pitch_sensor*0.01 > pitch_start)) {
gcs().send_text(MAV_SEVERITY_INFO, "Pullup airspeed %.1fm/s alt %.1fm AMSL", aspeed, current_loc.alt*0.01);
stage = Stage::WAIT_PITCH;
}
return false;
}
case Stage::WAIT_PITCH: {
if (ahrs.pitch_sensor*0.01 > pitch_start && fabsf(ahrs.roll_sensor*0.01) < 90) {
gcs().send_text(MAV_SEVERITY_INFO, "Pullup pitch p=%.1f r=%.1f alt %.1fm AMSL",
ahrs.pitch_sensor*0.01,
ahrs.roll_sensor*0.01,
current_loc.alt*0.01);
stage = Stage::WAIT_LEVEL;
}
return false;
}
case Stage::PUSH_NOSE_DOWN: {
if (fabsf(ahrs.roll_sensor*0.01) < aparm.roll_limit) {
stage = Stage::WAIT_LEVEL;
}
return false;
}
case Stage::WAIT_LEVEL: {
// When pitch has raised past lower limit used by speed controller, wait for airspeed to approach
// target value before handing over control of pitch demand to speed controller
bool pitchup_complete = ahrs.pitch_sensor*0.01 > MIN(0, aparm.pitch_limit_min);
const float pitch_lag_time = 1.0f * sqrtf(ahrs.get_EAS2TAS());
float aspeed;
const float aspeed_derivative = (ahrs.get_accel().x + GRAVITY_MSS * ahrs.get_DCM_rotation_body_to_ned().c.x) / ahrs.get_EAS2TAS();
bool airspeed_low = ahrs.airspeed_estimate(aspeed) ? (aspeed + aspeed_derivative * pitch_lag_time) < 0.01f * (float)plane.target_airspeed_cm : true;
bool roll_control_lost = fabsf(ahrs.roll_sensor*0.01) > aparm.roll_limit;
if (pitchup_complete && airspeed_low && !roll_control_lost) {
gcs().send_text(MAV_SEVERITY_INFO, "Pullup level r=%.1f p=%.1f alt %.1fm AMSL",
ahrs.roll_sensor*0.01, ahrs.pitch_sensor*0.01, current_loc.alt*0.01);
break;
} else if (pitchup_complete && roll_control_lost) {
// push nose down and wait to get roll control back
gcs().send_text(MAV_SEVERITY_ALERT, "Pullup level roll bad r=%.1f p=%.1f",
ahrs.roll_sensor*0.01,
ahrs.pitch_sensor*0.01);
stage = Stage::PUSH_NOSE_DOWN;
}
return false;
}
case Stage::NONE:
break;
}
// all done
stage = Stage::NONE;
return true;
}
/*
stabilize during pullup from balloon drop
*/
void GliderPullup::stabilize_pullup(void)
{
const float speed_scaler = plane.get_speed_scaler();
switch (stage) {
case Stage::WAIT_AIRSPEED: {
plane.pitchController.reset_I();
plane.yawController.reset_I();
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_offset*4500);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
plane.nav_pitch_cd = 0;
plane.nav_roll_cd = 0;
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(0, speed_scaler));
ng_demand = 0.0;
break;
}
case Stage::WAIT_PITCH: {
plane.yawController.reset_I();
plane.nav_roll_cd = 0;
plane.nav_pitch_cd = 0;
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(0, speed_scaler));
float aspeed;
const auto &ahrs = plane.ahrs;
if (ahrs.airspeed_estimate(aspeed)) {
// apply a rate of change limit to the ng pullup demand
ng_demand += MAX(ng_jerk_limit / ahrs.get_EAS2TAS(), 0.1f) * plane.scheduler.get_loop_period_s();
ng_demand = MIN(ng_demand, ng_limit);
const float VTAS_ref = ahrs.get_EAS2TAS() * aspeed;
const float pullup_accel = ng_demand * GRAVITY_MSS;
const float demanded_rate_dps = degrees(pullup_accel / VTAS_ref);
const uint32_t elev_trim_offset_cd = 4500.0f * elev_offset * (1.0f - ng_demand / ng_limit);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_trim_offset_cd + plane.pitchController.get_rate_out(demanded_rate_dps, speed_scaler));
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_offset*4500);
}
break;
}
case Stage::PUSH_NOSE_DOWN: {
plane.nav_pitch_cd = plane.aparm.pitch_limit_min*100;
plane.stabilize_pitch();
plane.nav_roll_cd = 0;
plane.stabilize_roll();
plane.stabilize_yaw();
ng_demand = 0.0f;
break;
}
case Stage::WAIT_LEVEL:
plane.nav_pitch_cd = MAX((plane.aparm.pitch_limit_min + 5), pitch_dem)*100;
plane.stabilize_pitch();
plane.nav_roll_cd = 0;
plane.stabilize_roll();
plane.stabilize_yaw();
ng_demand = 0.0f;
break;
case Stage::NONE:
break;
}
// we have done stabilisation
plane.last_stabilize_ms = AP_HAL::millis();
}
#endif // AP_PLANE_GLIDER_PULLUP_ENABLED