fix pwm: only update oneshot timers owned by the current pwm_out instance

bkueng · dagar · commit da1df5352c05 · 2021-10-18T18:45:19.000-04:00
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.
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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;

Original file line number	Diff line number	Diff line change
`@@ -429,9 +429,9 @@ static inline void io_timer_set_PWM_mode(unsigned channel)`
`429`	`429`	`px4_leave_critical_section(flags);`
`430`	`430`	`}`
`431`	`431`
`432`		`-void io_timer_trigger(void)`
	`432`	`+void io_timer_trigger(unsigned channel_mask)`
`433`	`433`	`{`
`434`		`- int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot);`
	`434`	`+ int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask;`
`435`	`435`	`struct {`
`436`	`436`	`uint32_t base;`
`437`	`437`	`uint16_t triggers;`
Original file line number	Diff line number	Diff line change
`@@ -144,9 +144,9 @@ int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate)`
`144`	`144`	`return io_timer_set_pwm_rate(channel, rate);`
`145`	`145`	`}`
`146`	`146`
`147`		`-void up_pwm_update(void)`
	`147`	`+void up_pwm_update(unsigned channel_mask)`
`148`	`148`	`{`
`149`		`- io_timer_trigger();`
	`149`	`+ io_timer_trigger(channel_mask);`
`150`	`150`	`}`
`151`	`151`
`152`	`152`	`uint32_t up_pwm_servo_get_rate_group(unsigned group)`
Original file line number	Diff line number	Diff line change
`@@ -518,9 +518,9 @@ static inline void io_timer_set_PWM_mode(unsigned timer)`
`518`	`518`	`px4_leave_critical_section(flags);`
`519`	`519`	`}`
`520`	`520`
`521`		`-void io_timer_trigger(void)`
	`521`	`+void io_timer_trigger(unsigned channel_mask)`
`522`	`522`	`{`
`523`		`- int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot);`
	`523`	`+ int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask;`
`524`	`524`	`uint32_t action_cache[MAX_IO_TIMERS] = {0};`
`525`	`525`	`int actions = 0;`
`526`	`526`
Original file line number	Diff line number	Diff line change
`@@ -143,9 +143,9 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)`
`143`	`143`	`return io_timer_set_pwm_rate(group, rate);`
`144`	`144`	`}`
`145`	`145`
`146`		`-void up_pwm_update(void)`
	`146`	`+void up_pwm_update(unsigned channel_mask)`
`147`	`147`	`{`
`148`		`- io_timer_trigger();`
	`148`	`+ io_timer_trigger(channel_mask);`
`149`	`149`	`}`
`150`	`150`
`151`	`151`	`uint32_t up_pwm_servo_get_rate_group(unsigned group)`
Original file line number	Diff line number	Diff line change
`@@ -514,9 +514,9 @@ static inline void io_timer_set_PWM_mode(unsigned timer)`
`514`	`514`	`px4_leave_critical_section(flags);`
`515`	`515`	`}`
`516`	`516`
`517`		`-void io_timer_trigger(void)`
	`517`	`+void io_timer_trigger(unsigned channel_mask)`
`518`	`518`	`{`
`519`		`- int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot);`
	`519`	`+ int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask;`
`520`	`520`	`uint32_t action_cache[MAX_IO_TIMERS] = {0};`
`521`	`521`	`int actions = 0;`
`522`	`522`
Original file line number	Diff line number	Diff line change
`@@ -142,9 +142,9 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)`
`142`	`142`	`return io_timer_set_pwm_rate(group, rate);`
`143`	`143`	`}`
`144`	`144`
`145`		`-void up_pwm_update(void)`
	`145`	`+void up_pwm_update(unsigned channel_mask)`
`146`	`146`	`{`
`147`		`- io_timer_trigger();`
	`147`	`+ io_timer_trigger(channel_mask);`
`148`	`148`	`}`
`149`	`149`
`150`	`150`	`uint32_t up_pwm_servo_get_rate_group(unsigned group)`