PWMOut:Use rates from single instance & init all instances channels

src/drivers/pwm_out/PWMOut.cpp

@@ -52,11 +52,18 @@ PWMOut::PWMOut(int instance, uint8_t output_base) :
 	OutputModuleInterface((instance == 0) ? MODULE_NAME"0" : MODULE_NAME"1", px4::wq_configurations::hp_default),
 	_instance(instance),
 	_output_base(output_base),
+	_output_mask(0),
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
 {
 	_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
 	_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
+
+	for (int i = 0; i < MAX_PER_INSTANCE; i++) {
+		_output_mask |= 1 << i;
+	}
+
+	_output_mask <<= _output_base;
 }
 
 PWMOut::~PWMOut()
@@ -382,19 +389,20 @@ int PWMOut::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_
 		 * common settings and can not be independent in terms of count frequency
 		 * (granularity of pulse width) and rate (period of repetition).
 		 *
-		 * To say it another way, all channels in a group moust have the same
+		 * To say it another way, all channels in a group must have the same
 		 * rate and mode. (See rates above.)
 		 */
 
 		for (unsigned group = 0; group < FMU_MAX_ACTUATORS; group++) {
 
 			// get the channel mask for this rate group
-			uint32_t mask = up_pwm_servo_get_rate_group(group);
+			uint32_t mask = _output_mask & up_pwm_servo_get_rate_group(group);
 
 			if (mask == 0) {
 				continue;
 			}
 
+			printf("group:%d  mask %x\n", group, mask);
 			// all channels in the group must be either default or alt-rate
 			uint32_t alt = rate_map & mask;
 
@@ -487,7 +495,7 @@ int PWMOut::task_spawn(int argc, char *argv[])
 	for (unsigned instance = 0; instance < (sizeof(_objects) / sizeof(_objects[0])); instance++) {
 
 		if (instance < PWM_OUT_MAX_INSTANCES) {
-			uint8_t base = instance * 8;  // TODO: configurable
+			uint8_t base = instance * MAX_PER_INSTANCE;  // TODO: configurable
 			PWMOut *dev = new PWMOut(instance, base);
 
 			if (dev) {
@@ -537,23 +545,12 @@ void PWMOut::update_pwm_out_state(bool on)
 		// Collect the PWM alt rate channels across all instances
 		uint32_t pwm_alt_rate_channels_new = 0;
 
-		// Collect the minimum default rate
-		unsigned default_rate_min = 0;
-		// Collect the maximum alternative rate (400 Hz or DSHOT outputs)
-		unsigned alt_rate_max = 0;
-
 		for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
 			if (_objects[i].load()) {
+
 				pwm_mask_new |= _objects[i].load()->get_pwm_mask();
 				pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels();
 
-				if (_objects[i].load()->get_alt_rate() > alt_rate_max) {
-					alt_rate_max = _objects[i].load()->get_alt_rate();
-				}
-
-				if (_objects[i].load()->get_default_rate() < default_rate_min) {
-					default_rate_min = _objects[i].load()->get_default_rate();
-				}
 			}
 		}
 
@@ -563,6 +560,7 @@ void PWMOut::update_pwm_out_state(bool on)
 
 		// Set rate is not affecting non-masked channels, so can be called
 		// individually
+		set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate());
 		_pwm_initialized = true;
 	}
 
@@ -1171,7 +1169,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(1):
 	case PWM_SERVO_SET(0):
 		if (arg <= 2100) {
-			up_pwm_servo_set(cmd - PWM_SERVO_SET(0 + _output_base), arg);
+			up_pwm_servo_set(cmd - PWM_SERVO_SET(0) + _output_base, arg);
 
 		} else {
 			ret = -EINVAL;
@@ -1243,7 +1241,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	/* FALLTHROUGH */
 	case PWM_SERVO_GET(1):
 	case PWM_SERVO_GET(0):
-		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0 + _output_base));
+		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0) +  _output_base);
 		break;
 
 	case PWM_SERVO_GET_RATEGROUP(0):
@@ -1268,7 +1266,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_RATEGROUP(12):
 	case PWM_SERVO_GET_RATEGROUP(13):
 #endif
-		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0 + _output_base));
+		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0) + _output_base);
 		break;
 
 	case PWM_SERVO_GET_COUNT:
@@ -1813,7 +1811,7 @@ int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz)
 
 } // namespace
 
-int PWMOut::test()
+int PWMOut::test(const char *dev)
 {
 	int	 fd;
 	unsigned servo_count = 0;
@@ -1828,7 +1826,7 @@ int PWMOut::test()
 		input_capture_config_t  chan;
 	} capture_conf[INPUT_CAPTURE_MAX_CHANNELS];
 
-	fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
+	fd = ::open(dev, O_RDWR);
 
 	if (fd < 0) {
 		PX4_ERR("open fail");
@@ -2006,8 +2004,33 @@ err_out_no_test:
 
 int PWMOut::custom_command(int argc, char *argv[])
 {
+
+	int ch = 0;
+	int myoptind = 0;
+	const char *myoptarg = nullptr;
+	const char *dev = PX4FMU_DEVICE_PATH;
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			if (nullptr == strstr(myoptarg, "/dev/")) {
+				PX4_WARN("device %s not valid", myoptarg);
+				print_usage(nullptr);
+				return 1;
+			}
+
+			dev = myoptarg;
+			break;
+		}
+	}
+
+	if (myoptind >= argc) {
+		print_usage(nullptr);
+		return 1;
+	}
+
 	PortMode new_mode = PORT_MODE_UNSET;
-	const char *verb = argv[0];
+	const char *verb = argv[myoptind];
 
 	/* does not operate on a FMU instance */
 	if (!strcmp(verb, "i2c")) {
@@ -2151,7 +2174,7 @@ int PWMOut::custom_command(int argc, char *argv[])
 	}
 
 	if (!strcmp(verb, "test")) {
-		return test();
+		return test(dev);
 	}
 
 	return print_usage("unknown command");

src/drivers/pwm_out/PWMOut.hpp

@@ -151,7 +151,7 @@ public:
 	/** change the FMU mode of the running module */
 	static int fmu_new_mode(PortMode new_mode);
 
-	static int test();
+	static int test(const char *dev);
 
 	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
 
@@ -182,6 +182,9 @@ private:
 
 	const int _instance;
 	const uint32_t _output_base;
+	uint32_t _output_mask;
+
+	static const int MAX_PER_INSTANCE{8};
 
 	MixingOutput _mixing_output{FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};