Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

SRV_Channel: fix override timeout without repeated calls

Browse Source
This commit is contained in:
Iampete1 2021-09-14 20:25:18 +01:00 committed by Andrew Tridgell
parent 84a89b0b94
commit f541653502
2 changed files with 20 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/SRV_Channel/SRV_Channel.cpp
View File

@ -113,6 +113,8 @@ uint16_t SRV_Channel::pwm_from_angle(int16_t scaled_value) const
void SRV_Channel::calc_pwm(int16_t output_scaled) void SRV_Channel::calc_pwm(int16_t output_scaled)
{ {
if (have_pwm_mask & (1U<<ch_num)) { if (have_pwm_mask & (1U<<ch_num)) {
// Note that this allows a set_output_pwm call to override E-Stop!!
// tricky to fix because we would endup E-stoping to individual SEROVx_MIN not MOT_PWM_MIN on copter
return; return;
} }
@ -123,13 +125,16 @@ void SRV_Channel::calc_pwm(int16_t output_scaled)
force = true; force = true;
} }
uint16_t pwm; if (!force && override_active) {
if (type_angle) { // don't overwrite a override
pwm = pwm_from_angle(output_scaled); return;
} else { }
pwm = pwm_from_range(output_scaled);
if (type_angle) {
output_pwm = pwm_from_angle(output_scaled);
} else {
output_pwm = pwm_from_range(output_scaled);
} }
set_output_pwm(pwm,force);
} }
void SRV_Channel::set_output_pwm(uint16_t pwm, bool force) void SRV_Channel::set_output_pwm(uint16_t pwm, bool force)

9
libraries/SRV_Channel/SRV_Channels.cpp
View File

@ -328,7 +328,16 @@ void SRV_Channels::set_output_pwm_chan_timeout(uint8_t chan, uint16_t value, uin
const uint32_t loop_count = ((timeout_ms * 1000U) + (loop_period_us - 1U)) / loop_period_us; 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); override_counter[chan] = constrain_int32(loop_count, 0, UINT16_MAX);
channels[chan].set_override(true); channels[chan].set_override(true);
const bool had_pwm = SRV_Channel::have_pwm_mask & (1U<<chan);
channels[chan].set_output_pwm(value,true); channels[chan].set_output_pwm(value,true);
if (!had_pwm) {
// clear the have PWM mask so the channel will default back to the scaled value when timeout expires
// this is also cleared by set_output_scaled but that requires it to be re-called as some point
// after the timeout is applied
// note that we can't default back to a pre-override PWM value as it is not stored
// checking had_pwm means the PWM will not change after the timeout, this was the existing behaviour
SRV_Channel::have_pwm_mask &= ~(1U<<chan);
}
} }
} }