diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7c125d75a1..a3887708fe 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include @@ -70,6 +71,7 @@ #include #include #include +#include #include #include @@ -99,9 +101,15 @@ public: enum Mode { MODE_NONE, MODE_2PWM, + MODE_2PWM2CAP, + MODE_3PWM, + MODE_3PWM1CAP, MODE_4PWM, MODE_6PWM, MODE_8PWM, + MODE_4CAP, + MODE_5CAP, + MODE_6CAP, }; PX4FMU(); virtual ~PX4FMU(); @@ -118,6 +126,10 @@ public: int set_i2c_bus_clock(unsigned bus, unsigned clock_hz); + static void capture_trampoline(void *context, uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, + uint32_t overflow); + private: enum RC_SCAN { RC_SCAN_PPM = 0, @@ -185,7 +197,6 @@ private: static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); } static void cycle_trampoline(void *arg); - void cycle(); void work_start(); void work_stop(); @@ -194,6 +205,8 @@ private: uint8_t control_group, uint8_t control_index, float &input); + void capture_callback(uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); void subscribe(); int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); @@ -217,6 +230,8 @@ private: uint32_t gpio_read(void); int gpio_ioctl(file *filp, int cmd, unsigned long arg); + int capture_ioctl(file *filp, int cmd, unsigned long arg); + /* do not allow to copy due to ptr data members */ PX4FMU(const PX4FMU &); PX4FMU operator=(const PX4FMU &); @@ -411,6 +426,11 @@ PX4FMU::set_mode(Mode mode) * are presented on the output pins. */ switch (mode) { + case MODE_2PWM2CAP: // v1 multi-port with flow control lines as PWM + up_input_capture_set(2, Rising, 0, NULL, NULL); + up_input_capture_set(3, Rising, 0, NULL, NULL); + DEVICE_DEBUG("MODE_2PWM2CAP"); + // no break case MODE_2PWM: // v1 multi-port with flow control lines as PWM DEVICE_DEBUG("MODE_2PWM"); @@ -425,6 +445,23 @@ PX4FMU::set_mode(Mode mode) break; + case MODE_3PWM1CAP: // v1 multi-port with flow control lines as PWM + DEVICE_DEBUG("MODE_3PWM1CAP"); + up_input_capture_set(3, Rising, 0, NULL, NULL); + // no break + case MODE_3PWM: // v1 multi-port with flow control lines as PWM + DEVICE_DEBUG("MODE_3PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0x7); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + break; + case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs DEVICE_DEBUG("MODE_4PWM"); @@ -641,6 +678,19 @@ PX4FMU::cycle_trampoline(void *arg) dev->cycle(); } +void +PX4FMU::capture_trampoline(void *context, uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +{ + PX4FMU *dev = reinterpret_cast(context); + dev->capture_callback(chan_index, edge_time, edge_state, overflow); +} + +void PX4FMU::capture_callback(uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +{ + fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state,overflow); +} void PX4FMU::fill_rc_in(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], hrt_abstime now, bool frame_drop, bool failsafe, @@ -812,9 +862,15 @@ PX4FMU::cycle() switch (_mode) { case MODE_2PWM: + case MODE_2PWM2CAP: num_outputs = 2; break; + case MODE_3PWM: + case MODE_3PWM1CAP: + num_outputs = 3; + break; + case MODE_4PWM: num_outputs = 4; break; @@ -1208,10 +1264,20 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) return ret; } + /* try it as a Capture ioctl next */ + ret = capture_ioctl(filp, cmd, arg); + + if (ret != -ENOTTY) { + return ret; + } + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ switch (_mode) { case MODE_2PWM: + case MODE_3PWM: case MODE_4PWM: + case MODE_2PWM2CAP: + case MODE_3PWM1CAP: case MODE_6PWM: #ifdef CONFIG_ARCH_BOARD_AEROCORE case MODE_8PWM: @@ -1469,12 +1535,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case PWM_SERVO_SET(3): - case PWM_SERVO_SET(2): if (_mode < MODE_4PWM) { ret = -EINVAL; break; } + /* FALLTHROUGH */ + case PWM_SERVO_SET(2): + if (_mode < MODE_3PWM) { + ret = -EINVAL; + break; + } + /* FALLTHROUGH */ case PWM_SERVO_SET(1): case PWM_SERVO_SET(0): @@ -1507,11 +1579,16 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case PWM_SERVO_GET(3): - case PWM_SERVO_GET(2): if (_mode < MODE_4PWM) { ret = -EINVAL; break; } + /* FALLTHROUGH */ + case PWM_SERVO_GET(2): + if (_mode < MODE_3PWM) { + ret = -EINVAL; + break; + } /* FALLTHROUGH */ case PWM_SERVO_GET(1): @@ -1550,7 +1627,13 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) *(unsigned *)arg = 4; break; + case MODE_3PWM: + case MODE_3PWM1CAP: + *(unsigned *)arg = 3; + break; + case MODE_2PWM: + case MODE_2PWM2CAP: *(unsigned *)arg = 2; break; @@ -1578,6 +1661,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_2PWM); break; + case 3: + set_mode(MODE_3PWM); + break; + case 4: set_mode(MODE_4PWM); break; @@ -2052,6 +2139,124 @@ PX4FMU::gpio_read(void) return bits; } +int +PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -EINVAL; + + lock(); + + input_capture_config_t *pconfig = 0; + + input_capture_stats_t *stats = (input_capture_stats_t *)arg; + + if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP) + { + + pconfig = (input_capture_config_t *)arg; + } + + switch (cmd) { + + case INPUT_CAP_SET: + if (pconfig) + { + ret = up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter, + pconfig->callback, pconfig->context); + } + break; + + case INPUT_CAP_SET_CALLBACK: + if (pconfig) + { + ret = up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context); + } + break; + + case INPUT_CAP_GET_CALLBACK: + if (pconfig) + { + ret = up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context); + } + break; + + case INPUT_CAP_GET_STATS: + if (arg) + { + ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, false); + } + break; + + case INPUT_CAP_GET_CLR_STATS: + if (arg) + { + ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, true); + } + break; + + case INPUT_CAP_SET_EDGE: + if (pconfig) + { + ret = up_input_capture_set_trigger(pconfig->channel, pconfig->edge); + } + break; + + case INPUT_CAP_GET_EDGE: + if (pconfig) + { + ret = up_input_capture_get_trigger(pconfig->channel, &pconfig->edge); + } + break; + case INPUT_CAP_SET_FILTER: + if (pconfig) + { + ret = up_input_capture_set_filter(pconfig->channel, pconfig->filter); + } + break; + case INPUT_CAP_GET_FILTER: + if (pconfig) + { + ret = up_input_capture_get_filter(pconfig->channel, &pconfig->filter); + } + break; + + case INPUT_CAP_GET_COUNT: + ret = OK; + switch(_mode) { + case MODE_3PWM1CAP: + *(unsigned *)arg = 1; + break; + case MODE_2PWM2CAP: + *(unsigned *)arg = 2; + break; + default: + ret = -EINVAL; + break; + } + break; + + case INPUT_CAP_SET_COUNT: + ret = OK; + switch(_mode) { + case MODE_3PWM1CAP: + set_mode(MODE_3PWM1CAP); + break; + case MODE_2PWM2CAP: + set_mode(MODE_2PWM2CAP); + break; + default: + ret = -EINVAL; + break; + } + break; + default: + ret = -ENOTTY; + break; + } + unlock(); + return ret; +} + int PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) { @@ -2135,6 +2340,11 @@ enum PortMode { PORT_PWM_AND_SERIAL, PORT_PWM_AND_GPIO, PORT_PWM4, + PORT_PWM3, + PORT_PWM2, + PORT_PWM3CAP1, + PORT_PWM2CAP2, + PORT_CAPTURE, }; PortMode g_port_mode; @@ -2176,6 +2386,26 @@ fmu_new_mode(PortMode new_mode) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; break; + + case PORT_PWM3: + /* select 4-pin PWM mode */ + servo_mode = PX4FMU::MODE_3PWM; + break; + + case PORT_PWM3CAP1: + /* select 3-pin PWM mode 1 capture */ + servo_mode = PX4FMU::MODE_3PWM1CAP; + break; + + case PORT_PWM2: + /* select 2-pin PWM mode */ + servo_mode = PX4FMU::MODE_2PWM; + break; + + case PORT_PWM2CAP2: + /* select 2-pin PWM mode 2 capture */ + servo_mode = PX4FMU::MODE_2PWM2CAP; + break; #endif /* mixed modes supported on v1 board only */ @@ -2304,9 +2534,15 @@ test(void) { int fd; unsigned servo_count = 0; + unsigned capture_count = 0; unsigned pwm_value = 1000; int direction = 1; int ret; + uint32_t rate_limit = 0; + struct input_capture_t { + bool valid; + input_capture_config_t chan; + } capture_conf[INPUT_CAPTURE_MAX_CHANNELS]; fd = open(PX4FMU_DEVICE_PATH, O_RDWR); @@ -2320,7 +2556,32 @@ test(void) err(1, "Unable to get servo count\n"); } - warnx("Testing %u servos", (unsigned)servo_count); + if (ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) { + fprintf(stdout,"Not in a capture mode\n"); + } + + warnx("Testing %u servos and %u input captures", (unsigned)servo_count, capture_count); + memset(capture_conf, 0, sizeof(capture_conf)); + if (capture_count !=0 ) { + for(unsigned i = 0; i < capture_count; i++) { + // Map to channel number + capture_conf[i].chan.channel = i+ servo_count; + /* Save handler */ + if (ioctl(fd, INPUT_CAP_GET_CALLBACK, (unsigned long)&capture_conf[i].chan.channel) != 0) { + err(1, "Unable to get capture callback for chan %u\n", capture_conf[i].chan.channel); + } else { + input_capture_config_t conf = capture_conf[i].chan; + conf.callback = &PX4FMU::capture_trampoline; + conf.context = g_fmu; + if (ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) { + capture_conf[i].valid = true; + } else { + err(1, "Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel); + } + } + + } + } struct pollfd fds; fds.fd = 0; /* stdin */ @@ -2382,7 +2643,26 @@ test(void) errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); } } + if (capture_count !=0 && (++rate_limit % 500 == 0)) { + for(unsigned i = 0; i < capture_count; i++) { + if (capture_conf[i].valid) { + input_capture_stats_t stats; + stats.chan_in_edges_out = capture_conf[i].chan.channel; + if (ioctl(fd, INPUT_CAP_GET_STATS, (unsigned long)&stats) != 0) { + err(1, "Unable to get stats for chan %u\n", capture_conf[i].chan.channel); + } else { + fprintf(stdout, "FMU: Status chan:%u edges: %d last time:%lld last state:%d overflows:%d lantency:%u\n", + capture_conf[i].chan.channel, + stats.chan_in_edges_out, + stats.last_time, + stats.last_edge, + stats.overflows, + stats.latnecy); + } + } + } + } /* Check if user wants to quit */ char c; ret = poll(&fds, 1, 0); @@ -2398,8 +2678,21 @@ test(void) } } + if (capture_count !=0 ) { + for(unsigned i = 0; i < capture_count; i++) { + // Map to channel number + if (capture_conf[i].valid) { + /* Save handler */ + if (ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&capture_conf[i].chan) != 0) { + err(1, "Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel); + } + } + } + } + close(fd); + exit(0); } @@ -2481,6 +2774,14 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; + } else if (!strcmp(verb, "mode_pwm2")) { + new_mode = PORT_PWM2; + } else if (!strcmp(verb, "mode_pwm3")) { + new_mode = PORT_PWM3; + } else if (!strcmp(verb, "mode_pwm3cap1")) { + new_mode = PORT_PWM3CAP1; + } else if (!strcmp(verb, "mode_pwm2cap2")) { + new_mode = PORT_PWM2CAP2; #endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)