APM:Libraries
AP_Parachute.cpp
Go to the documentation of this file.
1 #include "AP_Parachute.h"
2 #include <AP_Relay/AP_Relay.h>
3 #include <AP_Math/AP_Math.h>
6 #include <AP_Notify/AP_Notify.h>
7 #include <AP_HAL/AP_HAL.h>
8 
9 extern const AP_HAL::HAL& hal;
10 
12 
13  // @Param: ENABLED
14  // @DisplayName: Parachute release enabled or disabled
15  // @Description: Parachute release enabled or disabled
16  // @Values: 0:Disabled,1:Enabled
17  // @User: Standard
19 
20  // @Param: TYPE
21  // @DisplayName: Parachute release mechanism type (relay or servo)
22  // @Description: Parachute release mechanism type (relay or servo)
23  // @Values: 0:First Relay,1:Second Relay,2:Third Relay,3:Fourth Relay,10:Servo
24  // @User: Standard
26 
27  // @Param: SERVO_ON
28  // @DisplayName: Parachute Servo ON PWM value
29  // @Description: Parachute Servo PWM value in microseconds when parachute is released
30  // @Range: 1000 2000
31  // @Units: PWM
32  // @Increment: 1
33  // @User: Standard
34  AP_GROUPINFO("SERVO_ON", 2, AP_Parachute, _servo_on_pwm, AP_PARACHUTE_SERVO_ON_PWM_DEFAULT),
35 
36  // @Param: SERVO_OFF
37  // @DisplayName: Servo OFF PWM value
38  // @Description: Parachute Servo PWM value in microseconds when parachute is not released
39  // @Range: 1000 2000
40  // @Units: PWM
41  // @Increment: 1
42  // @User: Standard
43  AP_GROUPINFO("SERVO_OFF", 3, AP_Parachute, _servo_off_pwm, AP_PARACHUTE_SERVO_OFF_PWM_DEFAULT),
44 
45  // @Param: ALT_MIN
46  // @DisplayName: Parachute min altitude in meters above home
47  // @Description: Parachute min altitude above home. Parachute will not be released below this altitude. 0 to disable alt check.
48  // @Range: 0 32000
49  // @Units: m
50  // @Increment: 1
51  // @User: Standard
52  AP_GROUPINFO("ALT_MIN", 4, AP_Parachute, _alt_min, AP_PARACHUTE_ALT_MIN_DEFAULT),
53 
54  // @Param: DELAY_MS
55  // @DisplayName: Parachute release delay
56  // @Description: Delay in millseconds between motor stop and chute release
57  // @Range: 0 5000
58  // @Units: ms
59  // @Increment: 1
60  // @User: Standard
61  AP_GROUPINFO("DELAY_MS", 5, AP_Parachute, _delay_ms, AP_PARACHUTE_RELEASE_DELAY_MS),
62 
64 };
65 
67 void AP_Parachute::enabled(bool on_off)
68 {
69  _enabled = on_off;
70 
71  // clear release_time
72  _release_time = 0;
73 }
74 
77 {
78  // exit immediately if not enabled
79  if (_enabled <= 0) {
80  return;
81  }
82 
83  // set release time to current system time
84  if (_release_time == 0) {
86  }
87 
88  _release_initiated = true;
89 
90  // update AP_Notify
92 }
93 
96 {
97  // exit immediately if not enabled or parachute not to be released
98  if (_enabled <= 0) {
99  return;
100  }
101 
102  // calc time since release
103  uint32_t time_diff = AP_HAL::millis() - _release_time;
104  uint32_t delay_ms = _delay_ms<=0 ? 0: (uint32_t)_delay_ms;
105 
106  // check if we should release parachute
107  if ((_release_time != 0) && !_release_in_progress) {
108  if (time_diff >= delay_ms) {
110  // move servo
113  // set relay
115  }
116  _release_in_progress = true;
117  _released = true;
118  }
119  }else if ((_release_time == 0) || time_diff >= delay_ms + AP_PARACHUTE_RELEASE_DURATION_MS) {
121  // move servo back to off position
124  // set relay back to zero volts
126  }
127  // reset released flag and release_time
128  _release_in_progress = false;
129  _release_time = 0;
130  // update AP_Notify
132  }
133 }
bool _release_initiated
Definition: AP_Parachute.h:83
#define AP_PARAM_FLAG_ENABLE
Definition: AP_Param.h:56
AP_Int16 _servo_off_pwm
Definition: AP_Parachute.h:76
void update()
update - shuts off the trigger should be called at about 10hz
#define AP_PARACHUTE_RELEASE_DURATION_MS
Definition: AP_Parachute.h:16
#define AP_PARACHUTE_TRIGGER_TYPE_SERVO
Definition: AP_Parachute.h:13
#define AP_PARACHUTE_SERVO_OFF_PWM_DEFAULT
Definition: AP_Parachute.h:19
AP_Int8 _release_type
Definition: AP_Parachute.h:74
#define AP_GROUPINFO(name, idx, clazz, element, def)
Definition: AP_Param.h:102
void off(uint8_t relay)
Definition: AP_Relay.cpp:127
AP_Relay & _relay
Definition: AP_Parachute.h:81
AP_Int8 _enabled
Definition: OpticalFlow.h:90
#define AP_PARACHUTE_SERVO_ON_PWM_DEFAULT
Definition: AP_Parachute.h:18
static const struct AP_Param::GroupInfo var_info[]
Definition: AP_Parachute.h:69
#define AP_PARACHUTE_TRIGGER_TYPE_RELAY_0
Definition: AP_Parachute.h:9
bool _release_in_progress
Definition: AP_Parachute.h:84
void on(uint8_t relay)
Definition: AP_Relay.cpp:118
#define AP_GROUPINFO_FLAGS(name, idx, clazz, element, def, flags)
Definition: AP_Param.h:93
RC_Channel manager, with EEPROM-backed storage of constants.
static void set_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t value)
uint32_t millis()
Definition: system.cpp:157
AP_Int16 _servo_on_pwm
Definition: AP_Parachute.h:75
#define AP_PARACHUTE_TRIGGER_TYPE_RELAY_3
Definition: AP_Parachute.h:12
Class managing the release of a parachute.
Definition: AP_Parachute.h:25
bool enabled() const
enabled - returns true if parachute release is enabled
Definition: AP_Parachute.h:48
void release()
release - release parachute
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: AC_PID_test.cpp:14
static struct notify_flags_and_values_type flags
Definition: AP_Notify.h:117
#define AP_PARACHUTE_RELEASE_DELAY_MS
Definition: AP_Parachute.h:15
uint32_t _release_time
Definition: AP_Parachute.h:82
AP_Int8 _enabled
Definition: AP_Parachute.h:73
#define AP_PARACHUTE_ALT_MIN_DEFAULT
Definition: AP_Parachute.h:21
Parachute release library.
#define AP_GROUPEND
Definition: AP_Param.h:121
APM relay control class.
AP_Int16 _delay_ms
Definition: AP_Parachute.h:78