mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-07 09:13:32 +08:00
Convert HIL and FMU drivers to the new multirate PWM interface.
This commit is contained in:
px4dev
+54
-43
@@ -158,6 +158,7 @@ HIL::HIL() :
|
||||
CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/),
|
||||
_mode(MODE_NONE),
|
||||
_update_rate(50),
|
||||
_current_update_rate(0),
|
||||
_task(-1),
|
||||
_t_actuators(-1),
|
||||
_t_armed(-1),
|
||||
@@ -511,9 +512,14 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET_UPDATE_RATE:
|
||||
// HIL always outputs at the alternate (usually faster) rate
|
||||
g_hil->set_pwm_rate(arg);
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SELECT_UPDATE_RATE:
|
||||
// HIL always outputs at the alternate (usually faster) rate
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET(2):
|
||||
case PWM_SERVO_SET(3):
|
||||
if (_mode != MODE_4PWM) {
|
||||
@@ -549,6 +555,14 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
|
||||
// no restrictions on output grouping
|
||||
unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
|
||||
|
||||
*(uint32_t *)arg = (1 << channel);
|
||||
break;
|
||||
}
|
||||
|
||||
case MIXERIOCGETOUTPUTCOUNT:
|
||||
if (_mode == MODE_4PWM) {
|
||||
*(unsigned *)arg = 4;
|
||||
@@ -786,59 +800,51 @@ int
|
||||
hil_main(int argc, char *argv[])
|
||||
{
|
||||
PortMode new_mode = PORT_MODE_UNDEFINED;
|
||||
int pwm_update_rate_in_hz = 0;
|
||||
|
||||
if (!strcmp(argv[1], "test"))
|
||||
test();
|
||||
|
||||
if (!strcmp(argv[1], "fake"))
|
||||
fake(argc - 1, argv + 1);
|
||||
unsigned pwm_rate = 0;
|
||||
const char *verb = argv[1];
|
||||
|
||||
if (hil_start() != OK)
|
||||
errx(1, "failed to start the FMU driver");
|
||||
errx(1, "failed to start the HIL driver");
|
||||
|
||||
/*
|
||||
* Mode switches.
|
||||
*
|
||||
* XXX use getopt?
|
||||
*/
|
||||
for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */
|
||||
|
||||
if (!strcmp(argv[i], "mode_pwm")) {
|
||||
new_mode = PORT1_FULL_PWM;
|
||||
// this was all cut-and-pasted from the FMU driver; it's junk
|
||||
if (!strcmp(verb, "mode_pwm")) {
|
||||
int ch;
|
||||
|
||||
} else if (!strcmp(argv[i], "mode_pwm_serial")) {
|
||||
new_mode = PORT1_PWM_AND_SERIAL;
|
||||
while ((ch = getopt(argc - 1, argv + 1, "u:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'u':
|
||||
pwm_rate = strtol(optarg, nullptr, 0);
|
||||
break;
|
||||
|
||||
} else if (!strcmp(argv[i], "mode_pwm_gpio")) {
|
||||
new_mode = PORT1_PWM_AND_GPIO;
|
||||
|
||||
} else if (!strcmp(argv[i], "mode_port2_pwm8")) {
|
||||
new_mode = PORT2_8PWM;
|
||||
case ':':
|
||||
errx(1, "missing parameter");
|
||||
|
||||
} else if (!strcmp(argv[i], "mode_port2_pwm12")) {
|
||||
new_mode = PORT2_8PWM;
|
||||
|
||||
} else if (!strcmp(argv[i], "mode_port2_pwm16")) {
|
||||
new_mode = PORT2_8PWM;
|
||||
}
|
||||
|
||||
/* look for the optional pwm update rate for the supported modes */
|
||||
if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) {
|
||||
// if (new_mode == PORT1_FULL_PWM || new_mode == PORT1_PWM_AND_GPIO) {
|
||||
// XXX all modes have PWM settings
|
||||
if (argc > i + 1) {
|
||||
pwm_update_rate_in_hz = atoi(argv[i + 1]);
|
||||
printf("pwm update rate: %d Hz\n", pwm_update_rate_in_hz);
|
||||
} else {
|
||||
fprintf(stderr, "missing argument for pwm update rate (-u)\n");
|
||||
return 1;
|
||||
default:
|
||||
errx(1, "unrecognised option");
|
||||
}
|
||||
// } else {
|
||||
// fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n");
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
||||
new_mode = PORT1_FULL_PWM;
|
||||
|
||||
} else if (!strcmp(verb, "mode_pwm_serial")) {
|
||||
new_mode = PORT1_PWM_AND_SERIAL;
|
||||
|
||||
} else if (!strcmp(verb, "mode_pwm_gpio")) {
|
||||
new_mode = PORT1_PWM_AND_GPIO;
|
||||
|
||||
} else if (!strcmp(verb, "mode_port2_pwm8")) {
|
||||
new_mode = PORT2_8PWM;
|
||||
|
||||
} else if (!strcmp(verb, "mode_port2_pwm12")) {
|
||||
new_mode = PORT2_8PWM;
|
||||
|
||||
} else if (!strcmp(verb, "mode_port2_pwm16")) {
|
||||
new_mode = PORT2_8PWM;
|
||||
}
|
||||
|
||||
/* was a new mode set? */
|
||||
if (new_mode != PORT_MODE_UNDEFINED) {
|
||||
@@ -848,10 +854,15 @@ hil_main(int argc, char *argv[])
|
||||
return OK;
|
||||
|
||||
/* switch modes */
|
||||
return hil_new_mode(new_mode, pwm_update_rate_in_hz);
|
||||
return hil_new_mode(new_mode, pwm_rate);
|
||||
}
|
||||
|
||||
/* test, etc. here */
|
||||
if (!strcmp(verb, "test"))
|
||||
test();
|
||||
|
||||
if (!strcmp(verb, "fake"))
|
||||
fake(argc - 1, argv + 1);
|
||||
|
||||
|
||||
fprintf(stderr, "HIL: unrecognized command, try:\n");
|
||||
fprintf(stderr, " mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
|
||||
|
||||
+159
-82
@@ -90,14 +90,18 @@ public:
|
||||
virtual int init();
|
||||
|
||||
int set_mode(Mode mode);
|
||||
int set_pwm_rate(unsigned rate);
|
||||
|
||||
int set_pwm_alt_rate(unsigned rate);
|
||||
int set_pwm_alt_channels(uint32_t channels);
|
||||
|
||||
private:
|
||||
static const unsigned _max_actuators = 4;
|
||||
|
||||
Mode _mode;
|
||||
int _update_rate;
|
||||
int _current_update_rate;
|
||||
unsigned _pwm_default_rate;
|
||||
unsigned _pwm_alt_rate;
|
||||
uint32_t _pwm_alt_rate_channels;
|
||||
unsigned _current_update_rate;
|
||||
int _task;
|
||||
int _t_actuators;
|
||||
int _t_armed;
|
||||
@@ -121,6 +125,7 @@ private:
|
||||
uint8_t control_index,
|
||||
float &input);
|
||||
|
||||
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
|
||||
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
|
||||
|
||||
struct GPIOConfig {
|
||||
@@ -163,7 +168,10 @@ PX4FMU *g_fmu;
|
||||
PX4FMU::PX4FMU() :
|
||||
CDev("fmuservo", "/dev/px4fmu"),
|
||||
_mode(MODE_NONE),
|
||||
_update_rate(50),
|
||||
_pwm_default_rate(50),
|
||||
_pwm_alt_rate(50),
|
||||
_pwm_alt_rate_channels(0),
|
||||
_current_update_rate(0),
|
||||
_task(-1),
|
||||
_t_actuators(-1),
|
||||
_t_armed(-1),
|
||||
@@ -263,27 +271,31 @@ PX4FMU::set_mode(Mode mode)
|
||||
* are presented on the output pins.
|
||||
*/
|
||||
switch (mode) {
|
||||
case MODE_2PWM:
|
||||
debug("MODE_2PWM");
|
||||
/* multi-port with flow control lines as PWM */
|
||||
/* XXX magic numbers */
|
||||
up_pwm_servo_init(0x3);
|
||||
_update_rate = 50; /* default output rate */
|
||||
break;
|
||||
case MODE_2PWM: // multi-port with flow control lines as PWM
|
||||
case MODE_4PWM: // multi-port as 4 PWM outs
|
||||
debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
|
||||
|
||||
/* default output rates */
|
||||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
|
||||
case MODE_4PWM:
|
||||
debug("MODE_4PWM");
|
||||
/* multi-port as 4 PWM outs */
|
||||
/* XXX magic numbers */
|
||||
up_pwm_servo_init(0xf);
|
||||
_update_rate = 50; /* default output rate */
|
||||
up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
|
||||
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
|
||||
|
||||
break;
|
||||
|
||||
case MODE_NONE:
|
||||
debug("MODE_NONE");
|
||||
/* disable servo outputs and set a very low update rate */
|
||||
|
||||
_pwm_default_rate = 10; /* artificially reduced output rate */
|
||||
_pwm_alt_rate = 10;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
|
||||
/* disable servo outputs - no need to set rates */
|
||||
up_pwm_servo_deinit();
|
||||
_update_rate = 10;
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
@@ -295,15 +307,56 @@ PX4FMU::set_mode(Mode mode)
|
||||
}
|
||||
|
||||
int
|
||||
PX4FMU::set_pwm_rate(unsigned rate)
|
||||
PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
|
||||
{
|
||||
if ((rate > 500) || (rate < 10))
|
||||
return -EINVAL;
|
||||
for (unsigned pass = 0; pass < 2; pass++) {
|
||||
for (unsigned group = 0; group < _max_actuators; group++) {
|
||||
|
||||
// get the channel mask for this rate group
|
||||
uint32_t mask = up_pwm_servo_get_rate_group(group);
|
||||
if (mask == 0)
|
||||
continue;
|
||||
|
||||
// all channels in the group must be either default or alt-rate
|
||||
uint32_t alt = rate_map & mask;
|
||||
|
||||
if (pass == 0) {
|
||||
// preflight
|
||||
if ((alt != 0) && (alt != mask)) {
|
||||
// not a legal map, bail
|
||||
return -EINVAL;
|
||||
}
|
||||
} else {
|
||||
// set it - errors here are unexpected
|
||||
if (alt != 0) {
|
||||
if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK)
|
||||
return -EINVAL;
|
||||
} else {
|
||||
if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK)
|
||||
return -EINVAL;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
_pwm_alt_rate_channels = rate_map;
|
||||
_pwm_default_rate = default_rate;
|
||||
_pwm_alt_rate = alt_rate;
|
||||
|
||||
_update_rate = rate;
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
PX4FMU::set_pwm_alt_rate(unsigned rate)
|
||||
{
|
||||
return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate);
|
||||
}
|
||||
|
||||
int
|
||||
PX4FMU::set_pwm_alt_channels(uint32_t channels)
|
||||
{
|
||||
return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
|
||||
}
|
||||
|
||||
void
|
||||
PX4FMU::task_main()
|
||||
{
|
||||
@@ -353,24 +406,29 @@ PX4FMU::task_main()
|
||||
/* loop until killed */
|
||||
while (!_task_should_exit) {
|
||||
|
||||
/* handle update rate changes */
|
||||
if (_current_update_rate != _update_rate) {
|
||||
int update_rate_in_ms = int(1000 / _update_rate);
|
||||
/*
|
||||
* 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;
|
||||
_update_rate = 500;
|
||||
_current_update_rate = 500;
|
||||
}
|
||||
/* reject slower than 50 Hz updates */
|
||||
if (update_rate_in_ms > 20) {
|
||||
update_rate_in_ms = 20;
|
||||
_update_rate = 50;
|
||||
_current_update_rate = 50;
|
||||
}
|
||||
|
||||
orb_set_interval(_t_actuators, update_rate_in_ms);
|
||||
up_pwm_servo_set_rate(_update_rate);
|
||||
_current_update_rate = _update_rate;
|
||||
}
|
||||
|
||||
/* sleep waiting for data, stopping to check for PPM
|
||||
@@ -527,7 +585,6 @@ int
|
||||
PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = OK;
|
||||
int channel;
|
||||
|
||||
lock();
|
||||
|
||||
@@ -541,7 +598,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET_UPDATE_RATE:
|
||||
set_pwm_rate(arg);
|
||||
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SELECT_UPDATE_RATE:
|
||||
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET(2):
|
||||
@@ -555,9 +616,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
case PWM_SERVO_SET(0):
|
||||
case PWM_SERVO_SET(1):
|
||||
if (arg < 2100) {
|
||||
channel = cmd - PWM_SERVO_SET(0);
|
||||
up_pwm_servo_set(channel, arg);
|
||||
|
||||
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
|
||||
} else {
|
||||
ret = -EINVAL;
|
||||
}
|
||||
@@ -573,11 +632,16 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
|
||||
/* FALLTHROUGH */
|
||||
case PWM_SERVO_GET(0):
|
||||
case PWM_SERVO_GET(1): {
|
||||
channel = cmd - PWM_SERVO_GET(0);
|
||||
*(servo_position_t *)arg = up_pwm_servo_get(channel);
|
||||
break;
|
||||
}
|
||||
case PWM_SERVO_GET(1):
|
||||
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_RATEGROUP(0):
|
||||
case PWM_SERVO_GET_RATEGROUP(1):
|
||||
case PWM_SERVO_GET_RATEGROUP(2):
|
||||
case PWM_SERVO_GET_RATEGROUP(3):
|
||||
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
|
||||
break;
|
||||
|
||||
case MIXERIOCGETOUTPUTCOUNT:
|
||||
if (_mode == MODE_4PWM) {
|
||||
@@ -812,7 +876,7 @@ enum PortMode {
|
||||
PortMode g_port_mode;
|
||||
|
||||
int
|
||||
fmu_new_mode(PortMode new_mode, int update_rate)
|
||||
fmu_new_mode(PortMode new_mode)
|
||||
{
|
||||
uint32_t gpio_bits;
|
||||
PX4FMU::Mode servo_mode;
|
||||
@@ -864,9 +928,6 @@ fmu_new_mode(PortMode new_mode, int update_rate)
|
||||
/* (re)set the PWM output mode */
|
||||
g_fmu->set_mode(servo_mode);
|
||||
|
||||
if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
|
||||
g_fmu->set_pwm_rate(update_rate);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
@@ -962,57 +1023,57 @@ int
|
||||
fmu_main(int argc, char *argv[])
|
||||
{
|
||||
PortMode new_mode = PORT_MODE_UNSET;
|
||||
int pwm_update_rate_in_hz = 0;
|
||||
|
||||
if (!strcmp(argv[1], "test"))
|
||||
test();
|
||||
|
||||
if (!strcmp(argv[1], "fake"))
|
||||
fake(argc - 1, argv + 1);
|
||||
unsigned pwm_rate = 0;
|
||||
uint32_t alt_channels = 0;
|
||||
bool alt_channels_set = false;
|
||||
const char *verb = argv[1];
|
||||
|
||||
if (fmu_start() != OK)
|
||||
errx(1, "failed to start the FMU driver");
|
||||
|
||||
/*
|
||||
* Mode switches.
|
||||
*
|
||||
* XXX use getopt?
|
||||
*/
|
||||
for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */
|
||||
if (!strcmp(argv[i], "mode_gpio")) {
|
||||
new_mode = PORT_FULL_GPIO;
|
||||
if (!strcmp(verb, "mode_gpio")) {
|
||||
new_mode = PORT_FULL_GPIO;
|
||||
|
||||
} else if (!strcmp(argv[i], "mode_serial")) {
|
||||
new_mode = PORT_FULL_SERIAL;
|
||||
} else if (!strcmp(verb, "mode_serial")) {
|
||||
new_mode = PORT_FULL_SERIAL;
|
||||
|
||||
} else if (!strcmp(argv[i], "mode_pwm")) {
|
||||
new_mode = PORT_FULL_PWM;
|
||||
} else if (!strcmp(verb, "mode_pwm")) {
|
||||
int ch;
|
||||
char *ap;
|
||||
|
||||
} else if (!strcmp(argv[i], "mode_gpio_serial")) {
|
||||
new_mode = PORT_GPIO_AND_SERIAL;
|
||||
while ((ch = getopt(argc - 1, argv + 1, "c:u:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'u':
|
||||
pwm_rate = strtol(optarg, nullptr, 0);
|
||||
break;
|
||||
|
||||
} else if (!strcmp(argv[i], "mode_pwm_serial")) {
|
||||
new_mode = PORT_PWM_AND_SERIAL;
|
||||
case 'c':
|
||||
alt_channels = strtol(optarg, &ap, 0);
|
||||
if (*ap == '\0')
|
||||
alt_channels_set = true;
|
||||
break;
|
||||
|
||||
} else if (!strcmp(argv[i], "mode_pwm_gpio")) {
|
||||
new_mode = PORT_PWM_AND_GPIO;
|
||||
}
|
||||
case ':':
|
||||
errx(1, "missing parameter");
|
||||
|
||||
/* look for the optional pwm update rate for the supported modes */
|
||||
if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) {
|
||||
if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) {
|
||||
if (argc > i + 1) {
|
||||
pwm_update_rate_in_hz = atoi(argv[i + 1]);
|
||||
|
||||
} else {
|
||||
errx(1, "missing argument for pwm update rate (-u)");
|
||||
return 1;
|
||||
}
|
||||
|
||||
} else {
|
||||
errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio");
|
||||
default:
|
||||
errx(1, "unrecognised option");
|
||||
}
|
||||
}
|
||||
|
||||
new_mode = PORT_FULL_PWM;
|
||||
|
||||
} else if (!strcmp(verb, "mode_gpio_serial")) {
|
||||
new_mode = PORT_GPIO_AND_SERIAL;
|
||||
|
||||
} else if (!strcmp(verb, "mode_pwm_serial")) {
|
||||
new_mode = PORT_PWM_AND_SERIAL;
|
||||
|
||||
} else if (!strcmp(verb, "mode_pwm_gpio")) {
|
||||
new_mode = PORT_PWM_AND_GPIO;
|
||||
}
|
||||
|
||||
/* was a new mode set? */
|
||||
@@ -1023,12 +1084,28 @@ fmu_main(int argc, char *argv[])
|
||||
return OK;
|
||||
|
||||
/* switch modes */
|
||||
return fmu_new_mode(new_mode, pwm_update_rate_in_hz);
|
||||
return fmu_new_mode(new_mode);
|
||||
|
||||
/* if new modes are PWM modes, respect the -u and -c options */
|
||||
if ((new_mode == PORT_FULL_PWM) || (new_mode == PORT_PWM_AND_GPIO)) {
|
||||
if (pwm_rate != 0) {
|
||||
if (g_fmu->set_pwm_alt_rate(pwm_rate) != OK)
|
||||
errx(1, "error setting alternate PWM rate");
|
||||
}
|
||||
if (alt_channels_set) {
|
||||
if (g_fmu->set_pwm_alt_channels(alt_channels))
|
||||
errx(1, "serror setting alternate PWM rate channels");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* test, etc. here */
|
||||
if (!strcmp(verb, "test"))
|
||||
test();
|
||||
|
||||
if (!strcmp(verb, "fake"))
|
||||
fake(argc - 1, argv + 1);
|
||||
|
||||
fprintf(stderr, "FMU: unrecognised command, try:\n");
|
||||
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
|
||||
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_alt_rate_in_hz] [-c pwm_alt_rate_channel_mask], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
@@ -616,19 +616,19 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
|
||||
if (mask == 0)
|
||||
continue;
|
||||
|
||||
/* all channels in the group must be either high or low-rate */
|
||||
uint32_t high = map & mask;
|
||||
/* all channels in the group must be either default or alt-rate */
|
||||
uint32_t alt = map & mask;
|
||||
|
||||
if (pass == 0) {
|
||||
/* preflight */
|
||||
if ((high != 0) && (high != mask)) {
|
||||
if ((alt != 0) && (alt != mask)) {
|
||||
/* not a legal map, bail with an alarm */
|
||||
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
/* set it - errors here are unexpected */
|
||||
if (high != 0) {
|
||||
if (alt != 0) {
|
||||
if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK)
|
||||
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
|
||||
} else {
|
||||
|
||||
Reference in New Issue
Block a user