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