simplify oneshot mode selection; use zero PWM rate as indicator

cleanup and remove unused (new) params

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);

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)

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

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);
 }

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();
 		}
 
 	}

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)

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);

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);