dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
David Sidrane 2021-04-29 14:21:14 -07:00 committed by Lorenz Meier
parent dcbfc9de2d
commit 44cdc52ef8
2 changed files with 49 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
src/drivers/pwm_out/PWMOut.cpp
View File

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

5
src/drivers/pwm_out/PWMOut.hpp
View File

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