From 0d13c41108710f28bf3ec7ca8aa5cfff003f9ebe Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 26 Jan 2016 09:45:12 -1000 Subject: [PATCH] Ran Astyle --- src/drivers/drv_input_capture.h | 2 +- src/drivers/px4fmu/fmu.cpp | 133 ++++++++++++++++---------- src/drivers/stm32/drv_input_capture.c | 31 +++++- src/drivers/stm32/drv_io_timer.c | 3 +- src/drivers/stm32/drv_pwm_servo.c | 1 + 5 files changed, 115 insertions(+), 55 deletions(-) diff --git a/src/drivers/drv_input_capture.h b/src/drivers/drv_input_capture.h index 5a931dafa3..4d6fef29bb 100644 --- a/src/drivers/drv_input_capture.h +++ b/src/drivers/drv_input_capture.h @@ -176,7 +176,7 @@ typedef struct input_capture_config_t { */ __EXPORT int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter, - capture_callback_t callback, void *context); + capture_callback_t callback, void *context); __EXPORT int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter); __EXPORT int up_input_capture_set_filter(unsigned channel, capture_filter_t filter); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a3887708fe..dc4a0cfd56 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -127,8 +127,8 @@ 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); + hrt_abstime edge_time, uint32_t edge_state, + uint32_t overflow); private: enum RC_SCAN { @@ -206,7 +206,7 @@ private: uint8_t control_index, float &input); void capture_callback(uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); + 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); @@ -429,8 +429,9 @@ PX4FMU::set_mode(Mode 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 + DEVICE_DEBUG("MODE_2PWM2CAP"); + + // no break case MODE_2PWM: // v1 multi-port with flow control lines as PWM DEVICE_DEBUG("MODE_2PWM"); @@ -448,7 +449,8 @@ PX4FMU::set_mode(Mode mode) 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 + + // no break case MODE_3PWM: // v1 multi-port with flow control lines as PWM DEVICE_DEBUG("MODE_3PWM"); @@ -680,16 +682,16 @@ PX4FMU::cycle_trampoline(void *arg) void PX4FMU::capture_trampoline(void *context, uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) + 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) + 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); + 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], @@ -1583,6 +1585,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = -EINVAL; break; } + /* FALLTHROUGH */ case PWM_SERVO_GET(2): if (_mode < MODE_3PWM) { @@ -2150,8 +2153,7 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) input_capture_stats_t *stats = (input_capture_stats_t *)arg; - if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP) - { + if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP) { pconfig = (input_capture_config_t *)arg; } @@ -2159,100 +2161,112 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case INPUT_CAP_SET: - if (pconfig) - { + if (pconfig) { ret = up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter, - pconfig->callback, pconfig->context); + pconfig->callback, pconfig->context); } + break; case INPUT_CAP_SET_CALLBACK: - if (pconfig) - { + if (pconfig) { ret = up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context); } + break; case INPUT_CAP_GET_CALLBACK: - if (pconfig) - { + if (pconfig) { ret = up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context); } + break; case INPUT_CAP_GET_STATS: - if (arg) - { + if (arg) { ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, false); } + break; case INPUT_CAP_GET_CLR_STATS: - if (arg) - { + if (arg) { ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, true); } + break; case INPUT_CAP_SET_EDGE: - if (pconfig) - { + if (pconfig) { ret = up_input_capture_set_trigger(pconfig->channel, pconfig->edge); } + break; case INPUT_CAP_GET_EDGE: - if (pconfig) - { + if (pconfig) { ret = up_input_capture_get_trigger(pconfig->channel, &pconfig->edge); } + break; + case INPUT_CAP_SET_FILTER: - if (pconfig) - { + if (pconfig) { ret = up_input_capture_set_filter(pconfig->channel, pconfig->filter); } + break; + case INPUT_CAP_GET_FILTER: - if (pconfig) - { + if (pconfig) { ret = up_input_capture_get_filter(pconfig->channel, &pconfig->filter); } + break; case INPUT_CAP_GET_COUNT: ret = OK; - switch(_mode) { + + switch (_mode) { case MODE_3PWM1CAP: *(unsigned *)arg = 1; - break; + break; + case MODE_2PWM2CAP: *(unsigned *)arg = 2; - break; + break; + default: ret = -EINVAL; break; } + break; case INPUT_CAP_SET_COUNT: ret = OK; - switch(_mode) { + + switch (_mode) { case MODE_3PWM1CAP: set_mode(MODE_3PWM1CAP); - break; + break; + case MODE_2PWM2CAP: set_mode(MODE_2PWM2CAP); - break; + break; + default: ret = -EINVAL; break; } + break; + default: ret = -ENOTTY; break; - } + } + unlock(); return ret; } @@ -2557,24 +2571,29 @@ test(void) } if (ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) { - fprintf(stdout,"Not in a capture mode\n"); + 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++) { + + if (capture_count != 0) { + for (unsigned i = 0; i < capture_count; i++) { // Map to channel number - capture_conf[i].chan.channel = i+ servo_count; + 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); } @@ -2584,7 +2603,9 @@ test(void) } struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; warnx("Press CTRL-C or 'c' to abort."); @@ -2643,26 +2664,30 @@ 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_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); + 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); @@ -2678,11 +2703,11 @@ test(void) } } - if (capture_count !=0 ) { - for(unsigned i = 0; i < capture_count; i++) { + if (capture_count != 0) { + for (unsigned i = 0; i < capture_count; i++) { // Map to channel number if (capture_conf[i].valid) { - /* Save handler */ + /* 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); } @@ -2774,12 +2799,16 @@ 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 diff --git a/src/drivers/stm32/drv_input_capture.c b/src/drivers/stm32/drv_input_capture.c index c3da341049..a2b0cf7629 100644 --- a/src/drivers/stm32/drv_input_capture.c +++ b/src/drivers/stm32/drv_input_capture.c @@ -154,7 +154,9 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt if (edge > Both) { return -EINVAL; } + int rv = io_timer_validate_channel_index(channel); + if (rv == 0) { if (edge == Disabled) { @@ -176,20 +178,24 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt } rv = up_input_capture_set_filter(channel, filter); + if (rv == 0) { rv = up_input_capture_set_trigger(channel, edge); + if (rv == 0) { rv = io_timer_set_enable(true, IOTimerChanMode_Capture, 1 << channel); } } } } + return rv; } int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter) { int rv = io_timer_validate_channel_index(channel); + if (rv == 0) { rv = -ENXIO; @@ -201,6 +207,7 @@ int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter) uint32_t timer = timer_io_channels[channel].timer_index; uint16_t rvalue; rv = OK; + switch (timer_io_channels[channel].timer_channel) { case 1: @@ -228,6 +235,7 @@ int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter) } } } + return rv; } int up_input_capture_set_filter(unsigned channel, capture_filter_t filter) @@ -237,6 +245,7 @@ int up_input_capture_set_filter(unsigned channel, capture_filter_t filter) } int rv = io_timer_validate_channel_index(channel); + if (rv == 0) { rv = -ENXIO; @@ -280,15 +289,18 @@ int up_input_capture_set_filter(unsigned channel, capture_filter_t filter) default: rv = -EIO; } + irqrestore(flags); } } + return rv; } int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge) { int rv = io_timer_validate_channel_index(channel); + if (rv == 0) { rv = -ENXIO; @@ -301,6 +313,7 @@ int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge) uint32_t timer = timer_io_channels[channel].timer_index; uint16_t rvalue; + switch (timer_io_channels[channel].timer_channel) { case 1: @@ -325,29 +338,35 @@ int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge) default: rv = -EIO; } + if (rv == 0) { - switch(rvalue) { + switch (rvalue) { case 0: *edge = Rising; break; + case (GTIM_CCER_CC1P | GTIM_CCER_CC1NP): *edge = Both; break; + case (GTIM_CCER_CC1P): *edge = Falling; break; + default: rv = -EIO; } } } } + return rv; } int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge) { int rv = io_timer_validate_channel_index(channel); + if (rv == 0) { rv = -ENXIO; @@ -417,15 +436,18 @@ int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge) default: rv = -EIO; } + irqrestore(flags); } } + return rv; } int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback, void **context) { int rv = io_timer_validate_channel_index(channel); + if (rv == 0) { rv = -ENXIO; @@ -441,6 +463,7 @@ int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback rv = OK; } } + return rv; } @@ -448,6 +471,7 @@ int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, void *context) { int rv = io_timer_validate_channel_index(channel); + if (rv == 0) { rv = -ENXIO; @@ -459,19 +483,24 @@ int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, rv = 0; } } + return rv; } int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear) { int rv = io_timer_validate_channel_index(channel); + if (rv == 0) { irqstate_t flags = irqsave(); *stats = channel_stats[channel]; + if (clear) { memset(&channel_stats[channel], 0, sizeof(*stats)); } + irqrestore(flags); } + return rv; } diff --git a/src/drivers/stm32/drv_io_timer.c b/src/drivers/stm32/drv_io_timer.c index d4801074fb..13acd717ec 100644 --- a/src/drivers/stm32/drv_io_timer.c +++ b/src/drivers/stm32/drv_io_timer.c @@ -300,6 +300,7 @@ int io_timer_get_mode_channels(io_timer_channel_mode_t mode) if (mode < IOTimerChanModeSize) { return channel_allocations[mode]; } + return 0; } @@ -671,7 +672,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann } else { - rCR1(actions) = 0; + rCR1(actions) = 0; } irqrestore(flags); diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index df76267045..bee1cccdd6 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -107,6 +107,7 @@ int up_pwm_servo_init(uint32_t channel_mask) channel_mask &= ~(1 << channel); } } + return OK; }