zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_AdvancedFailsafe: re-work for use as vehicle derived class

master
Andrew Tridgell 9 years ago
parent
5d6dfd927b
commit
838728348f
2 changed files with 28 additions and 72 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 69
      libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
  2. 31
      libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h

69
libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
Unescape View File

@ -152,7 +152,7 @@ const AP_Param::GroupInfo AP_AdvancedFailsafe::var_info[] = { @@ -152,7 +152,7 @@ const AP_Param::GroupInfo AP_AdvancedFailsafe::var_info[] = {
// check for Failsafe conditions. This is called at 10Hz by the main
// ArduPlane code
void
AP_AdvancedFailsafe::check(AP_AdvancedFailsafe::control_mode mode, uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms)
AP_AdvancedFailsafe::check(uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms)
{
if (!_enable) {
return;
@ -167,10 +167,12 @@ AP_AdvancedFailsafe::check(AP_AdvancedFailsafe::control_mode mode, uint32_t last @@ -167,10 +167,12 @@ AP_AdvancedFailsafe::check(AP_AdvancedFailsafe::control_mode mode, uint32_t last
}
}
enum control_mode mode = afs_mode();
// check if RC termination is enabled
// check for RC failure in manual mode or RC failure when AFS_RC_MANUAL is 0
if (_state != STATE_PREFLIGHT && !_terminate && _enable_RC_fs &&
(mode == AFS_MANUAL || mode == AFS_FBW || !_rc_term_manual_only) &&
(mode == AFS_MANUAL || mode == AFS_STABILIZED || !_rc_term_manual_only) &&
_rc_fail_time_seconds > 0 &&
(AP_HAL::millis() - last_valid_rc_ms) > (_rc_fail_time_seconds * 1000.0f)) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "RC failure terminate");
@ -344,75 +346,26 @@ AP_AdvancedFailsafe::check_altlimit(void) @@ -344,75 +346,26 @@ AP_AdvancedFailsafe::check_altlimit(void)
}
/*
setup the IO boards failsafe values for if the FMU firmware crashes
return true if we should crash the vehicle
*/
void AP_AdvancedFailsafe::setup_failsafe(void)
bool AP_AdvancedFailsafe::should_crash_vehicle(void)
{
if (!_enable) {
return;
}
const RC_Channel *ch_roll = RC_Channel::rc_channel(rcmap.roll()-1);
const RC_Channel *ch_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
const RC_Channel *ch_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
const RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
// setup primary channel output values
hal.rcout->set_failsafe_pwm(1U<<(rcmap.roll()-1), ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.pitch()-1), ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.yaw()-1), ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.throttle()-1), ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
// and all aux channels
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
}
/*
setu radio_out values for all channels to termination values if we
are terminating
*/
void AP_AdvancedFailsafe::check_crash_plane(void)
{
if (!_enable) {
return;
return false;
}
// ensure failsafe values are setup for if FMU crashes on PX4/Pixhawk
if (!_failsafe_setup) {
_failsafe_setup = true;
setup_failsafe();
setup_IO_failsafe();
}
// should we crash the plane? Only possible with
// FS_TERM_ACTTION set to 42
if (!_terminate || _terminate_action != 42) {
// not terminating
return;
return false;
}
// we are terminating. Setup primary output channels radio_out values
RC_Channel *ch_roll = RC_Channel::rc_channel(rcmap.roll()-1);
RC_Channel *ch_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
RC_Channel *ch_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
ch_roll->set_radio_out(ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
ch_pitch->set_radio_out(ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
ch_yaw->set_radio_out(ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
ch_throttle->set_radio_out(ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
// and all aux channels
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
// we are crashing
return true;
}

31
libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h
Unescape View File

@ -34,9 +34,9 @@ class AP_AdvancedFailsafe @@ -34,9 +34,9 @@ class AP_AdvancedFailsafe
{
public:
enum control_mode {
AFS_MANUAL = 0,
AFS_FBW = 1,
AFS_AUTO = 2
AFS_MANUAL = 0,
AFS_STABILIZED = 1,
AFS_AUTO = 2
};
enum state {
@ -64,19 +64,28 @@ public: @@ -64,19 +64,28 @@ public:
}
// check that everything is OK
void check(enum control_mode control_mode, uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms);
void check(uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms);
// generate heartbeat msgs, so external failsafe boards are happy
// during sensor calibration
void heartbeat(void);
// called in servo output code to set servos to crash position if needed
void check_crash_plane(void);
// return true if we are terminating (deliberately crashing the vehicle)
bool should_crash_vehicle(void);
// for holding parameters
static const struct AP_Param::GroupInfo var_info[];
private:
// called to set all outputs to termination state
virtual void terminate_vehicle(void) = 0;
protected:
// setup failsafe values for if FMU firmware stops running
virtual void setup_IO_failsafe(void) = 0;
// return the AFS mapped control mode
virtual enum control_mode afs_mode(void) = 0;
enum state _state;
AP_Mission &mission;
@ -127,11 +136,5 @@ private: @@ -127,11 +136,5 @@ private:
// have the failsafe values been setup?
bool _failsafe_setup:1;
// setup failsafe values for if FMU firmware stops running
void setup_failsafe(void);
bool check_altlimit(void);
};
// map from ArduPlane control_mode to AP_AdvancedFailsafe::control_mode
#define AFS_MODE_PLANE(control_mode) (auto_throttle_mode?AP_AdvancedFailsafe::AFS_AUTO:(control_mode==MANUAL?AP_AdvancedFailsafe::AFS_MANUAL:AP_AdvancedFailsafe::AFS_FBW))

Write Preview
Loading…
Cancel
Save