diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index e6e0de9c43..3c0b3ff20c 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/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"); diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 86707b28bf..bb2437dec1 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/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};