Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: re-work AFS for new AP_AdvancedFailsafe API

This commit is contained in:
Andrew Tridgell 2016-07-22 18:14:53 +10:00
parent 838728348f
commit adb5a3ee1f
5 changed files with 113 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.cpp
View File

@ -291,7 +291,7 @@ void Plane::update_logging2(void)
void Plane::afs_fs_check(void)
{
// perform AFS failsafe checks
afs.check(AFS_MODE_PLANE(control_mode), failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
}

13
ArduPlane/Attitude.cpp
View File

@ -854,6 +854,14 @@ uint16_t Plane::throttle_min(void) const
*****************************************/
void Plane::set_servos(void)
{
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
// OBC rules
if (afs.should_crash_vehicle()) {
afs.terminate_vehicle();
return;
}
int16_t last_throttle = channel_throttle->get_radio_out();
// do any transition updates for quadplane
@ -1219,11 +1227,6 @@ void Plane::set_servos(void)
}
}
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
// OBC rules
afs.check_crash_plane();
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// get the servos to the GCS immediately for HIL

23
ArduPlane/Plane.h
View File

@ -126,6 +126,26 @@ protected:
bool ins_checks(bool report);
};
/*
a plane specific AP_AdvancedFailsafe class
*/
class AP_AdvancedFailsafe_Plane : public AP_AdvancedFailsafe
{
public:
AP_AdvancedFailsafe_Plane(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap);
// called to set all outputs to termination state
void terminate_vehicle(void);
protected:
// setup failsafe values for if FMU firmware stops running
void setup_IO_failsafe(void);
// return the AFS mapped control mode
enum control_mode afs_mode(void);
};
/*
main APM:Plane class
*/
@ -137,6 +157,7 @@ public:
friend class AP_Arming_Plane;
friend class QuadPlane;
friend class AP_Tuning_Plane;
friend class AP_AdvancedFailsafe_Plane;
Plane(void);
@ -632,7 +653,7 @@ private:
AP_ADSB adsb {ahrs};
// Outback Challenge Failsafe Support
AP_AdvancedFailsafe afs {mission, barometer, gps, rcmap};
AP_AdvancedFailsafe_Plane afs {mission, barometer, gps, rcmap};
/*
meta data to support counting the number of circles in a loiter

78
ArduPlane/afs_plane.cpp Normal file
View File

@ -0,0 +1,78 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
plane specific AP_AdvancedFailsafe class
*/
#include "Plane.h"
// Constructor
AP_AdvancedFailsafe_Plane::AP_AdvancedFailsafe_Plane(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap) :
AP_AdvancedFailsafe(_mission, _baro, _gps, _rcmap)
{}
/*
setup radio_out values for all channels to termination values
*/
void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
{
// 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);
}
void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void)
{
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);
}
/*
return an AFS_MODE for current control mode
*/
AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void)
{
if (plane.auto_throttle_mode) {
return AP_AdvancedFailsafe::AFS_AUTO;
}
if (plane.control_mode == MANUAL) {
return AP_AdvancedFailsafe::AFS_MANUAL;
}
return AP_AdvancedFailsafe::AFS_STABILIZED;
}

5
ArduPlane/failsafe.cpp
View File

@ -82,7 +82,10 @@ void Plane::failsafe_check(void)
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
// OBC rules
afs.check_crash_plane();
if (afs.should_crash_vehicle()) {
afs.terminate_vehicle();
return;
}
if (!demoing_servos) {
channel_roll->output();