From d17e9f321a85a7c2e9e5d4aad702e5a2583814eb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 13 Jan 2018 08:13:00 +1100
Subject: [PATCH] HAL_ChibOS: fixed mixture of brushed and normal PWM
allow non-brushed PWM servos with brushed main motors
---
libraries/AP_HAL_ChibiOS/RCOutput.cpp | 15 +++++++++++++--
libraries/AP_HAL_ChibiOS/RCOutput.h | 2 ++
2 files changed, 15 insertions(+), 2 deletions(-)
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 57d69412ae..a9e8eb79ff 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -81,12 +81,20 @@ void ChibiRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
grp_ch_mask |= (1U<<pwm_group_list[i].chan[j]);
}
}
- if ((grp_ch_mask & chmask) == grp_ch_mask) {
+ if ((grp_ch_mask & chmask) != 0) {
+ /*
+ we enable the new frequency on all groups that have one
+ of the requested channels. This means we may enable high
+ speed on some channels that aren't requested, but that
+ is needed in order to fly a vehicle such a a hex
+ multicopter properly
+ */
update_mask |= grp_ch_mask;
pwmChangePeriod(pwm_group_list[i].pwm_drv,
pwm_group_list[i].pwm_cfg.frequency/freq_hz);
}
}
+ fast_channel_mask |= update_mask;
if (chmask != update_mask) {
hal.console->printf("RCOutput: Failed to set PWM frequency req %x set %x\n", (unsigned)chmask, (unsigned)update_mask);
}
@@ -195,7 +203,10 @@ void ChibiRCOutput::push_local(void)
uint8_t chan = pwm_group_list[i].chan[j];
if (outmask & (1UL<<chan)) {
uint32_t period_us = period[chan];
- if(_output_mode == MODE_PWM_BRUSHED) {
+ if(_output_mode == MODE_PWM_BRUSHED && (fast_channel_mask & (1UL<<chan))) {
+ // note that we only use brushed signals on fast
+ // channels. This allows for ordinary PWM on
+ // servos attached to a brushed vehicle
if (period_us <= _esc_pwm_min) {
period_us = 0;
} else if (period_us >= _esc_pwm_max) {
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index 06a1900ab6..ef19dcf4b2 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -79,6 +79,8 @@ private:
uint16_t period[16];
uint8_t num_channels;
bool corked;
+ // mask of channels that are running in high speed
+ uint16_t fast_channel_mask;
// push out values to local PWM
void push_local(void);