Line data Source code
1 : #include "AP_Parachute.h"
2 :
3 : #if HAL_PARACHUTE_ENABLED
4 :
5 : #include <AP_Relay/AP_Relay.h>
6 : #include <AP_Math/AP_Math.h>
7 : #include <RC_Channel/RC_Channel.h>
8 : #include <SRV_Channel/SRV_Channel.h>
9 : #include <AP_Notify/AP_Notify.h>
10 : #include <AP_HAL/AP_HAL.h>
11 : #include <AP_Logger/AP_Logger.h>
12 : #include <GCS_MAVLink/GCS.h>
13 : #include <AP_Arming/AP_Arming.h>
14 :
15 : extern const AP_HAL::HAL& hal;
16 :
17 : const AP_Param::GroupInfo AP_Parachute::var_info[] = {
18 :
19 : // @Param: ENABLED
20 : // @DisplayName: Parachute release enabled or disabled
21 : // @Description: Parachute release enabled or disabled
22 : // @Values: 0:Disabled,1:Enabled
23 : // @User: Standard
24 : AP_GROUPINFO_FLAGS("ENABLED", 0, AP_Parachute, _enabled, 0, AP_PARAM_FLAG_ENABLE),
25 :
26 : // @Param: TYPE
27 : // @DisplayName: Parachute release mechanism type (relay or servo)
28 : // @Description: Parachute release mechanism type (relay number in versions prior to 4.5, or servo). Values 0-3 all are relay. Relay number used for release is set by RELAYx_FUNCTION in 4.5 or later.
29 : // @Values: 0: Relay,10:Servo
30 :
31 : // @User: Standard
32 : AP_GROUPINFO("TYPE", 1, AP_Parachute, _release_type, AP_PARACHUTE_TRIGGER_TYPE_RELAY_0),
33 :
34 : // @Param: SERVO_ON
35 : // @DisplayName: Parachute Servo ON PWM value
36 : // @Description: Parachute Servo PWM value in microseconds when parachute is released
37 : // @Range: 1000 2000
38 : // @Units: PWM
39 : // @Increment: 1
40 : // @User: Standard
41 : AP_GROUPINFO("SERVO_ON", 2, AP_Parachute, _servo_on_pwm, AP_PARACHUTE_SERVO_ON_PWM_DEFAULT),
42 :
43 : // @Param: SERVO_OFF
44 : // @DisplayName: Servo OFF PWM value
45 : // @Description: Parachute Servo PWM value in microseconds when parachute is not released
46 : // @Range: 1000 2000
47 : // @Units: PWM
48 : // @Increment: 1
49 : // @User: Standard
50 : AP_GROUPINFO("SERVO_OFF", 3, AP_Parachute, _servo_off_pwm, AP_PARACHUTE_SERVO_OFF_PWM_DEFAULT),
51 :
52 : // @Param: ALT_MIN
53 : // @DisplayName: Parachute min altitude in meters above home
54 : // @Description: Parachute min altitude above home. Parachute will not be released below this altitude. 0 to disable alt check.
55 : // @Range: 0 32000
56 : // @Units: m
57 : // @Increment: 1
58 : // @User: Standard
59 : AP_GROUPINFO("ALT_MIN", 4, AP_Parachute, _alt_min, AP_PARACHUTE_ALT_MIN_DEFAULT),
60 :
61 : // @Param: DELAY_MS
62 : // @DisplayName: Parachute release delay
63 : // @Description: Delay in millseconds between motor stop and chute release
64 : // @Range: 0 5000
65 : // @Units: ms
66 : // @Increment: 1
67 : // @User: Standard
68 : AP_GROUPINFO("DELAY_MS", 5, AP_Parachute, _delay_ms, AP_PARACHUTE_RELEASE_DELAY_MS),
69 :
70 : // @Param: CRT_SINK
71 : // @DisplayName: Critical sink speed rate in m/s to trigger emergency parachute
72 : // @Description: Release parachute when critical sink rate is reached
73 : // @Range: 0 15
74 : // @Units: m/s
75 : // @Increment: 1
76 : // @User: Standard
77 : AP_GROUPINFO("CRT_SINK", 6, AP_Parachute, _critical_sink, AP_PARACHUTE_CRITICAL_SINK_DEFAULT),
78 :
79 : // @Param: OPTIONS
80 : // @DisplayName: Parachute options
81 : // @Description: Optional behaviour for parachute
82 : // @Bitmask: 0:hold open forever after release,1:skip disarm before parachute release
83 : // @User: Standard
84 : AP_GROUPINFO("OPTIONS", 7, AP_Parachute, _options, AP_PARACHUTE_OPTIONS_DEFAULT),
85 :
86 : AP_GROUPEND
87 : };
88 :
89 : /// enabled - enable or disable parachute release
90 44 : void AP_Parachute::enabled(bool on_off)
91 : {
92 44 : _enabled.set(on_off);
93 :
94 : // clear release_time
95 44 : _release_time = 0;
96 :
97 44 : LOGGER_WRITE_EVENT(_enabled ? LogEvent::PARACHUTE_ENABLED : LogEvent::PARACHUTE_DISABLED);
98 44 : }
99 :
100 : /// release - release parachute
101 15 : void AP_Parachute::release()
102 : {
103 : // exit immediately if not enabled
104 15 : if (_enabled <= 0) {
105 0 : return;
106 : }
107 :
108 15 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Parachute: Released");
109 15 : LOGGER_WRITE_EVENT(LogEvent::PARACHUTE_RELEASED);
110 :
111 15 : bool need_disarm = (_options.get() & uint32_t(Options::SkipDisarmBeforeParachuteRelease)) == 0;
112 15 : if (need_disarm) {
113 : // stop motors to avoid parachute tangling
114 15 : AP::arming().disarm(AP_Arming::Method::PARACHUTE_RELEASE);
115 : }
116 :
117 : // set release time to current system time
118 15 : if (_release_time == 0) {
119 15 : _release_time = AP_HAL::millis();
120 : }
121 :
122 15 : _release_initiated = true;
123 :
124 : // update AP_Notify
125 15 : AP_Notify::flags.parachute_release = true;
126 : }
127 :
128 : /// update - shuts off the trigger should be called at about 10hz
129 643739 : void AP_Parachute::update()
130 : {
131 : // exit immediately if not enabled or parachute not to be released
132 643739 : if (_enabled <= 0) {
133 530907 : return;
134 : }
135 :
136 : // calc time since release
137 112832 : uint32_t time_diff = AP_HAL::millis() - _release_time;
138 112832 : uint32_t delay_ms = _delay_ms<=0 ? 0: (uint32_t)_delay_ms;
139 :
140 112832 : bool hold_forever = (_options.get() & uint32_t(Options::HoldOpen)) != 0;
141 :
142 : // check if we should release parachute
143 112832 : if ((_release_time != 0) && !_release_in_progress) {
144 2032 : if (time_diff >= delay_ms) {
145 15 : if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
146 : // move servo
147 15 : SRV_Channels::set_output_pwm(SRV_Channel::k_parachute_release, _servo_on_pwm);
148 : #if AP_RELAY_ENABLED
149 0 : } else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) {
150 : // set relay
151 0 : AP_Relay*_relay = AP::relay();
152 0 : if (_relay != nullptr) {
153 0 : _relay->set(AP_Relay_Params::FUNCTION::PARACHUTE, true);
154 : }
155 : #endif
156 : }
157 15 : _release_in_progress = true;
158 15 : _released = true;
159 : }
160 112832 : } else if ((_release_time == 0) ||
161 5335 : (!hold_forever && time_diff >= delay_ms + AP_PARACHUTE_RELEASE_DURATION_MS)) {
162 105469 : if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
163 : // move servo back to off position
164 105468 : SRV_Channels::set_output_pwm(SRV_Channel::k_parachute_release, _servo_off_pwm);
165 : #if AP_RELAY_ENABLED
166 1 : } else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) {
167 : // set relay back to zero volts
168 1 : AP_Relay*_relay = AP::relay();
169 1 : if (_relay != nullptr) {
170 1 : _relay->set(AP_Relay_Params::FUNCTION::PARACHUTE, false);
171 : }
172 : #endif
173 : }
174 : // reset released flag and release_time
175 105469 : _release_in_progress = false;
176 105469 : _release_time = 0;
177 : // update AP_Notify
178 105469 : AP_Notify::flags.parachute_release = false;
179 : }
180 : }
181 :
182 : // set_sink_rate - set vehicle sink rate
183 643747 : void AP_Parachute::set_sink_rate(float sink_rate)
184 : {
185 : // reset sink time if critical sink rate check is disabled or vehicle is not flying
186 643747 : if ((_critical_sink <= 0) || !_is_flying) {
187 643385 : _sink_time_ms = 0;
188 643385 : return;
189 : }
190 :
191 : // reset sink_time if vehicle is not sinking too fast
192 362 : if (sink_rate <= _critical_sink) {
193 349 : _sink_time_ms = 0;
194 349 : return;
195 : }
196 :
197 : // start time when sinking too fast
198 13 : if (_sink_time_ms == 0) {
199 1 : _sink_time_ms = AP_HAL::millis();
200 : }
201 : }
202 :
203 : // trigger parachute release if sink_rate is below critical_sink_rate for 1sec
204 558244 : void AP_Parachute::check_sink_rate()
205 : {
206 : // return immediately if parachute is being released or vehicle is not flying
207 558244 : if (_release_initiated || !_is_flying) {
208 154542 : return;
209 : }
210 :
211 : // if vehicle is sinking too fast for more than a second release parachute
212 403702 : if ((_sink_time_ms > 0) && ((AP_HAL::millis() - _sink_time_ms) > 1000)) {
213 1 : release();
214 : }
215 : }
216 :
217 : // check settings are valid
218 49591 : bool AP_Parachute::arming_checks(size_t buflen, char *buffer) const
219 : {
220 49591 : if (_enabled > 0) {
221 430 : if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
222 430 : if (!SRV_Channels::function_assigned(SRV_Channel::k_parachute_release)) {
223 0 : hal.util->snprintf(buffer, buflen, "Chute has no channel");
224 0 : return false;
225 : }
226 : } else {
227 : #if AP_RELAY_ENABLED
228 0 : AP_Relay*_relay = AP::relay();
229 0 : if (_relay == nullptr || !_relay->enabled(AP_Relay_Params::FUNCTION::PARACHUTE)) {
230 0 : hal.util->snprintf(buffer, buflen, "Chute has no relay");
231 0 : return false;
232 : }
233 : #else
234 : hal.util->snprintf(buffer, buflen, "AP_Relay not available");
235 : return false;
236 : #endif
237 : }
238 :
239 430 : if (_release_initiated) {
240 38 : hal.util->snprintf(buffer, buflen, "Chute is released");
241 38 : return false;
242 : }
243 : }
244 49553 : return true;
245 : }
246 :
247 : #if AP_RELAY_ENABLED
248 : // Return the relay index that would be used for param conversion to relay functions
249 1693 : bool AP_Parachute::get_legacy_relay_index(int8_t &index) const
250 : {
251 : // PARAMETER_CONVERSION - Added: Dec-2023
252 1693 : if (_release_type > AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) {
253 : // Not relay type
254 24 : return false;
255 : }
256 1669 : if (!enabled() && !_enabled.configured()) {
257 : // Disabled and parameter never changed
258 : // Copter manual parachute release enables and deploys in one step
259 : // This means it is possible for parachute to still function correctly if disabled at boot
260 : // Checking if the enable param is configured means the user must have setup chute at some point
261 : // this means relay parm conversion will be done if parachute has ever been enabled
262 : // Parachute has priority in relay param conversion, so this might mean we overwrite some other function
263 1630 : return false;
264 : }
265 39 : index = _release_type;
266 39 : return true;
267 : }
268 : #endif
269 :
270 : // singleton instance
271 : AP_Parachute *AP_Parachute::_singleton;
272 :
273 : namespace AP {
274 :
275 80091 : AP_Parachute *parachute()
276 : {
277 80091 : return AP_Parachute::get_singleton();
278 : }
279 :
280 : }
281 : #endif // HAL_PARACHUTE_ENABLED
|
