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

SRV_Channel: Add servo chan override w/ timeout 

Adds a feature to override a servo output for a given time. Scripting
bindings and example included.
This commit is contained in:
Andrew Tridgell 2020-05-25 12:45:17 +10:00
parent 5e74152444
commit 5bfcb0ed4a
2 changed files with 36 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/SRV_Channel/SRV_Channel.h
View File

@ -317,6 +317,9 @@ public:
// set output value for a specific function channel as a pwm value
static void set_output_pwm_chan(uint8_t chan, uint16_t value);
// set output value for a specific function channel as a pwm value for specified override time in ms
static void set_output_pwm_chan_timeout(uint8_t chan, uint16_t value, uint16_t timeout_ms);
// set output value for a function channel as a scaled value. This
// calls calc_pwm() to also set the pwm value
static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value);
@ -531,6 +534,9 @@ private:
SRV_Channel obj_channels[NUM_SERVO_CHANNELS];
// override loop counter
static uint16_t override_counter[NUM_SERVO_CHANNELS];
static struct srv_function {
// mask of what channels this applies to
SRV_Channel::servo_mask_t channel_mask;
@ -548,4 +554,7 @@ private:
}
static bool emergency_stop;
// semaphore for multi-thread use of override_counter array
HAL_Semaphore override_counter_sem;
};

29
libraries/SRV_Channel/SRV_Channels.cpp
View File

@ -41,6 +41,7 @@ SRV_Channels *SRV_Channels::_singleton;
AP_Volz_Protocol *SRV_Channels::volz_ptr;
AP_SBusOut *SRV_Channels::sbus_ptr;
AP_RobotisServo *SRV_Channels::robotis_ptr;
uint16_t SRV_Channels::override_counter[NUM_SERVO_CHANNELS];
#if HAL_SUPPORT_RCOUT_SERIAL
AP_BLHeli *SRV_Channels::blheli_ptr;
@ -153,7 +154,7 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = {
// @Group: _ROB_
// @Path: ../AP_RobotisServo/AP_RobotisServo.cpp
AP_SUBGROUPINFO(robotis, "_ROB_", 22, SRV_Channels, AP_RobotisServo),
AP_GROUPEND
};
@ -208,8 +209,16 @@ void SRV_Channels::setup_failsafe_trim_all_non_motors(void)
*/
void SRV_Channels::calc_pwm(void)
{
WITH_SEMAPHORE(_singleton->override_counter_sem);
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
channels[i].calc_pwm(functions[channels[i].function].output_scaled);
// check if channel has been locked out for this loop
// if it has, decrement the loop count for that channel
if (override_counter[i] == 0) {
channels[i].calc_pwm(functions[channels[i].function].output_scaled);
} else {
override_counter[i]--;
}
}
}
@ -221,6 +230,22 @@ void SRV_Channels::set_output_pwm_chan(uint8_t chan, uint16_t value)
}
}
// set output value for a specific function channel as a pwm value with loop based timeout
// timeout_ms of zero will clear override of the channel
// minimum override is 1 MAIN_LOOP
void SRV_Channels::set_output_pwm_chan_timeout(uint8_t chan, uint16_t value, uint16_t timeout_ms)
{
WITH_SEMAPHORE(_singleton->override_counter_sem);
if (chan < NUM_SERVO_CHANNELS) {
const uint32_t loop_period_us = AP::scheduler().get_loop_period_us();
// round up so any non-zero requested value will result in at least one loop
const uint32_t loop_count = ((timeout_ms * 1000U) + (loop_period_us - 1U)) / loop_period_us;
override_counter[chan] = constrain_int32(loop_count, 0, UINT16_MAX);
SRV_Channels::set_output_pwm_chan(chan, value);
}
}
/*
wrapper around hal.rcout->cork()
*/