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

APM_OBC: setup termination values in PX4IO

Browse Source 
this sets up the PX4IO board with failsafe values in case the FMU is
not running
This commit is contained in:
Andrew Tridgell 2014-04-21 10:35:39 +10:00
parent 0d4985079e
commit be9d0c1c4d
2 changed files with 79 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/APM_OBC/APM_OBC.cpp
View File

@ -22,6 +22,8 @@
*/
#include <AP_HAL.h>
#include <APM_OBC.h>
#include <RC_Channel.h>
#include <RC_Channel_aux.h>
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);
}

20
libraries/APM_OBC/APM_OBC.h
View File

@ -27,6 +27,7 @@
#include <AP_Mission.h>
#include <AP_Baro.h>
#include <AP_GPS.h>
#include <AP_RCMapper.h>
#include <inttypes.h>
@ -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);
};