From 2d5588ae02560aae029844410212aab38785d671 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sat, 18 Feb 2017 20:35:13 -0700
Subject: [PATCH] simplify oneshot mode selection; use zero PWM rate as
indicator
cleanup and remove unused (new) params
---
src/drivers/px4fmu/fmu.cpp | 60 +++++++--------------------
src/drivers/stm32/drv_io_timer.c | 32 +++++++-------
src/drivers/stm32/drv_io_timer.h | 3 +-
src/drivers/stm32/drv_pwm_servo.c | 50 ++++++----------------
src/modules/px4iofirmware/mixer.cpp | 4 +-
src/modules/px4iofirmware/px4io.c | 1 -
src/modules/px4iofirmware/registers.c | 19 +++++----
src/systemcmds/pwm/pwm.c | 11 ++---
8 files changed, 64 insertions(+), 116 deletions(-)
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 9aae69f5ac..526fb71b14 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -184,6 +184,7 @@ private:
static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS;
Mode _mode;
+ bool _oneshot_mode;
unsigned _pwm_default_rate;
unsigned _pwm_alt_rate;
uint32_t _pwm_alt_rate_channels;
@@ -322,6 +323,7 @@ PX4FMU *g_fmu;
PX4FMU::PX4FMU() :
CDev("fmu", PX4FMU_DEVICE_PATH),
_mode(MODE_NONE),
+ _oneshot_mode(false),
_pwm_default_rate(50),
_pwm_alt_rate(50),
_pwm_alt_rate_channels(0),
@@ -466,7 +468,7 @@ PX4FMU::init()
_task = px4_task_spawn_cmd("fmuservo",
SCHED_DEFAULT,
SCHED_PRIORITY_FAST_DRIVER - 1,
- 1200,
+ 1280,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);
@@ -693,6 +695,11 @@ PX4FMU::set_mode(Mode mode)
return OK;
}
+/* This routine is called from two IOCTLs: PWM_SERVO_SET_UPDATE_RATE and PWM_SERVO_SET_SELECT_UPDATE_RATE
+ * In the first case, it is intended to set the "alternate" PWM rate, which may be [25,400]Hz.
+ * In the second case, it specifies which channels are set at the alternate rate, with all others
+ * implicitly set to the default rate.
+ */
int
PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
{
@@ -714,7 +721,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
if (pass == 0) {
// preflight
if ((alt != 0) && (alt != mask)) {
- warn("rate group %u mask %x bad overlap %x", group, mask, alt);
+ PX4_WARN("rate group %u mask %x bad overlap %x", group, mask, alt);
// not a legal map, bail
return -EINVAL;
}
@@ -722,14 +729,14 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
} else {
// set it - errors here are unexpected
if (alt != 0) {
- if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) {
- warn("rate group set alt failed");
+ if (up_pwm_servo_set_rate_group_update(group, alt_rate) != OK) {
+ PX4_WARN("rate group set alt failed");
return -EINVAL;
}
} else {
- if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
- warn("rate group set default failed");
+ if (up_pwm_servo_set_rate_group_update(group, default_rate) != OK) {
+ PX4_WARN("rate group set default failed");
return -EINVAL;
}
}
@@ -984,7 +991,6 @@ void
PX4FMU::update_pwm_out_state(bool on)
{
if (on && !_pwm_initialized && _pwm_mask != 0) {
- up_pwm_set_oneshot_mode(true);
up_pwm_servo_init(_pwm_mask);
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
_pwm_initialized = true;
@@ -1047,40 +1053,6 @@ PX4FMU::task_main()
_current_update_rate = 0;
}
- /*
- * Adjust actuator topic update rate to keep up with
- * the highest servo update rate configured.
- *
- * We always mix at max rate; some channels may update slower.
- */
- unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
-
- if (_current_update_rate != max_rate) {
- _current_update_rate = max_rate;
- int update_rate_in_ms = int(1000 / _current_update_rate);
-
- /* reject faster than 500 Hz updates */
- if (update_rate_in_ms < 2) {
- update_rate_in_ms = 2;
- }
-
- /* reject slower than 10 Hz updates */
- if (update_rate_in_ms > 100) {
- update_rate_in_ms = 100;
- }
-
- PX4_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms);
-
- for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
- if (_control_subs[i] > 0) {
- orb_set_interval(_control_subs[i], update_rate_in_ms);
- }
- }
-
- // set to current max rate, even if we are actually checking slower/faster
- _current_update_rate = max_rate;
- }
-
/* wait for an update */
int ret = ::poll(_poll_fds, _poll_fds_num, 20);
@@ -1261,10 +1233,8 @@ PX4FMU::task_main()
pwm_output_set(i, pwm_limited[i]);
}
- // force an update if in oneshot mode
- if (up_pwm_get_oneshot_mode()) {
- up_pwm_force_update();
- }
+ // force an update of oneshot channels
+ up_pwm_force_update();
publish_pwm_outputs(pwm_limited, num_outputs);
perf_end(_ctl_latency);
diff --git a/src/drivers/stm32/drv_io_timer.c b/src/drivers/stm32/drv_io_timer.c
index 82eae6776f..0c1d26e9bb 100644
--- a/src/drivers/stm32/drv_io_timer.c
+++ b/src/drivers/stm32/drv_io_timer.c
@@ -378,8 +378,9 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
static int timer_set_rate(unsigned timer, unsigned rate)
{
-
/* configure the timer to update at the desired rate */
+ timer_freq[timer] = 1;
+ rPSC(timer) = (io_timers[timer].clock_freq / 1000000) - 1;
rARR(timer) = timer_freq[timer] * 1000000 / rate;
/* generate an update event; reloads the counter and all registers */
@@ -388,30 +389,29 @@ static int timer_set_rate(unsigned timer, unsigned rate)
return 0;
}
-//#define FAKE_ONESHOT
void io_timer_set_oneshot_mode(unsigned timer)
{
timer_freq[timer] = 8;
+ rPSC(timer) = (io_timers[timer].clock_freq / 8000000) - 1;
-#ifdef FAKE_ONESHOT
- // to eliminate jitter with max additional latency of 500usec, set PWM rate to 2KHz.
- // Note that the FMU mixer jitter is on the order of 2msec.
- timer_set_rate(timer, 2000);
-#else
// set rate to 125Hz (lowest possible at 8MHz is ~122Hz)
// io_timer_set_ccr() must be called at > 125Hz to minimize latency
- timer_set_rate(timer, 125);
-#endif
+ rARR(timer) = timer_freq[timer] * 1000000 / 125;
}
-extern void io_timer_force_update(unsigned timer)
+uint8_t io_timer_get_freq(unsigned timer)
{
-#ifdef FAKE_ONESHOT
- // let's not and say we did
-#else
- // force update of channel compare register
- rEGR(timer) |= GTIM_EGR_UG;
-#endif
+ return timer_freq[timer];
+}
+
+void io_timer_force_update()
+{
+ for (int i = 0; i < MAX_IO_TIMERS; i++) {
+ if (timer_freq[i] == 8) {
+ // force update of channel compare register
+ rEGR(i) |= GTIM_EGR_UG;
+ }
+ }
}
int io_timer_init_timer(unsigned timer)
diff --git a/src/drivers/stm32/drv_io_timer.h b/src/drivers/stm32/drv_io_timer.h
index 9bc61ea5e1..30774784f9 100644
--- a/src/drivers/stm32/drv_io_timer.h
+++ b/src/drivers/stm32/drv_io_timer.h
@@ -134,6 +134,7 @@ __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
*
* @param timer The timer to force.
*/
-__EXPORT extern void io_timer_force_update(unsigned timer);
+__EXPORT extern void io_timer_force_update(void);
+__EXPORT extern uint8_t io_timer_get_freq(unsigned timer);
__END_DECLS
diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c
index 0810ed50d3..2bd86bc9f0 100644
--- a/src/drivers/stm32/drv_pwm_servo.c
+++ b/src/drivers/stm32/drv_pwm_servo.c
@@ -63,22 +63,6 @@
#include <stm32_tim.h>
-/* pwm_oneshot_mode true implies all servo outputs are oneshot */
-static bool pwm_oneshot_mode = false;
-
-/* this is a bitfield: if bit N is set, that timer is used for oneshot only */
-static uint8_t oneshot_timers;
-
-void up_pwm_set_oneshot_mode(bool value)
-{
- pwm_oneshot_mode = value;
-}
-
-bool up_pwm_get_oneshot_mode()
-{
- return pwm_oneshot_mode;
-}
-
int up_pwm_servo_set(unsigned channel, servo_position_t value)
{
return io_timer_set_ccr(channel, value);
@@ -94,10 +78,6 @@ int up_pwm_servo_init(uint32_t channel_mask)
/* Init channels */
io_timer_channel_mode_t chmode = IOTimerChanMode_PWMOut;
- if (pwm_oneshot_mode) {
- chmode = IOTimerChanMode_OneShot;
- }
-
uint32_t current = io_timer_get_mode_channels(chmode);
// First free the current set of PWMs
@@ -109,8 +89,6 @@ int up_pwm_servo_init(uint32_t channel_mask)
}
}
- oneshot_timers = 0;
-
// Now allocate the new set
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
@@ -122,13 +100,13 @@ int up_pwm_servo_init(uint32_t channel_mask)
io_timer_free_channel(channel);
}
+ if (io_timer_get_freq(timer_io_channels[channel].timer_index) == 8) {
+ chmode = IOTimerChanMode_OneShot;
+ }
+
io_timer_channel_init(channel, chmode, NULL, NULL);
channel_mask &= ~(1 << channel);
- if (pwm_oneshot_mode) {
- // update the oneshot_timers bitfield
- oneshot_timers |= (1 << timer_io_channels[channel].timer_index);
- }
}
}
@@ -143,6 +121,12 @@ void up_pwm_servo_deinit(void)
int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
{
+ if (rate == 0) {
+ /* configure this group for OneShot125 PWM */
+ io_timer_set_oneshot_mode(group);
+ return OK;
+ }
+
/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
if (rate < 1) {
return -ERANGE;
@@ -163,11 +147,7 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
void up_pwm_force_update(void)
{
- for (unsigned i = 0; i < 8; i++) {
- if (oneshot_timers & (1 << i)) {
- io_timer_force_update(i);
- }
- }
+ io_timer_force_update();
}
int up_pwm_servo_set_rate(unsigned rate)
@@ -187,10 +167,6 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group)
void
up_pwm_servo_arm(bool armed)
{
- if (pwm_oneshot_mode) {
- io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
-
- } else {
- io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
- }
+ io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
+ io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
}
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 284d194bff..0cdf639d85 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -313,9 +313,7 @@ mixer_tick(void)
if (new_fmu_data) {
new_fmu_data = false;
- if (up_pwm_get_oneshot_mode()) {
- up_pwm_force_update();
- }
+ up_pwm_force_update();
}
}
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index ee424a0bb3..b20a6e1f06 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -235,7 +235,6 @@ int
user_start(int argc, char *argv[])
{
/* configure the first 8 PWM outputs (i.e. all of them) */
- up_pwm_set_oneshot_mode(true);
up_pwm_servo_init(0xff);
#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 5de27777a2..63ecbe1df3 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -325,9 +325,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_RAW_PWM;
- if (up_pwm_get_oneshot_mode()) {
- up_pwm_force_update();
- }
+ up_pwm_force_update();
break;
@@ -632,12 +630,17 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break;
case PX4IO_P_SETUP_PWM_ALTRATE:
- if (value < 25) {
- value = 25;
- }
- if (value > 400) {
- value = 400;
+ /* a rate of zero implies oneshot mode */
+ if (value != 0) {
+ /* constrain normal PWM to [25,400]Hz */
+ if (value < 25) {
+ value = 25;
+ }
+
+ if (value > 400) {
+ value = 400;
+ }
}
pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index e5fd2f6368..4e37c14303 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -327,15 +327,16 @@ pwm_main(int argc, char *argv[])
} else if (!strcmp(argv[1], "rate")) {
/* change alternate PWM rate */
- if (alt_rate > 0) {
+// if (alt_rate > 0) {
ret = px4_ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
- if (ret != OK) {
- PX4_ERR("PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
- return error_on_warn;
- }
+ if (ret != OK) {
+ PX4_ERR("PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
+ return error_on_warn;
}
+// }
+
/* directly supplied channel mask */
if (set_mask > 0) {
ret = px4_ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask);
--
GitLab