From be9d0c1c4d73dd1b80983854c22beba3524278d0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Apr 2014 10:35:39 +1000 Subject: [PATCH] APM_OBC: setup termination values in PX4IO this sets up the PX4IO board with failsafe values in case the FMU is not running --- libraries/APM_OBC/APM_OBC.cpp | 66 +++++++++++++++++++++++++++++++++++ libraries/APM_OBC/APM_OBC.h | 20 +++++++---- 2 files changed, 79 insertions(+), 7 deletions(-) diff --git a/libraries/APM_OBC/APM_OBC.cpp b/libraries/APM_OBC/APM_OBC.cpp index d823f6de1e..5e8610b75f 100644 --- a/libraries/APM_OBC/APM_OBC.cpp +++ b/libraries/APM_OBC/APM_OBC.cpp @@ -22,6 +22,8 @@ */ #include #include +#include +#include extern const AP_HAL::HAL& hal; @@ -241,3 +243,67 @@ APM_OBC::check_altlimit(void) // all OK return false; } + +/* + setup the IO boards failsafe values for if the FMU firmware crashes + */ +void APM_OBC::setup_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_MAX)); + 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_MAX); + 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); +} + +/* + setu radio_out values for all channels to termination values if we + are terminating + */ +void APM_OBC::check_crash_plane(void) +{ + // ensure failsafe values are setup for if FMU crashes on PX4/Pixhawk + if (!_failsafe_setup) { + _failsafe_setup = true; + setup_failsafe(); + } + + // should we crash the plane? Only possible with + // FS_TERM_ACTTION set to 42 + if (!_terminate || _terminate_action != 42) { + // not terminating + return; + } + + // 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->radio_out = ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX); + ch_pitch->radio_out = ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX); + ch_yaw->radio_out = ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX); + ch_throttle->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_MAX); + 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); +} diff --git a/libraries/APM_OBC/APM_OBC.h b/libraries/APM_OBC/APM_OBC.h index ddfe8cd8ba..6e77e23136 100644 --- a/libraries/APM_OBC/APM_OBC.h +++ b/libraries/APM_OBC/APM_OBC.h @@ -27,6 +27,7 @@ #include #include #include +#include #include @@ -47,10 +48,11 @@ public: }; // Constructor - APM_OBC(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps) : + APM_OBC(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap) : mission(_mission), baro(_baro), - gps(_gps) + gps(_gps), + rcmap(_rcmap) { AP_Param::setup_object_defaults(this, var_info); @@ -64,11 +66,8 @@ public: void check(enum control_mode control_mode, uint32_t last_heartbeat_ms); - // should we crash the plane? Only possible with - // FS_TERM_ACTTION set to 42 - bool crash_plane(void) { - return _terminate && _terminate_action == 42; - } + // called in servo output code to set servos to crash position if needed + void check_crash_plane(void); // for holding parameters static const struct AP_Param::GroupInfo var_info[]; @@ -79,6 +78,7 @@ private: AP_Mission &mission; AP_Baro &baro; const AP_GPS &gps; + const RCMapper &rcmap; // digital output pins for communicating with the failsafe board AP_Int8 _heartbeat_pin; @@ -105,6 +105,12 @@ private: // saved waypoint for resuming mission uint8_t _saved_wp; + // 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); };