diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-03-17 02:49:18 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-03-17 02:49:18 -0700 |
commit | 1ae4edab220e93405cd99f105648c4a8f948b41f (patch) | |
tree | 60cdc225b5349fdb48f93a84920ade185145b518 | |
parent | 39218d5fcc82a0dbb8bbe61c00952e8ccc7c28b8 (diff) | |
parent | b5b460e7ca7339bce024697bed076a8362f9dc67 (diff) | |
download | px4-firmware-1ae4edab220e93405cd99f105648c4a8f948b41f.tar.gz px4-firmware-1ae4edab220e93405cd99f105648c4a8f948b41f.tar.bz2 px4-firmware-1ae4edab220e93405cd99f105648c4a8f948b41f.zip |
Merge pull request #224 from PX4/pwm-multirate
Pwm multirate
-rw-r--r-- | apps/drivers/drv_pwm_output.h | 47 | ||||
-rw-r--r-- | apps/drivers/hil/hil.cpp | 86 | ||||
-rw-r--r-- | apps/drivers/px4fmu/fmu.cpp | 219 | ||||
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 176 | ||||
-rw-r--r-- | apps/drivers/stm32/drv_pwm_servo.c | 49 | ||||
-rw-r--r-- | apps/px4io/mixer.cpp | 9 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 23 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 4 | ||||
-rw-r--r-- | apps/px4io/registers.c | 81 | ||||
-rw-r--r-- | apps/systemcmds/pwm/Makefile | 42 | ||||
-rw-r--r-- | apps/systemcmds/pwm/pwm.c | 207 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/nsh/appconfig | 1 |
12 files changed, 697 insertions, 247 deletions
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 27c885ed7..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: @@ -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) { diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ec69cdd64..f38593d2a 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -93,13 +93,11 @@ mixer_tick(void) /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - lowsyslog("AP RX timeout"); + isr_debug(1, "AP RX timeout"); } r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; - /* XXX this is questionable - vehicle may not make sense for direct control */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; @@ -179,7 +177,10 @@ mixer_tick(void) /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)); + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && + /* FMU is available or FMU is not available but override is an option */ + ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) + ); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 14d221b5b..8d8b7e941 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -76,7 +76,7 @@ #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) /* static configuration page */ -#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ @@ -88,7 +88,7 @@ #define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ /* dynamic status page */ -#define PX4IO_PAGE_STATUS 1 +#define PX4IO_PAGE_STATUS 1 #define PX4IO_P_STATUS_FREEMEM 0 #define PX4IO_P_STATUS_CPULOAD 1 @@ -112,28 +112,33 @@ #define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ #define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ #define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ #define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ #define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */ /* array of post-mix actuator outputs, -10000..10000 */ -#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* array of PWM servo output values, microseconds */ -#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* array of raw RC input values, microseconds */ -#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_PAGE_RAW_RC_INPUT 4 #define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ #define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ /* array of scaled RC input values, -10000..10000 */ -#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_PAGE_RC_INPUT 5 #define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ #define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ /* array of raw ADC values */ -#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ #define PX4IO_PAGE_SETUP 100 @@ -146,8 +151,8 @@ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ -#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_PWM_HIGHRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ #define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ #define PX4IO_P_SETUP_IBATT_SCALE 7 /* battery current scaling factor (float) */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 7b4b07c2c..202e9d9d9 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -94,8 +94,8 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] #define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] -#define r_setup_pwm_lowrate r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] -#define r_setup_pwm_highrate r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] +#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] +#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #define r_control_values (&r_page_controls[0]) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 645c1565d..6c09def9e 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -48,6 +48,7 @@ #include "protocol.h" static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); +static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate); /** * PAGE 0 @@ -116,11 +117,12 @@ uint16_t r_page_rc_input[] = { }; /** - * PAGE 6 + * Scratch page; used for registers that are constructed as-read. * - * Raw ADC input. + * PAGE 6 Raw ADC input. + * PAGE 7 PWM rate maps. */ -uint16_t r_page_adc[ADC_CHANNEL_COUNT]; +uint16_t r_page_scratch[32]; /** * PAGE 100 @@ -132,8 +134,8 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_FEATURES] = 0, [PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0, - [PX4IO_P_SETUP_PWM_LOWRATE] = 50, - [PX4IO_P_SETUP_PWM_HIGHRATE] = 200, + [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, + [PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_RELAYS] = 0, [PX4IO_P_SETUP_VBATT_SCALE] = 10000, [PX4IO_P_SETUP_IBATT_SCALE] = 0, @@ -321,26 +323,23 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_PWM_RATES: value &= PX4IO_P_SETUP_RATES_VALID; - r_setup_pwm_rates = value; - /* XXX re-configure timers */ + pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); break; - case PX4IO_P_SETUP_PWM_LOWRATE: + case PX4IO_P_SETUP_PWM_DEFAULTRATE: if (value < 50) value = 50; if (value > 400) value = 400; - r_setup_pwm_lowrate = value; - /* XXX re-configure timers */ + pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; - case PX4IO_P_SETUP_PWM_HIGHRATE: + case PX4IO_P_SETUP_PWM_ALTRATE: if (value < 50) value = 50; if (value > 400) value = 400; - r_setup_pwm_highrate = value; - /* XXX re-configure timers */ + pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; case PX4IO_P_SETUP_RELAYS: @@ -529,10 +528,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val break; case PX4IO_PAGE_RAW_ADC_INPUT: - r_page_adc[0] = adc_measure(ADC_VBATT); - r_page_adc[1] = adc_measure(ADC_IN5); + memset(r_page_scratch, 0, sizeof(r_page_scratch)); + r_page_scratch[0] = adc_measure(ADC_VBATT); + r_page_scratch[1] = adc_measure(ADC_IN5); - SELECT_PAGE(r_page_adc); + SELECT_PAGE(r_page_scratch); + break; + + case PX4IO_PAGE_PWM_INFO: + memset(r_page_scratch, 0, sizeof(r_page_scratch)); + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); + + SELECT_PAGE(r_page_scratch); break; /* @@ -593,3 +601,44 @@ last_offset = offset; return 0; } + +/* + * Helper function to handle changes to the PWM rate control registers. + */ +static void +pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) +{ + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < IO_SERVO_COUNT; 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 = map & mask; + + if (pass == 0) { + /* preflight */ + 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 (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 { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + } + } + } + r_setup_pwm_rates = map; + r_setup_pwm_defaultrate = defaultrate; + r_setup_pwm_altrate = altrate; +} diff --git a/apps/systemcmds/pwm/Makefile b/apps/systemcmds/pwm/Makefile new file mode 100644 index 000000000..60885aa0a --- /dev/null +++ b/apps/systemcmds/pwm/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build the pwm tool. +# + +APPNAME = pwm +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/pwm/pwm.c b/apps/systemcmds/pwm/pwm.c new file mode 100644 index 000000000..bf4a1b579 --- /dev/null +++ b/apps/systemcmds/pwm/pwm.c @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pwm.c + * + * PWM servo output configuration and monitoring tool. + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <sys/mount.h> +#include <sys/ioctl.h> +#include <sys/stat.h> + +#include <nuttx/i2c.h> +#include <nuttx/mtd.h> +#include <nuttx/fs/nxffs.h> +#include <nuttx/fs/ioctl.h> + +#include <arch/board/board.h> + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" +#include "drivers/drv_pwm_output.h" + +static void usage(const char *reason); +__EXPORT int pwm_main(int argc, char *argv[]); + + +static void +usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + errx(1, + "usage:\n" + "pwm [-v] [-d <device>] [-u <alt_rate>] [-c <alt_channel_mask>] [arm|disarm] [<channel_value> ...]\n" + " -v Print information about the PWM device\n" + " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " <alt_rate> PWM update rate for channels in <alt_channel_mask>\n" + " <alt_channel_mask> Bitmask of channels to update at the alternate rate\n" + " arm | disarm Arm or disarm the ouptut\n" + " <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"); +} + +int +pwm_main(int argc, char *argv[]) +{ + const char *dev = PWM_OUTPUT_DEVICE_PATH; + unsigned alt_rate = 0; + uint32_t alt_channels; + bool alt_channels_set = false; + bool print_info = false; + int ch; + int ret; + char *ep; + + while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) { + switch (ch) { + case 'c': + alt_channels = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad alt_channel_mask value"); + alt_channels_set = true; + break; + + case 'd': + dev = optarg; + break; + + case 'u': + alt_rate = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad alt_rate value"); + + case 'v': + print_info = true; + break; + + default: + usage(NULL); + } + } + argc -= optind; + argv += optind; + + /* open for ioctl only */ + int fd = open(dev, 0); + if (fd < 0) + err(1, "can't open %s", dev); + + /* change alternate PWM rate */ + if (alt_rate > 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + if (ret != OK) + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } + + /* assign alternate rate to channels */ + if (alt_channels_set) { + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, alt_channels); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE (check mask vs. device capabilities)"); + } + + /* iterate remaining arguments */ + unsigned channel = 0; + while (argc--) { + const char *arg = argv[0]; + argv++; + if (!strcmp(arg, "arm")) { + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); + continue; + } + if (!strcmp(arg, "disarm")) { + ret = ioctl(fd, PWM_SERVO_DISARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_DISARM"); + continue; + } + unsigned pwm_value = strtol(arg, &ep, 0); + if (*ep == '\0') { + ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", channel); + channel++; + continue; + } + usage("unrecognised option"); + } + + /* print verbose info */ + if (print_info) { + /* get the number of servo channels */ + unsigned count; + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); + + /* print current servo values */ + printf("PWM output values:\n"); + for (unsigned i = 0; i < count; i++) { + servo_position_t spos; + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); + if (ret == OK) { + printf("%u: %uus\n", i, spos); + } else { + printf("%u: ERROR\n", i); + } + } + + /* print rate groups */ + printf("Available alt_channel_mask groups:\n"); + for (unsigned i = 0; i < count; i++) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); + if (ret != OK) + break; + if (group_mask != 0) + printf(" 0x%x", group_mask); + } + printf("\n"); + fflush(stdout); + } + exit(0); +}
\ No newline at end of file diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index f1f70e228..80cf6f312 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -67,6 +67,7 @@ CONFIGURED_APPS += systemcmds/boardinfo CONFIGURED_APPS += systemcmds/mixer CONFIGURED_APPS += systemcmds/eeprom CONFIGURED_APPS += systemcmds/param +CONFIGURED_APPS += systemcmds/pwm CONFIGURED_APPS += systemcmds/bl_update CONFIGURED_APPS += systemcmds/preflight_check CONFIGURED_APPS += systemcmds/delay_test |