aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Parachute: initial draft library

Browse Source
This commit is contained in:
Randy Mackay 2014-04-01 20:57:03 +09:00
parent 15b99300a2
commit 9d807802cc
2 changed files with 170 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
libraries/AP_Parachute/AP_Parachute.cpp Normal file
View File

@ -0,0 +1,99 @@
// -*- 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_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:Relay 0,1:Relay 1,2:Relay 2,3:Relay 3,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. When vehicle is below this alt parachute cannot be released
// @Range: 0 10000
// @Units: Centimeters
// @Increment: 100
// @User: Standard
AP_GROUPINFO("ALT_MIN", 4, AP_Parachute, _alt_min_cm, AP_PARACHUTE_ALT_MIN_DEFAULT),
AP_GROUPEND
};
/// release - release parachute
void AP_Parachute::release()
{
// exit immediately if not enabled
if (_enabled <= 0) {
return;
}
// trigger the servo or relay
if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_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) {
_relay.on(_release_type);
}
// leave a message that it should be active for this many loops (assumes 50hz loops)
_release_time = hal.scheduler->millis();
}
/// update - shuts off the trigger should be called at about 10hz
void AP_Parachute::update()
{
// exit immediately if not enabled or parachute not released
if (_enabled <= 0 || _release_time == 0) {
return;
}
// check if release mechanism has been triggered more than 1 second ago
if (hal.scheduler->millis() - _release_time > AP_PARACHUTE_RELEASE_DURATION_MS) {
// trigger the servo or relay
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.on(_release_type);
}
// reset release_time
_release_time = 0;
}
}

71
libraries/AP_Parachute/AP_Parachute.h Normal file
View File

@ -0,0 +1,71 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AP_Parachute.h
/// @brief Parachute release library
#ifndef AP_PARACHUTE_H
#define AP_PARACHUTE_H
#include <AP_Param.h>
#include <AP_Common.h>
#include <AP_Relay.h>
#define AP_PARACHUTE_TRIGGER_TYPE_RELAY_0 0
#define AP_PARACHUTE_TRIGGER_TYPE_RELAY_1 1
#define AP_PARACHUTE_TRIGGER_TYPE_RELAY_2 2
#define AP_PARACHUTE_TRIGGER_TYPE_RELAY_3 3
#define AP_PARACHUTE_TRIGGER_TYPE_SERVO 10
#define AP_PARACHUTE_RELEASE_DURATION_MS 1000 // when parachute is released, servo or relay stay at their released position/value for 1000ms (1second)
#define AP_PARACHUTE_SERVO_ON_PWM_DEFAULT 1300 // default PWM value to move servo to when shutter is activated
#define AP_PARACHUTE_SERVO_OFF_PWM_DEFAULT 1100 // default PWM value to move servo to when shutter is deactivated
#define AP_PARACHUTE_ALT_MIN_DEFAULT 1000 // default min altitude the vehicle should have before parachute is released
/// @class AP_Parachute
/// @brief Class managing the release of a parachute
class AP_Parachute {
public:
/// Constructor
AP_Parachute(AP_Relay& relay) :
_relay(relay),
_release_time(0)
{
// setup parameter defaults
AP_Param::setup_object_defaults(this, var_info);
}
/// enabled - enable or disable parachute release
void enabled(bool on_off) { _enabled = on_off; }
/// enabled - returns true if parachute release is enabled
bool enabled() const { return _enabled; }
/// release - release parachute
void release();
/// update - shuts off the trigger should be called at about 10hz
void update();
/// alt_min_cm - returns the min altitude above home the vehicle should have before parachute is released
int16_t alt_min_cm() const { return _alt_min_cm; }
static const struct AP_Param::GroupInfo var_info[];
private:
// Parameters
AP_Int8 _enabled; // 1 if parachute release is enabled
AP_Int8 _release_type; // 0:Servo,1:Relay
AP_Int16 _servo_on_pwm; // PWM value to move servo to when shutter is activated
AP_Int16 _servo_off_pwm; // PWM value to move servo to when shutter is deactivated
AP_Int16 _alt_min_cm; // min altitude the vehicle should have before parachute is released
// internal variables
AP_Relay& _relay; // pointer to relay object from the base class Relay. The subclasses could be AP_Relay_APM1 or AP_Relay_APM2
uint32_t _release_time; // count of number of cycles servo or relay has been at on position
};
#endif /* AP_PARACHUTE_H */