mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
06bef6e3b8
This resolves an issue in which the parachute could be suddenly released when the user enabled the parachute. The sequence that could have caused this bad behaviour were (1) the parachute is triggered (2) the user disables the parachute in the 0.5sec between the trigger and the actual release, (3) the user re-enables the parachute and the old release time from (1) is used.
120 lines
3.9 KiB
C++
120 lines
3.9 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include <AP_Parachute.h>
|
|
#include <AP_Relay.h>
|
|
#include <AP_Math.h>
|
|
#include <RC_Channel.h>
|
|
#include <AP_Notify.h>
|
|
#include <AP_HAL.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
const AP_Param::GroupInfo AP_Parachute::var_info[] PROGMEM = {
|
|
|
|
// @Param: ENABLED
|
|
// @DisplayName: Parachute release enabled or disabled
|
|
// @Description: Parachute release enabled or disabled
|
|
// @Values: 0:Disabled,1:Enabled
|
|
// @User: Standard
|
|
AP_GROUPINFO("ENABLED", 0, AP_Parachute, _enabled, 0),
|
|
|
|
// @Param: TYPE
|
|
// @DisplayName: Parachute release mechanism type (relay or servo)
|
|
// @Description: Parachute release mechanism type (relay or servo)
|
|
// @Values: 0:First Relay,1:Second Relay,2:Third Relay,3:Fourth Relay,10:Servo
|
|
// @User: Standard
|
|
AP_GROUPINFO("TYPE", 1, AP_Parachute, _release_type, AP_PARACHUTE_TRIGGER_TYPE_RELAY_0),
|
|
|
|
// @Param: SERVO_ON
|
|
// @DisplayName: Parachute Servo ON PWM value
|
|
// @Description: Parachute Servo PWM value when parachute is released
|
|
// @Range: 1000 2000
|
|
// @Units: pwm
|
|
// @Increment: 1
|
|
// @User: Standard
|
|
AP_GROUPINFO("SERVO_ON", 2, AP_Parachute, _servo_on_pwm, AP_PARACHUTE_SERVO_ON_PWM_DEFAULT),
|
|
|
|
// @Param: SERVO_OFF
|
|
// @DisplayName: Servo OFF PWM value
|
|
// @Description: Parachute Servo PWM value when parachute is not released
|
|
// @Range: 1000 2000
|
|
// @Units: pwm
|
|
// @Increment: 1
|
|
// @User: Standard
|
|
AP_GROUPINFO("SERVO_OFF", 3, AP_Parachute, _servo_off_pwm, AP_PARACHUTE_SERVO_OFF_PWM_DEFAULT),
|
|
|
|
// @Param: ALT_MIN
|
|
// @DisplayName: Parachute min altitude in cm above home
|
|
// @Description: Parachute min altitude above home. Parachute will not be released below this altitude. 0 to disable alt check.
|
|
// @Range: 0 32000
|
|
// @Units: Meters
|
|
// @Increment: 1
|
|
// @User: Standard
|
|
AP_GROUPINFO("ALT_MIN", 4, AP_Parachute, _alt_min, AP_PARACHUTE_ALT_MIN_DEFAULT),
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
/// enabled - enable or disable parachute release
|
|
void AP_Parachute::enabled(bool on_off)
|
|
{
|
|
_enabled = on_off;
|
|
|
|
// clear release_time
|
|
_release_time = 0;
|
|
}
|
|
|
|
/// release - release parachute
|
|
void AP_Parachute::release()
|
|
{
|
|
// exit immediately if not enabled
|
|
if (_enabled <= 0) {
|
|
return;
|
|
}
|
|
|
|
// set release time to current system time
|
|
_release_time = hal.scheduler->millis();
|
|
|
|
// update AP_Notify
|
|
AP_Notify::flags.parachute_release = 1;
|
|
}
|
|
|
|
/// update - shuts off the trigger should be called at about 10hz
|
|
void AP_Parachute::update()
|
|
{
|
|
// exit immediately if not enabled or parachute not to be released
|
|
if (_enabled <= 0 || _release_time == 0) {
|
|
return;
|
|
}
|
|
|
|
// calc time since release
|
|
uint32_t time_diff = hal.scheduler->millis() - _release_time;
|
|
|
|
// check if we should release parachute
|
|
if (!_released) {
|
|
if (time_diff >= AP_PARACHUTE_RELEASE_DELAY_MS) {
|
|
if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
|
|
// move servo
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_parachute_release, _servo_on_pwm);
|
|
}else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) {
|
|
// set relay
|
|
_relay.on(_release_type);
|
|
}
|
|
_released = true;
|
|
}
|
|
}else if (time_diff >= AP_PARACHUTE_RELEASE_DELAY_MS + AP_PARACHUTE_RELEASE_DURATION_MS) {
|
|
if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
|
|
// move servo back to off position
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_parachute_release, _servo_off_pwm);
|
|
}else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) {
|
|
// set relay back to zero volts
|
|
_relay.off(_release_type);
|
|
}
|
|
// reset released flag and release_time
|
|
_released = false;
|
|
_release_time = 0;
|
|
// update AP_Notify
|
|
AP_Notify::flags.parachute_release = 0;
|
|
}
|
|
}
|