From da1df5352c054d8cb41fc4611952e14d56eec26a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 8 Oct 2021 09:42:42 +0200 Subject: [PATCH] fix pwm: only update oneshot timers owned by the current pwm_out instance This fixes the case where oneshot was enabled with multi-instance pwm_out, triggering oneshot updates too close to each other and as a result could lead to spinning motors while disarmed. --- platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h | 2 +- platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c | 4 ++-- platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c | 4 ++-- .../nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h | 2 +- platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c | 4 ++-- platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c | 4 ++-- .../nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h | 2 +- platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c | 4 ++-- platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c | 4 ++-- .../src/px4/stm/stm32_common/include/px4_arch/io_timer.h | 2 +- platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c | 4 ++-- platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c | 4 ++-- src/drivers/drv_pwm_output.h | 2 +- src/drivers/pwm_out/PWMOut.cpp | 2 +- src/modules/px4iofirmware/registers.c | 2 +- src/systemcmds/pwm/pwm.cpp | 2 +- 16 files changed, 24 insertions(+), 24 deletions(-) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h index df878380ca..4e5c552b4e 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h @@ -142,7 +142,7 @@ __EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t __EXPORT int io_timer_unallocate_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); -__EXPORT extern void io_timer_trigger(void); +__EXPORT extern void io_timer_trigger(unsigned channel_mask); /** * Reserve a timer diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c index bec852a8af..5b0a316a47 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c @@ -429,9 +429,9 @@ static inline void io_timer_set_PWM_mode(unsigned channel) px4_leave_critical_section(flags); } -void io_timer_trigger(void) +void io_timer_trigger(unsigned channel_mask) { - int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask; struct { uint32_t base; uint16_t triggers; diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c index 541e94c845..07edc7f334 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c @@ -144,9 +144,9 @@ int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate) return io_timer_set_pwm_rate(channel, rate); } -void up_pwm_update(void) +void up_pwm_update(unsigned channel_mask) { - io_timer_trigger(); + io_timer_trigger(channel_mask); } uint32_t up_pwm_servo_get_rate_group(unsigned group) diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h index 79d463f89d..a6e11b9ad8 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h @@ -138,7 +138,7 @@ __EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t __EXPORT int io_timer_unallocate_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); -__EXPORT extern void io_timer_trigger(void); +__EXPORT extern void io_timer_trigger(unsigned channel_mask); /** * Reserve a timer diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c index 4a8470fc9e..2ab103ee84 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c @@ -518,9 +518,9 @@ static inline void io_timer_set_PWM_mode(unsigned timer) px4_leave_critical_section(flags); } -void io_timer_trigger(void) +void io_timer_trigger(unsigned channel_mask) { - int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask; uint32_t action_cache[MAX_IO_TIMERS] = {0}; int actions = 0; diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c index 73926711a7..408fbb37f1 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c @@ -143,9 +143,9 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) return io_timer_set_pwm_rate(group, rate); } -void up_pwm_update(void) +void up_pwm_update(unsigned channel_mask) { - io_timer_trigger(); + io_timer_trigger(channel_mask); } uint32_t up_pwm_servo_get_rate_group(unsigned group) diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h index bc90001689..97d4198738 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h @@ -131,7 +131,7 @@ __EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t __EXPORT int io_timer_unallocate_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); -__EXPORT extern void io_timer_trigger(void); +__EXPORT extern void io_timer_trigger(unsigned channel_mask); /** * Reserve a timer diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c index c4d043bff1..492e296115 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c @@ -514,9 +514,9 @@ static inline void io_timer_set_PWM_mode(unsigned timer) px4_leave_critical_section(flags); } -void io_timer_trigger(void) +void io_timer_trigger(unsigned channel_mask) { - int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask; uint32_t action_cache[MAX_IO_TIMERS] = {0}; int actions = 0; diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c index 5b82fe882e..9af9b4c8e8 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c @@ -142,9 +142,9 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) return io_timer_set_pwm_rate(group, rate); } -void up_pwm_update(void) +void up_pwm_update(unsigned channel_mask) { - io_timer_trigger(); + io_timer_trigger(channel_mask); } uint32_t up_pwm_servo_get_rate_group(unsigned group) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index c476e2c8df..163372c354 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -147,7 +147,7 @@ __EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t __EXPORT int io_timer_unallocate_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); -__EXPORT extern void io_timer_trigger(void); +__EXPORT extern void io_timer_trigger(unsigned channels_mask); __EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable); /** diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 7106319354..5b44d70d6b 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -579,9 +579,9 @@ static inline void io_timer_set_PWM_mode(unsigned timer) rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1; } -void io_timer_trigger(void) +void io_timer_trigger(unsigned channels_mask) { - int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channels_mask; if (oneshots != 0) { uint32_t action_cache[MAX_IO_TIMERS] = {0}; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c index 40397fb4a7..d2b6d202ec 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c @@ -141,9 +141,9 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) return io_timer_set_pwm_rate(group, rate); } -void up_pwm_update(void) +void up_pwm_update(unsigned channels_mask) { - io_timer_trigger(); + io_timer_trigger(channels_mask); } uint32_t up_pwm_servo_get_rate_group(unsigned group) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index dd98e9180f..e8c6685691 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -358,7 +358,7 @@ __EXPORT extern int up_pwm_servo_set_rate_group_update(unsigned group, unsigned * Nothing is done if not in oneshot mode. * */ -__EXPORT extern void up_pwm_update(void); +__EXPORT extern void up_pwm_update(unsigned channel_mask); /** * Set the current output value for a channel. diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index a85f1354a4..0dd2f411ec 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -438,7 +438,7 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], * the oneshots with updated values. */ if (num_control_groups_updated > 0) { - up_pwm_update(); // TODO: review for multi + up_pwm_update(_pwm_mask); } return true; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index bce448e3c0..d8166d1dbb 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -207,7 +207,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* Trigger all timer's channels in Oneshot mode to fire * the oneshots with updated values. */ - up_pwm_update(); + up_pwm_update(0xff); break; diff --git a/src/systemcmds/pwm/pwm.cpp b/src/systemcmds/pwm/pwm.cpp index 9ec21e3948..60a41d2d99 100644 --- a/src/systemcmds/pwm/pwm.cpp +++ b/src/systemcmds/pwm/pwm.cpp @@ -701,7 +701,7 @@ pwm_main(int argc, char *argv[]) * the oneshots with updated values. */ - up_pwm_update(); + up_pwm_update(0xff); #endif } rv = 0;