libraries/AP_Parachute/AP_Parachute.cpp

   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 void AP_Parachute::enabled(bool on_off)
  91 {
  92     _enabled.set(on_off);
  93
  94     // clear release_time
  95     _release_time = 0;
  96
  97     LOGGER_WRITE_EVENT(_enabled ? LogEvent::PARACHUTE_ENABLED : LogEvent::PARACHUTE_DISABLED);
  98 }
  99
 100 /// release - release parachute
 101 void AP_Parachute::release()
 102 {
 103     // exit immediately if not enabled
 104     if (_enabled <= 0) {
 105         return;
 106     }
 107
 108     GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Parachute: Released");
 109     LOGGER_WRITE_EVENT(LogEvent::PARACHUTE_RELEASED);
 110
 111     bool need_disarm = (_options.get() & uint32_t(Options::SkipDisarmBeforeParachuteRelease)) == 0;
 112     if (need_disarm) {
 113         // stop motors to avoid parachute tangling
 114         AP::arming().disarm(AP_Arming::Method::PARACHUTE_RELEASE);
 115     }
 116
 117     // set release time to current system time
 118     if (_release_time == 0) {
 119         _release_time = AP_HAL::millis();
 120     }
 121
 122     _release_initiated = true;
 123
 124     // update AP_Notify
 125     AP_Notify::flags.parachute_release = true;
 126 }
 127
 128 /// update - shuts off the trigger should be called at about 10hz
 129 void AP_Parachute::update()
 130 {
 131     // exit immediately if not enabled or parachute not to be released
 132     if (_enabled <= 0) {
 133         return;
 134     }
 135
 136     // calc time since release
 137     uint32_t time_diff = AP_HAL::millis() - _release_time;
 138     uint32_t delay_ms = _delay_ms<=0 ? 0: (uint32_t)_delay_ms;
 139
 140     bool hold_forever = (_options.get() & uint32_t(Options::HoldOpen)) != 0;
 141
 142     // check if we should release parachute
 143     if ((_release_time != 0) && !_release_in_progress) {
 144         if (time_diff >= delay_ms) {
 145             if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
 146                 // move servo
 147                 SRV_Channels::set_output_pwm(SRV_Channel::k_parachute_release, _servo_on_pwm);
 148 #if AP_RELAY_ENABLED
 149             } else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) {
 150                 // set relay
 151                 AP_Relay*_relay = AP::relay();
 152                 if (_relay != nullptr) {
 153                     _relay->set(AP_Relay_Params::FUNCTION::PARACHUTE, true);
 154                 }
 155 #endif
 156             }
 157             _release_in_progress = true;
 158             _released = true;
 159         }
 160     } else if ((_release_time == 0) ||
 161                (!hold_forever && time_diff >= delay_ms + AP_PARACHUTE_RELEASE_DURATION_MS)) {
 162         if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
 163             // move servo back to off position
 164             SRV_Channels::set_output_pwm(SRV_Channel::k_parachute_release, _servo_off_pwm);
 165 #if AP_RELAY_ENABLED
 166         } else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) {
 167             // set relay back to zero volts
 168             AP_Relay*_relay = AP::relay();
 169             if (_relay != nullptr) {
 170                 _relay->set(AP_Relay_Params::FUNCTION::PARACHUTE, false);
 171             }
 172 #endif
 173         }
 174         // reset released flag and release_time
 175         _release_in_progress = false;
 176         _release_time = 0;
 177         // update AP_Notify
 178         AP_Notify::flags.parachute_release = false;
 179     }
 180 }
 181
 182 // set_sink_rate - set vehicle sink rate
 183 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     if ((_critical_sink <= 0) || !_is_flying) {
 187         _sink_time_ms = 0;
 188         return;
 189     }
 190
 191     // reset sink_time if vehicle is not sinking too fast
 192     if (sink_rate <= _critical_sink) {
 193         _sink_time_ms = 0;
 194         return;
 195     }
 196
 197     // start time when sinking too fast
 198     if (_sink_time_ms == 0) {
 199         _sink_time_ms = AP_HAL::millis();
 200     }
 201 }
 202
 203 // trigger parachute release if sink_rate is below critical_sink_rate for 1sec
 204 void AP_Parachute::check_sink_rate()
 205 {
 206     // return immediately if parachute is being released or vehicle is not flying
 207     if (_release_initiated || !_is_flying) {
 208         return;
 209     }
 210
 211     // if vehicle is sinking too fast for more than a second release parachute
 212     if ((_sink_time_ms > 0) && ((AP_HAL::millis() - _sink_time_ms) > 1000)) {
 213         release();
 214     }
 215 }
 216
 217 // check settings are valid
 218 bool AP_Parachute::arming_checks(size_t buflen, char *buffer) const
 219 {
 220     if (_enabled > 0) {
 221         if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
 222             if (!SRV_Channels::function_assigned(SRV_Channel::k_parachute_release)) {
 223                 hal.util->snprintf(buffer, buflen, "Chute has no channel");
 224                 return false;
 225             }
 226         } else {
 227 #if AP_RELAY_ENABLED
 228             AP_Relay*_relay = AP::relay();
 229             if (_relay == nullptr || !_relay->enabled(AP_Relay_Params::FUNCTION::PARACHUTE)) {
 230                 hal.util->snprintf(buffer, buflen, "Chute has no relay");
 231                 return false;
 232             }
 233 #else
 234             hal.util->snprintf(buffer, buflen, "AP_Relay not available");
 235 #endif
 236         }
 237
 238         if (_release_initiated) {
 239             hal.util->snprintf(buffer, buflen, "Chute is released");
 240             return false;
 241         }
 242     }
 243     return true;
 244 }
 245
 246 #if AP_RELAY_ENABLED
 247 // Return the relay index that would be used for param conversion to relay functions
 248 bool AP_Parachute::get_legacy_relay_index(int8_t &index) const
 249 {
 250     // PARAMETER_CONVERSION - Added: Dec-2023
 251     if (_release_type > AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) {
 252         // Not relay type
 253         return false;
 254     }
 255     if (!enabled() && !_enabled.configured()) {
 256         // Disabled and parameter never changed
 257         // Copter manual parachute release enables and deploys in one step
 258         // This means it is possible for parachute to still function correctly if disabled at boot
 259         // Checking if the enable param is configured means the user must have setup chute at some point
 260         // this means relay parm conversion will be done if parachute has ever been enabled
 261         // Parachute has priority in relay param conversion, so this might mean we overwrite some other function
 262         return false;
 263     }
 264     index = _release_type;
 265     return true;
 266 }
 267 #endif
 268
 269 // singleton instance
 270 AP_Parachute *AP_Parachute::_singleton;
 271
 272 namespace AP {
 273
 274 AP_Parachute *parachute()
 275 {
 276     return AP_Parachute::get_singleton();
 277 }
 278
 279 }
 280 #endif // HAL_PARACHUTE_ENABLED