From 3efaeabd5b3aa7c78789221412890e51bf646fba Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 09:42:34 +0100 Subject: [PATCH] pwm_input: astyle --- src/drivers/pwm_input/pwm_input.cpp | 205 ++++++++++++++++------------ 1 file changed, 117 insertions(+), 88 deletions(-) diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index c8d3308c14..be01e848db 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file pwm_input.cpp + * @file pwm_input.cpp * * PWM input driver based on earlier driver from Evan Slatyer, * which in turn was based on drv_hrt.c @@ -192,32 +192,32 @@ * Specific registers and bits used by HRT sub-functions */ #if PWMIN_TIMER_CHANNEL == 1 - #define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */ - #define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */ - #define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */ - #define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */ - #define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */ - #define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT)) - #define CCMR2_PWMIN 0 - #define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) - #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) - #define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT) - #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */ +#define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */ +#define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */ +#define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */ +#define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */ +#define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT)) +#define CCMR2_PWMIN 0 +#define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) +#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) +#define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT) +#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) #elif PWMIN_TIMER_CHANNEL == 2 - #define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */ - #define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */ - #define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */ - #define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */ - #define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */ - #define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */ - #define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT)) - #define CCMR2_PWMIN 0 - #define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) - #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) - #define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT) - #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */ +#define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */ +#define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */ +#define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */ +#define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */ +#define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */ +#define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT)) +#define CCMR2_PWMIN 0 +#define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) +#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) +#define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT) +#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) #else - #error PWMIN_TIMER_CHANNEL must be either 1 and 2. +#error PWMIN_TIMER_CHANNEL must be either 1 and 2. #endif #define TIMEOUT 300000 /* reset after no responce over this time in microseconds [0.3 secs] */ @@ -235,18 +235,18 @@ public: void _publish(uint16_t status, uint32_t period, uint32_t pulse_width); void _print_info(void); - void hard_reset(); + void hard_reset(); private: uint32_t error_count; uint32_t pulses_captured; uint32_t last_period; uint32_t last_width; - uint64_t last_poll_time; + uint64_t last_poll_time; RingBuffer *reports; bool timer_started; - range_finder_report data; + range_finder_report data; orb_advert_t range_finder_pub; hrt_call hard_reset_call; // HRT callout for note completion @@ -254,9 +254,9 @@ private: void timer_init(void); - void turn_on(); - void turn_off(); - void freeze_test(); + void turn_on(); + void turn_off(); + void freeze_test(); }; @@ -276,15 +276,16 @@ PWMIN::PWMIN() : last_width(0), reports(nullptr), timer_started(false), - range_finder_pub(-1) + range_finder_pub(-1) { memset(&data, 0, sizeof(data)); } PWMIN::~PWMIN() { - if (reports != nullptr) + if (reports != nullptr) { delete reports; + } } /* @@ -300,21 +301,22 @@ PWMIN::init() // activate the timer when requested to when the device is opened CDev::init(); - data.type = RANGE_FINDER_TYPE_LASER; - data.minimum_distance = 0.20f; - data.maximum_distance = 7.0f; + data.type = RANGE_FINDER_TYPE_LASER; + data.minimum_distance = 0.20f; + data.maximum_distance = 7.0f; - range_finder_pub = orb_advertise(ORB_ID(sensor_range_finder), &data); - fprintf(stderr, "[pwm_input] advertising %d\n" - ,range_finder_pub); + range_finder_pub = orb_advertise(ORB_ID(sensor_range_finder), &data); + fprintf(stderr, "[pwm_input] advertising %d\n" + , range_finder_pub); reports = new RingBuffer(2, sizeof(struct pwm_input_s)); + if (reports == nullptr) { return -ENOMEM; } - // Schedule freeze check to invoke periodically - hrt_call_every(&freeze_test_call, 0, TIMEOUT, reinterpret_cast(&PWMIN::freeze_test), this); + // Schedule freeze check to invoke periodically + hrt_call_every(&freeze_test_call, 0, TIMEOUT, reinterpret_cast(&PWMIN::freeze_test), this); return OK; } @@ -322,7 +324,7 @@ PWMIN::init() /* * Initialise the timer we are going to use. */ -void PWMIN::timer_init(void) +void PWMIN::timer_init(void) { // run with interrupts disabled in case the timer is already // setup. We don't want it firing while we are doing the setup @@ -333,7 +335,7 @@ void PWMIN::timer_init(void) irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr); /* Clear no bits, set timer enable bit.*/ - modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); + modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); /* disable and configure the timer */ rCR1 = 0; @@ -351,10 +353,10 @@ void PWMIN::timer_init(void) // for simplicity scale by the clock in MHz. This gives us // readings in microseconds which is typically what is needed // for a PWM input driver - uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL; + uint32_t prescaler = PWMIN_TIMER_CLOCK / 1000000UL; - /* - * define the clock speed. We want the highest possible clock + /* + * define the clock speed. We want the highest possible clock * speed that avoids overflows. */ rPSC = prescaler - 1; @@ -380,13 +382,14 @@ void PWMIN::timer_init(void) void PWMIN::freeze_test() { - /* timeout is true if least read was away back */ - bool timeout = false; - timeout = (hrt_absolute_time() - last_poll_time > TIMEOUT) ? true : false; - if (timeout) { - fprintf(stderr, "[pwm_input] Lidar is down, reseting\n"); - hard_reset(); - } + /* timeout is true if least read was away back */ + bool timeout = false; + timeout = (hrt_absolute_time() - last_poll_time > TIMEOUT) ? true : false; + + if (timeout) { + fprintf(stderr, "[pwm_input] Lidar is down, reseting\n"); + hard_reset(); + } } void @@ -398,8 +401,8 @@ PWMIN::turn_off() { stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); } void PWMIN::hard_reset() { - turn_off(); - hrt_call_after(&hard_reset_call, 9000, reinterpret_cast(&PWMIN::turn_on), this); + turn_off(); + hrt_call_after(&hard_reset_call, 9000, reinterpret_cast(&PWMIN::turn_on), this); } /* @@ -412,10 +415,13 @@ PWMIN::open(struct file *filp) if (g_dev == nullptr) { return -EIO; } - int ret = CDev::open(filp); + + int ret = CDev::open(filp); + if (ret == OK && !timer_started) { g_dev->timer_init(); } + return ret; } @@ -429,14 +435,17 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 500)) + if ((arg < 1) || (arg > 500)) { return -EINVAL; + } irqstate_t flags = irqsave(); + if (!reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } + irqrestore(flags); return OK; @@ -471,8 +480,9 @@ PWMIN::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } while (count--) { if (reports->get(buf)) { @@ -496,7 +506,7 @@ void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) return; } - last_poll_time = hrt_absolute_time(); + last_poll_time = hrt_absolute_time(); struct pwm_input_s pwmin_report; pwmin_report.timestamp = last_poll_time; @@ -504,18 +514,20 @@ void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) pwmin_report.period = period; pwmin_report.pulse_width = pulse_width; - data.distance = pulse_width * 1e-3f; - data.timestamp = pwmin_report.timestamp; - data.error_count = error_count; + data.distance = pulse_width * 1e-3f; + data.timestamp = pwmin_report.timestamp; + data.error_count = error_count; - if (data.distance < data.minimum_distance || data.distance > data.maximum_distance) { - data.valid = false; - } else { - data.valid = true; - } - if (range_finder_pub > 0) { - orb_publish(ORB_ID(sensor_range_finder), range_finder_pub, &data); - } + if (data.distance < data.minimum_distance || data.distance > data.maximum_distance) { + data.valid = false; + + } else { + data.valid = true; + } + + if (range_finder_pub > 0) { + orb_publish(ORB_ID(sensor_range_finder), range_finder_pub, &data); + } reports->force(&pwmin_report); @@ -531,6 +543,7 @@ void PWMIN::_print_info(void) { if (!timer_started) { printf("timer not started - try the 'test' command\n"); + } else { printf("count=%u period=%u width=%u\n", (unsigned)pulses_captured, @@ -568,20 +581,27 @@ static void pwmin_start(bool full_start) printf("driver already started\n"); exit(1); } + g_dev = new PWMIN(); + if (g_dev == nullptr) { errx(1, "driver allocation failed"); } + if (g_dev->init() != OK) { errx(1, "driver init failed"); } - if (full_start) { - int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); - if (fd == -1) { - errx(1, "Failed to open device"); - } - close(fd); - } + + if (full_start) { + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + + if (fd == -1) { + errx(1, "Failed to open device"); + } + + close(fd); + } + exit(0); } @@ -591,15 +611,18 @@ static void pwmin_start(bool full_start) static void pwmin_test(void) { int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { errx(1, "Failed to open device"); } + uint64_t start_time = hrt_absolute_time(); printf("Showing samples for 5 seconds\n"); - while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) { + while (hrt_absolute_time() < start_time + 5U * 1000UL * 1000UL) { struct pwm_input_s buf; + if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) { printf("period=%u width=%u error_count=%u\n", (unsigned)buf.period, @@ -607,6 +630,7 @@ static void pwmin_test(void) (unsigned)buf.error_count); } } + close(fd); exit(0); } @@ -616,14 +640,17 @@ static void pwmin_test(void) */ static void pwmin_reset(void) { - g_dev->hard_reset(); + g_dev->hard_reset(); int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { errx(1, "Failed to open device"); } + if (ioctl(fd, SENSORIOCRESET, 0) != OK) { errx(1, "reset failed"); } + close(fd); exit(0); } @@ -637,6 +664,7 @@ static void pwmin_info(void) printf("driver not started\n"); exit(1); } + g_dev->_print_info(); exit(0); } @@ -645,22 +673,23 @@ static void pwmin_info(void) /* driver entry point */ -int pwm_input_main(int argc, char * argv[]) +int pwm_input_main(int argc, char *argv[]) { const char *verb = argv[1]; - /* - * init driver and start reading - */ - bool full_start = false; - if (!strcmp(argv[2], "full")) { - full_start = true; - } + /* + * init driver and start reading + */ + bool full_start = false; + + if (!strcmp(argv[2], "full")) { + full_start = true; + } /* * Start/load the driver. */ if (!strcmp(verb, "start")) { - pwmin_start(full_start); + pwmin_start(full_start); } /*