aboutsummaryrefslogtreecommitdiff
path: root/apps/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'apps/drivers')
-rw-r--r--apps/drivers/blinkm/blinkm.cpp8
-rw-r--r--apps/drivers/drv_pwm_output.h47
-rw-r--r--apps/drivers/hil/hil.cpp86
-rw-r--r--apps/drivers/px4fmu/fmu.cpp219
-rw-r--r--apps/drivers/px4io/px4io.cpp178
-rw-r--r--apps/drivers/stm32/drv_pwm_servo.c49
6 files changed, 366 insertions, 221 deletions
diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp
index 54c7d4266..56265995f 100644
--- a/apps/drivers/blinkm/blinkm.cpp
+++ b/apps/drivers/blinkm/blinkm.cpp
@@ -135,7 +135,7 @@ public:
virtual int setMode(int mode);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
- static const char *script_names[];
+ static const char *const script_names[];
private:
enum ScriptID {
@@ -219,7 +219,7 @@ namespace
}
/* list of script names, must match script ID numbers */
-const char *BlinkM::script_names[] = {
+const char *const BlinkM::script_names[] = {
"USER",
"RGB",
"WHITE_FLASH",
@@ -499,7 +499,7 @@ BlinkM::led()
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
}
- printf("<blinkm> cells found:%u\n", num_of_cells);
+ printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
@@ -827,7 +827,7 @@ BlinkM::get_firmware_version(uint8_t version[2])
{
const uint8_t msg = 'Z';
- return transfer(&msg, sizeof(msg), version, sizeof(version));
+ return transfer(&msg, sizeof(msg), version, 2);
}
void blinkm_usage() {
diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index f3f753ebe..07831f20c 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/apps/drivers/drv_pwm_output.h
@@ -36,7 +36,7 @@
*
* Servo values can be set with the PWM_SERVO_SET ioctl, by writing a
* pwm_output_values structure to the device, or by publishing to the
- * output_pwm ObjDev.
+ * output_pwm ORB topic.
* Writing a value of 0 to a channel suppresses any output for that
* channel.
*/
@@ -60,7 +60,7 @@ __BEGIN_DECLS
#define PWM_OUTPUT_DEVICE_PATH "/dev/pwm_output"
/**
- * Maximum number of PWM output channels in the system.
+ * Maximum number of PWM output channels supported by the device.
*/
#define PWM_OUTPUT_MAX_CHANNELS 16
@@ -82,14 +82,14 @@ struct pwm_output_values {
};
/*
- * ObjDev tag for PWM outputs.
+ * ORB tag for PWM outputs.
*/
ORB_DECLARE(output_pwm);
/*
* ioctl() definitions
*
- * Note that ioctls and ObjDev updates should not be mixed, as the
+ * Note that ioctls and ORB updates should not be mixed, as the
* behaviour of the system in this case is not defined.
*/
#define _PWM_SERVO_BASE 0x2a00
@@ -100,20 +100,14 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
-/** set update rate in Hz */
-#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+/** set alternate servo update rate */
+#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
/** get the number of servos in *(unsigned *)arg */
#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3)
-/** set debug level for servo IO */
-#define PWM_SERVO_SET_DEBUG _IOC(_PWM_SERVO_BASE, 4)
-
-/** enable in-air restart */
-#define PWM_SERVO_INAIR_RESTART_ENABLE _IOC(_PWM_SERVO_BASE, 5)
-
-/** disable in-air restart */
-#define PWM_SERVO_INAIR_RESTART_DISABLE _IOC(_PWM_SERVO_BASE, 6)
+/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
+#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
@@ -121,6 +115,10 @@ ORB_DECLARE(output_pwm);
/** get a single specific servo value */
#define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo)
+/** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels
+ * whose update rates must be the same.
+ */
+#define PWM_SERVO_GET_RATEGROUP(_n) _IOC(_PWM_SERVO_BASE, 0x60 + _n)
/*
* Low-level PWM output interface.
@@ -157,7 +155,7 @@ __EXPORT extern void up_pwm_servo_deinit(void);
__EXPORT extern void up_pwm_servo_arm(bool armed);
/**
- * Set the servo update rate
+ * Set the servo update rate for all rate groups.
*
* @param rate The update rate in Hz to set.
* @return OK on success, -ERANGE if an unsupported update rate is set.
@@ -165,6 +163,25 @@ __EXPORT extern void up_pwm_servo_arm(bool armed);
__EXPORT extern int up_pwm_servo_set_rate(unsigned rate);
/**
+ * Get a bitmap of output channels assigned to a given rate group.
+ *
+ * @param group The rate group to query. Rate groups are assigned contiguously
+ * starting from zero.
+ * @return A bitmap of channels assigned to the rate group, or zero if
+ * the group number has no channels.
+ */
+__EXPORT extern uint32_t up_pwm_servo_get_rate_group(unsigned group);
+
+/**
+ * Set the update rate for a given rate group.
+ *
+ * @param group The rate group whose update rate will be changed.
+ * @param rate The update rate in Hz.
+ * @return OK if the group was adjusted, -ERANGE if an unsupported update rate is set.
+ */
+__EXPORT extern int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate);
+
+/**
* Set the current output value for a channel.
*
* @param channel The channel to set.
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index 861ed7924..d9aa772d4 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -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;
@@ -641,7 +655,7 @@ enum PortMode {
PortMode g_port_mode;
int
-hil_new_mode(PortMode new_mode, int update_rate)
+hil_new_mode(PortMode new_mode)
{
// uint32_t gpio_bits;
@@ -699,8 +713,6 @@ hil_new_mode(PortMode new_mode, int update_rate)
/* (re)set the PWM output mode */
g_hil->set_mode(servo_mode);
- if ((servo_mode != HIL::MODE_NONE) && (update_rate != 0))
- g_hil->set_pwm_rate(update_rate);
return OK;
}
@@ -786,59 +798,34 @@ 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);
+ 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")) {
+ new_mode = PORT1_FULL_PWM;
- } else if (!strcmp(argv[i], "mode_pwm_serial")) {
- new_mode = PORT1_PWM_AND_SERIAL;
+ } else if (!strcmp(verb, "mode_pwm_serial")) {
+ new_mode = PORT1_PWM_AND_SERIAL;
- } 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;
+ } else if (!strcmp(verb, "mode_pwm_gpio")) {
+ new_mode = PORT1_PWM_AND_GPIO;
- } else if (!strcmp(argv[i], "mode_port2_pwm12")) {
- new_mode = PORT2_8PWM;
+ } else if (!strcmp(verb, "mode_port2_pwm8")) {
+ new_mode = PORT2_8PWM;
- } else if (!strcmp(argv[i], "mode_port2_pwm16")) {
- new_mode = PORT2_8PWM;
- }
+ } else if (!strcmp(verb, "mode_port2_pwm12")) {
+ 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;
- }
- // } else {
- // fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n");
- // }
- }
- }
+ } else if (!strcmp(verb, "mode_port2_pwm16")) {
+ new_mode = PORT2_8PWM;
+ }
/* was a new mode set? */
if (new_mode != PORT_MODE_UNDEFINED) {
@@ -848,12 +835,17 @@ 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);
}
- /* 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");
+ fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
return -EINVAL;
}
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index 8e13f7c62..476adb7f0 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -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,63 @@ 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;
+ debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate);
+
+ 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)) {
+ warn("rate group %u mask %x bad overlap %x", group, mask, alt);
+ // 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) {
+ 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");
+ 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 +413,30 @@ 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;
}
+ debug("adjusted actuator update interval to %ums", update_rate_in_ms);
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 +593,6 @@ int
PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
- int channel;
lock();
@@ -541,7 +606,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 +624,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,12 +640,18 @@ 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 PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
@@ -812,7 +885,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 +937,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 +1032,31 @@ 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);
+ 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")) {
+ new_mode = PORT_FULL_PWM;
- } else if (!strcmp(argv[i], "mode_gpio_serial")) {
- new_mode = PORT_GPIO_AND_SERIAL;
+ } else if (!strcmp(verb, "mode_gpio_serial")) {
+ new_mode = PORT_GPIO_AND_SERIAL;
- } else if (!strcmp(argv[i], "mode_pwm_serial")) {
- new_mode = PORT_PWM_AND_SERIAL;
-
- } else if (!strcmp(argv[i], "mode_pwm_gpio")) {
- new_mode = PORT_PWM_AND_GPIO;
- }
-
- /* 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 if (!strcmp(verb, "mode_pwm_serial")) {
+ new_mode = PORT_PWM_AND_SERIAL;
- } else {
- errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio");
- }
- }
+ } else if (!strcmp(verb, "mode_pwm_gpio")) {
+ new_mode = PORT_PWM_AND_GPIO;
}
/* was a new mode set? */
@@ -1023,12 +1067,17 @@ fmu_main(int argc, char *argv[])
return OK;
/* switch modes */
- return fmu_new_mode(new_mode, pwm_update_rate_in_hz);
+ int ret = fmu_new_mode(new_mode);
+ exit(ret == OK ? 0 : 1);
}
- /* 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, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
exit(1);
}
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 791964087..4f6938a14 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -86,6 +86,8 @@
#include "uploader.h"
#include <debug.h>
+#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
+#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
class PX4IO : public device::I2C
{
@@ -98,6 +100,17 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
+ /**
+ * Set the update rate for actuator outputs from FMU to IO.
+ *
+ * @param rate The rate in Hz actuator outpus are sent to IO.
+ * Min 10 Hz, max 400 Hz
+ */
+ int set_update_rate(int rate);
+
+ /**
+ * Print the current status of IO
+ */
void print_status();
private:
@@ -122,7 +135,7 @@ private:
uint16_t _alarms;
/* subscribed topics */
- int _t_actuators; ///< actuator output topic
+ int _t_actuators; ///< actuator controls topic
int _t_armed; ///< system armed control topic
int _t_vstatus; ///< system / vehicle status
int _t_param; ///< parameter update topic
@@ -1223,14 +1236,15 @@ PX4IO::print_status()
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"));
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
- printf("alarms 0x%04x%s%s%s%s%s%s\n",
+ printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
alarms,
((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""),
- ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""));
+ ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""),
+ ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""));
printf("vbatt %u ibatt %u\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT));
@@ -1269,10 +1283,10 @@ PX4IO::print_status()
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
- printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n",
+ printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
- io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE),
- io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE),
+ io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
+ io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
printf("vbatt scale %u ibatt scale %u ibatt bias %u\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE),
@@ -1320,36 +1334,39 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
break;
- case PWM_SERVO_INAIR_RESTART_ENABLE:
- /* set the 'in-air restart' bit */
- ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK);
+ case PWM_SERVO_SET_UPDATE_RATE:
+ /* set the requested alternate rate */
+ if ((arg >= 50) && (arg <= 400)) { /* TODO: we could go higher for e.g. TurboPWM */
+ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
+ } else {
+ ret = -EINVAL;
+ }
break;
- case PWM_SERVO_INAIR_RESTART_DISABLE:
- /* unset the 'in-air restart' bit */
- ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0);
- break;
+ case PWM_SERVO_SELECT_UPDATE_RATE: {
- case PWM_SERVO_SET_UPDATE_RATE:
- /* set the requested rate */
- if ((arg >= 50) && (arg <= 400)) {
- ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE, arg);
- } else {
+ /* blindly clear the PWM update alarm - might be set for some other reason */
+ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
+
+ /* attempt to set the rate map */
+ io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg);
+
+ /* check that the changes took */
+ uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
+ if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) {
ret = -EINVAL;
+ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
}
break;
+ }
case PWM_SERVO_GET_COUNT:
*(unsigned *)arg = _max_actuators;
break;
- case PWM_SERVO_SET_DEBUG:
- /* set the debug level */
- ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
- break;
-
- case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): {
+ case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
+ /* TODO: we could go lower for e.g. TurboPWM */
unsigned channel = cmd - PWM_SERVO_SET(0);
if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
ret = -EINVAL;
@@ -1361,7 +1378,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break;
}
- case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS): {
+ case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): {
unsigned channel = cmd - PWM_SERVO_GET(0);
@@ -1379,6 +1396,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break;
}
+ case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
+
+ unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
+
+ uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
+
+ *(uint32_t *)arg = value;
+ break;
+ }
+
case GPIO_RESET:
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
break;
@@ -1443,6 +1470,20 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break;
}
+ case PX4IO_SET_DEBUG:
+ /* set the debug level */
+ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
+ break;
+
+ case PX4IO_INAIR_RESTART_ENABLE:
+ /* set/clear the 'in-air restart' bit */
+ if (arg) {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK);
+ } else {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0);
+ }
+ break;
+
default:
/* not a recognized value */
ret = -ENOTTY;
@@ -1466,12 +1507,51 @@ PX4IO::write(file *filp, const char *buffer, size_t len)
return count * 2;
}
+int
+PX4IO::set_update_rate(int rate)
+{
+ int interval_ms = 1000 / rate;
+ if (interval_ms < 5) {
+ interval_ms = 5;
+ warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
+ }
+
+ if (interval_ms > 100) {
+ interval_ms = 100;
+ warnx("update rate too low, limiting to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
+ }
+
+ _update_interval = interval_ms;
+ return 0;
+}
+
+
extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
namespace
{
void
+start(int argc, char *argv[])
+{
+ if (g_dev != nullptr)
+ errx(1, "already loaded");
+
+ /* create the driver - it will set g_dev */
+ (void)new PX4IO();
+
+ if (g_dev == nullptr)
+ errx(1, "driver alloc failed");
+
+ if (OK != g_dev->init()) {
+ delete g_dev;
+ errx(1, "driver init failed");
+ }
+
+ exit(0);
+}
+
+void
test(void)
{
int fd;
@@ -1565,32 +1645,20 @@ px4io_main(int argc, char *argv[])
if (argc < 2)
goto out;
- if (!strcmp(argv[1], "start")) {
-
- if (g_dev != nullptr)
- errx(1, "already loaded");
-
- /* create the driver - it will set g_dev */
- (void)new PX4IO();
+ if (!strcmp(argv[1], "start"))
+ start(argc - 1, argv + 1);
- if (g_dev == nullptr)
- errx(1, "driver alloc failed");
+ if (!strcmp(argv[1], "limit")) {
- if (OK != g_dev->init()) {
- delete g_dev;
- errx(1, "driver init failed");
- }
+ if (g_dev != nullptr) {
- /* look for the optional pwm update rate for the supported modes */
- if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) {
- if (argc > 2 + 1) {
-#warning implement this
+ if ((argc > 2)) {
+ g_dev->set_update_rate(atoi(argv[2]));
} else {
- fprintf(stderr, "missing argument for pwm update rate (-u)\n");
+ errx(1, "missing argument (50 - 200 Hz)");
return 1;
}
}
-
exit(0);
}
@@ -1602,7 +1670,7 @@ px4io_main(int argc, char *argv[])
* We can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
- g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0);
+ g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
} else {
errx(1, "not loaded");
}
@@ -1623,16 +1691,16 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
- if (g_dev != nullptr) {
- printf("[px4io] loaded\n");
- g_dev->print_status();
- } else {
- printf("[px4io] not loaded\n");
- }
-
- exit(0);
+ if (g_dev != nullptr) {
+ printf("[px4io] loaded\n");
+ g_dev->print_status();
+ } else {
+ printf("[px4io] not loaded\n");
}
+ exit(0);
+ }
+
if (!strcmp(argv[1], "debug")) {
if (argc <= 2) {
printf("usage: px4io debug LEVEL\n");
@@ -1646,7 +1714,7 @@ px4io_main(int argc, char *argv[])
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
- int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level);
+ int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
if (ret != 0) {
printf("SET_DEBUG failed - %d\n", ret);
exit(1);
@@ -1721,5 +1789,5 @@ px4io_main(int argc, char *argv[])
monitor();
out:
- errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug' or 'update'");
+ errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit' or 'update'");
}
diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c
index 2b8641f00..d7316e1f7 100644
--- a/apps/drivers/stm32/drv_pwm_servo.c
+++ b/apps/drivers/stm32/drv_pwm_servo.c
@@ -68,10 +68,6 @@
#include <stm32_gpio.h>
#include <stm32_tim.h>
-
-/* default rate (in Hz) of PWM updates */
-static uint32_t pwm_update_rate = 50;
-
#define REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET)
@@ -93,6 +89,10 @@ static uint32_t pwm_update_rate = 50;
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
+static void pwm_timer_init(unsigned timer);
+static void pwm_timer_set_rate(unsigned timer, unsigned rate);
+static void pwm_channel_init(unsigned channel);
+
static void
pwm_timer_init(unsigned timer)
{
@@ -113,11 +113,8 @@ pwm_timer_init(unsigned timer)
/* configure the timer to free-run at 1MHz */
rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
- /* and update at the desired rate */
- rARR(timer) = (1000000 / pwm_update_rate) - 1;
-
- /* generate an update event; reloads the counter and all registers */
- rEGR(timer) = GTIM_EGR_UG;
+ /* default to updating at 50Hz */
+ pwm_timer_set_rate(timer, 50);
/* note that the timer is left disabled - arming is performed separately */
}
@@ -272,19 +269,41 @@ up_pwm_servo_deinit(void)
}
int
-up_pwm_servo_set_rate(unsigned rate)
+up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
{
- if ((rate < 50) || (rate > 400))
+ /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
+ if (rate < 1)
+ return -ERANGE;
+ if (rate > 10000)
return -ERANGE;
- for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
- if (pwm_timers[i].base != 0)
- pwm_timer_set_rate(i, rate);
- }
+ if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0))
+ return ERROR;
+
+ pwm_timer_set_rate(group, rate);
return OK;
}
+int
+up_pwm_servo_set_rate(unsigned rate)
+{
+ for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++)
+ up_pwm_servo_set_rate_group_update(i, rate);
+}
+
+uint32_t
+up_pwm_servo_get_rate_group(unsigned group)
+{
+ unsigned channels = 0;
+
+ for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
+ if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group))
+ channels |= (1 << i);
+ }
+ return channels;
+}
+
void
up_pwm_servo_arm(bool armed)
{
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-08 03:38:52 +0000