Line data Source code
1 : #include "AP_LandingGear.h"
2 :
3 : #if AP_LANDINGGEAR_ENABLED
4 :
5 : #include <AP_Math/AP_Math.h>
6 : #include <SRV_Channel/SRV_Channel.h>
7 : #include <AP_HAL/AP_HAL.h>
8 : #include <AP_Logger/AP_Logger.h>
9 : #include <GCS_MAVLink/GCS.h>
10 :
11 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
12 : #include <SITL/SITL.h>
13 : #endif
14 :
15 : #if defined(APM_BUILD_TYPE)
16 : // - this is just here to encourage the build system to supply the "legacy build defines". The actual dependecy is in the AP_LandingGear.h and AP_LandingGear_config.h headers
17 : #endif
18 :
19 : extern const AP_HAL::HAL& hal;
20 :
21 : const AP_Param::GroupInfo AP_LandingGear::var_info[] = {
22 :
23 : // 0 and 1 used by previous retract and deploy pwm, now replaced with SERVOn_MIN/MAX/REVERSED
24 :
25 : // @Param: ENABLE
26 : // @DisplayName: Enable landing gear
27 : // @Description: Enable landing gear control
28 : // @Values: 0:Disabled, 1:Enabled
29 : // @User: Standard
30 : AP_GROUPINFO_FLAGS("ENABLE", 10, AP_LandingGear, _enable, 0, AP_PARAM_FLAG_ENABLE),
31 :
32 : // @Param: STARTUP
33 : // @DisplayName: Landing Gear Startup position
34 : // @Description: Landing Gear Startup behaviour control
35 : // @Values: 0:WaitForPilotInput, 1:Retract, 2:Deploy
36 : // @User: Standard
37 : AP_GROUPINFO("STARTUP", 2, AP_LandingGear, _startup_behaviour, (uint8_t)AP_LandingGear::LandingGear_Startup_WaitForPilotInput),
38 :
39 : // @Param: DEPLOY_PIN
40 : // @DisplayName: Chassis deployment feedback pin
41 : // @Description: Pin number to use for detection of gear deployment. If set to -1 feedback is disabled. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
42 : // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
43 : // @Range: -1 127
44 : // @User: Standard
45 : // @RebootRequired: True
46 : AP_GROUPINFO("DEPLOY_PIN", 3, AP_LandingGear, _pin_deployed, -1),
47 :
48 : // @Param: DEPLOY_POL
49 : // @DisplayName: Chassis deployment feedback pin polarity
50 : // @Description: Polarity for feedback pin. If this is 1 then the pin should be high when gear are deployed. If set to 0 then then deployed gear level is low.
51 : // @Values: 0:Low,1:High
52 : // @User: Standard
53 : AP_GROUPINFO("DEPLOY_POL", 4, AP_LandingGear, _pin_deployed_polarity, 0),
54 :
55 : // @Param: WOW_PIN
56 : // @DisplayName: Weight on wheels feedback pin
57 : // @Description: Pin number to use for feedback of weight on wheels condition. If set to -1 feedback is disabled. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
58 : // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
59 : // @Range: -1 127
60 : // @User: Standard
61 : // @RebootRequired: True
62 : AP_GROUPINFO("WOW_PIN", 5, AP_LandingGear, _pin_weight_on_wheels, -1),
63 :
64 : // @Param: WOW_POL
65 : // @DisplayName: Weight on wheels feedback pin polarity
66 : // @Description: Polarity for feedback pin. If this is 1 then the pin should be high when there is weight on wheels. If set to 0 then then weight on wheels level is low.
67 : // @Values: 0:Low,1:High
68 : // @User: Standard
69 : AP_GROUPINFO("WOW_POL", 6, AP_LandingGear, _pin_weight_on_wheels_polarity, 0),
70 :
71 : // @Param: DEPLOY_ALT
72 : // @DisplayName: Landing gear deployment altitude
73 : // @Description: Altitude where the landing gear will be deployed. This should be lower than the RETRACT_ALT. If zero then altitude is not used for deploying landing gear. Only applies when vehicle is armed.
74 : // @Units: m
75 : // @Range: 0 1000
76 : // @Increment: 1
77 : // @User: Standard
78 : AP_GROUPINFO("DEPLOY_ALT", 7, AP_LandingGear, _deploy_alt_m, 0),
79 :
80 : // @Param: RETRACT_ALT
81 : // @DisplayName: Landing gear retract altitude
82 : // @Description: Altitude where the landing gear will be retracted. This should be higher than the DEPLOY_ALT. If zero then altitude is not used for retracting landing gear. Only applies when vehicle is armed.
83 : // @Units: m
84 : // @Range: 0 1000
85 : // @Increment: 1
86 : // @User: Standard
87 : AP_GROUPINFO("RETRACT_ALT", 8, AP_LandingGear, _retract_alt_m, 0),
88 :
89 : // @Param: OPTIONS
90 : // @DisplayName: Landing gear auto retract/deploy options
91 : // @Description{Copter}: Options to retract or deploy landing gear in Auto or Guided mode
92 : // @Description{Plane}: Options to retract or deploy landing gear in Auto, Takeoff and Autoland modes
93 : // @Bitmask: 0:Retract after Takeoff,1:Deploy during Land
94 : // @User: Standard
95 : AP_GROUPINFO("OPTIONS", 9, AP_LandingGear, _options, 3),
96 :
97 : // index 10 is enable, placed at the top of the table
98 :
99 : AP_GROUPEND
100 : };
101 :
102 : AP_LandingGear *AP_LandingGear::_singleton;
103 :
104 : /// initialise state of landing gear
105 1693 : void AP_LandingGear::init()
106 : {
107 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
108 1693 : if (AP::sitl()->wow_pin > 0) {
109 0 : _pin_weight_on_wheels.set_and_default(AP::sitl()->wow_pin);
110 0 : _pin_weight_on_wheels_polarity.set_and_default(1);
111 : }
112 : #endif
113 :
114 3375 : if (!_enable.configured() && (SRV_Channels::function_assigned(SRV_Channel::k_landing_gear_control) ||
115 1682 : (_pin_deployed > 0) || (_pin_weight_on_wheels > 0))) {
116 : // if not configured set enable param if output servo or sense pins are defined
117 0 : _enable.set_and_save(1);
118 : }
119 :
120 1693 : if (_pin_deployed != -1) {
121 0 : hal.gpio->pinMode(_pin_deployed, HAL_GPIO_INPUT);
122 : // set pullup/pulldown to default to non-deployed state
123 0 : hal.gpio->write(_pin_deployed, !_pin_deployed_polarity);
124 0 : log_wow_state(wow_state_current);
125 : }
126 :
127 1693 : if (_pin_weight_on_wheels != -1) {
128 0 : hal.gpio->pinMode(_pin_weight_on_wheels, HAL_GPIO_INPUT);
129 : // set pullup/pulldown to default to flying state
130 0 : hal.gpio->write(_pin_weight_on_wheels, !_pin_weight_on_wheels_polarity);
131 0 : log_wow_state(wow_state_current);
132 : }
133 :
134 1693 : switch ((enum LandingGearStartupBehaviour)_startup_behaviour.get()) {
135 1693 : default:
136 : case LandingGear_Startup_WaitForPilotInput:
137 : // do nothing
138 1693 : break;
139 0 : case LandingGear_Startup_Retract:
140 0 : retract();
141 0 : break;
142 0 : case LandingGear_Startup_Deploy:
143 0 : deploy();
144 0 : break;
145 : }
146 1693 : }
147 :
148 : /// set landing gear position to retract, deploy or deploy-and-keep-deployed
149 17 : void AP_LandingGear::set_position(LandingGearCommand cmd)
150 : {
151 17 : switch (cmd) {
152 1 : case LandingGear_Retract:
153 1 : retract();
154 1 : break;
155 16 : case LandingGear_Deploy:
156 16 : deploy();
157 16 : break;
158 : }
159 17 : }
160 :
161 : /// deploy - deploy landing gear
162 710 : void AP_LandingGear::deploy()
163 : {
164 710 : if (!_enable) {
165 705 : return;
166 : }
167 :
168 : // set servo and send message only if output has been configured and not already deployed
169 9 : if (!_deployed &&
170 4 : SRV_Channels::function_assigned(SRV_Channel::k_landing_gear_control)) {
171 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "LandingGear: DEPLOY");
172 : // set deployed flag
173 4 : _deployed = true;
174 4 : _have_changed = true;
175 4 : LOGGER_WRITE_EVENT(LogEvent::LANDING_GEAR_DEPLOYED);
176 : // set servo PWM to deployed position
177 4 : SRV_Channels::set_output_limit(SRV_Channel::k_landing_gear_control, SRV_Channel::Limit::MAX);
178 : }
179 : }
180 :
181 : /// retract - retract landing gear
182 269 : void AP_LandingGear::retract()
183 : {
184 269 : if (!_enable) {
185 267 : return;
186 : }
187 :
188 : // set servo and send message only if output has been configured and already deployed
189 4 : if ((_deployed || !_have_changed ) &&
190 2 : SRV_Channels::function_assigned(SRV_Channel::k_landing_gear_control)) {
191 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "LandingGear: RETRACT");
192 : // reset deployed flag
193 2 : _deployed = false;
194 2 : _have_changed = true;
195 2 : LOGGER_WRITE_EVENT(LogEvent::LANDING_GEAR_RETRACTED);
196 : // set servo PWM to retracted position
197 2 : SRV_Channels::set_output_limit(SRV_Channel::k_landing_gear_control, SRV_Channel::Limit::MIN);
198 : }
199 : }
200 :
201 0 : bool AP_LandingGear::deployed()
202 : {
203 0 : if (_pin_deployed == -1) {
204 0 : return _deployed;
205 : } else {
206 0 : return hal.gpio->read(_pin_deployed) == _pin_deployed_polarity;
207 : }
208 : }
209 :
210 55206906 : AP_LandingGear::LG_WOW_State AP_LandingGear::get_wow_state()
211 : {
212 55206906 : return wow_state_current;
213 : }
214 :
215 110 : AP_LandingGear::LG_LandingGear_State AP_LandingGear::get_state()
216 : {
217 110 : return gear_state_current;
218 : }
219 :
220 0 : uint32_t AP_LandingGear::get_gear_state_duration_ms() const
221 : {
222 0 : if (last_gear_event_ms == 0) {
223 0 : return 0;
224 : }
225 :
226 0 : return AP_HAL::millis() - last_gear_event_ms;
227 : }
228 :
229 0 : uint32_t AP_LandingGear::get_wow_state_duration_ms() const
230 : {
231 0 : if (last_wow_event_ms == 0) {
232 0 : return 0;
233 : }
234 :
235 0 : return AP_HAL::millis() - last_wow_event_ms;
236 : }
237 :
238 273670 : void AP_LandingGear::update(float height_above_ground_m)
239 : {
240 273670 : if (_pin_weight_on_wheels == -1) {
241 273670 : last_wow_event_ms = 0;
242 273670 : wow_state_current = LG_WOW_UNKNOWN;
243 : } else {
244 0 : LG_WOW_State wow_state_new = hal.gpio->read(_pin_weight_on_wheels) == _pin_weight_on_wheels_polarity ? LG_WOW : LG_NO_WOW;
245 :
246 0 : if (wow_state_new != wow_state_current) {
247 : // we changed states, lets note the time.
248 0 : last_wow_event_ms = AP_HAL::millis();
249 0 : log_wow_state(wow_state_new);
250 : }
251 :
252 0 : wow_state_current = wow_state_new;
253 : }
254 :
255 273670 : if (_pin_deployed == -1) {
256 273670 : last_gear_event_ms = 0;
257 :
258 : // If there was no pilot input and state is still unknown - leave it as it is
259 273670 : if (gear_state_current != LG_UNKNOWN) {
260 0 : gear_state_current = (_deployed == true ? LG_DEPLOYED : LG_RETRACTED);
261 : }
262 : } else {
263 : LG_LandingGear_State gear_state_new;
264 :
265 0 : if (_deployed) {
266 0 : gear_state_new = (deployed() == true ? LG_DEPLOYED : LG_DEPLOYING);
267 : } else {
268 0 : gear_state_new = (deployed() == false ? LG_RETRACTED : LG_RETRACTING);
269 : }
270 :
271 0 : if (gear_state_new != gear_state_current) {
272 : // we changed states, lets note the time.
273 0 : last_gear_event_ms = AP_HAL::millis();
274 :
275 0 : log_wow_state(wow_state_current);
276 : }
277 :
278 0 : gear_state_current = gear_state_new;
279 : }
280 :
281 : /*
282 : check for height based triggering
283 : */
284 273670 : float alt_m = MAX(height_above_ground_m, 0.0);
285 :
286 273670 : if (hal.util->get_soft_armed()) {
287 : // only do height based triggering when armed
288 621436 : if (!_deployed &&
289 207122 : _deploy_alt_m > 0 &&
290 414281 : alt_m <= _deploy_alt_m &&
291 2 : _last_height_above_ground_m > _deploy_alt_m) {
292 2 : deploy();
293 207120 : } else if ((_deployed || !_have_changed)&&
294 207153 : _retract_alt_m > 0 &&
295 38 : _retract_alt_m >= _deploy_alt_m &&
296 414311 : alt_m >= _retract_alt_m &&
297 1 : _last_height_above_ground_m < _retract_alt_m) {
298 1 : retract();
299 : }
300 : }
301 :
302 273670 : _last_height_above_ground_m = alt_m;
303 273670 : }
304 :
305 : #if HAL_LOGGING_ENABLED
306 : // log weight on wheels state
307 0 : void AP_LandingGear::log_wow_state(LG_WOW_State state)
308 : {
309 0 : AP::logger().Write("LGR", "TimeUS,LandingGear,WeightOnWheels", "Qbb",
310 : AP_HAL::micros64(),
311 0 : (int8_t)gear_state_current, (int8_t)state);
312 0 : }
313 : #endif
314 :
315 110 : bool AP_LandingGear::check_before_land(void)
316 : {
317 : // If the landing gear state is not known (most probably as it is not used)
318 110 : if (get_state() == LG_UNKNOWN) {
319 110 : return true;
320 : }
321 :
322 : // If the landing gear was not used - return true, otherwise - check for deployed
323 0 : return (get_state() == LG_DEPLOYED);
324 : }
325 :
326 : // retract after takeoff if configured via the OPTIONS parameter
327 267 : void AP_LandingGear::retract_after_takeoff()
328 : {
329 267 : if (_options.get() & (uint16_t)Option::RETRACT_AFTER_TAKEOFF) {
330 267 : retract();
331 : }
332 267 : }
333 :
334 : // deploy for landing if configured via the OPTIONS parameter
335 692 : void AP_LandingGear::deploy_for_landing()
336 : {
337 692 : if (_options.get() & (uint16_t)Option::DEPLOY_DURING_LANDING) {
338 692 : deploy();
339 : }
340 692 : }
341 :
342 : #endif
|
