Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

SRV_Channel: add global E-stop

Browse Source
This commit is contained in:
IamPete1 2019-01-21 01:36:10 +00:00 committed by Randy Mackay
parent 3da3f75c41
commit cbb871e7bb
3 changed files with 31 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/SRV_Channel/SRV_Channel.cpp
View File

@ -112,6 +112,12 @@ void SRV_Channel::calc_pwm(int16_t output_scaled)
if (have_pwm_mask & (1U<<ch_num)) { if (have_pwm_mask & (1U<<ch_num)) {
return; return;
} }
// check for E - stop
if (SRV_Channel::should_e_stop(get_function()) && SRV_Channels::emergency_stop) {
output_scaled = 0;
}
uint16_t pwm; uint16_t pwm;
if (type_angle) { if (type_angle) {
pwm = pwm_from_angle(output_scaled); pwm = pwm_from_angle(output_scaled);
@ -187,3 +193,13 @@ bool SRV_Channel::is_motor(SRV_Channel::Aux_servo_function_t function)
return ((function >= SRV_Channel::k_motor1 && function <= SRV_Channel::k_motor8) || return ((function >= SRV_Channel::k_motor1 && function <= SRV_Channel::k_motor8) ||
(function >= SRV_Channel::k_motor9 && function <= SRV_Channel::k_motor12)); (function >= SRV_Channel::k_motor9 && function <= SRV_Channel::k_motor12));
} }
// return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
bool SRV_Channel::should_e_stop(SRV_Channel::Aux_servo_function_t function)
{
return ((function >= SRV_Channel::k_heli_rsc && function <= SRV_Channel::k_motor8) ||
function == SRV_Channel::k_starter || function == SRV_Channel::k_throttle ||
function == SRV_Channel::k_throttleLeft || function == SRV_Channel::k_throttleRight ||
(function >= SRV_Channel::k_boost_throttle && function <= SRV_Channel::k_motor12) ||
function == k_engine_run_enable);
}

13
libraries/SRV_Channel/SRV_Channel.h
View File

@ -193,6 +193,9 @@ public:
// return true if function is for a multicopter motor // return true if function is for a multicopter motor
static bool is_motor(SRV_Channel::Aux_servo_function_t function); static bool is_motor(SRV_Channel::Aux_servo_function_t function);
// return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
static bool should_e_stop(SRV_Channel::Aux_servo_function_t function);
// return the function of a channel // return the function of a channel
SRV_Channel::Aux_servo_function_t get_function(void) const { SRV_Channel::Aux_servo_function_t get_function(void) const {
return (SRV_Channel::Aux_servo_function_t)function.get(); return (SRV_Channel::Aux_servo_function_t)function.get();
@ -457,6 +460,14 @@ public:
digital_mask |= mask; digital_mask |= mask;
} }
// Set E - stop
static void set_emergency_stop(bool state) {
emergency_stop = state;
}
// get E - stop
static bool get_emergency_stop() { return emergency_stop;}
private: private:
struct { struct {
bool k_throttle_reversible:1; bool k_throttle_reversible:1;
@ -516,4 +527,6 @@ private:
static bool passthrough_disabled(void) { static bool passthrough_disabled(void) {
return disabled_passthrough; return disabled_passthrough;
} }
static bool emergency_stop;
}; };

1
libraries/SRV_Channel/SRV_Channels.cpp
View File

@ -51,6 +51,7 @@ uint16_t SRV_Channels::reversible_mask;
bool SRV_Channels::disabled_passthrough; bool SRV_Channels::disabled_passthrough;
bool SRV_Channels::initialised; bool SRV_Channels::initialised;
bool SRV_Channels::emergency_stop;
Bitmask SRV_Channels::function_mask{SRV_Channel::k_nr_aux_servo_functions}; Bitmask SRV_Channels::function_mask{SRV_Channel::k_nr_aux_servo_functions};
SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions]; SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions];