From 2fc10320697ecaa9c4e0c52d4d047424e41e6336 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 23 Oct 2012 23:38:45 -0700 Subject: Major formatting/whitespace cleanup --- apps/drivers/bma180/bma180.cpp | 44 +++-- apps/drivers/boards/px4fmu/px4fmu_adc.c | 19 +- apps/drivers/boards/px4fmu/px4fmu_can.c | 47 +++-- apps/drivers/boards/px4fmu/px4fmu_init.c | 264 ++++++++++++++------------- apps/drivers/boards/px4fmu/px4fmu_spi.c | 5 +- apps/drivers/boards/px4fmu/px4fmu_usb.c | 16 +- apps/drivers/device/device.h | 18 +- apps/drivers/device/i2c.cpp | 6 +- apps/drivers/device/i2c.h | 2 +- apps/drivers/device/spi.cpp | 2 + apps/drivers/device/spi.h | 2 +- apps/drivers/drv_accel.h | 2 +- apps/drivers/drv_gyro.h | 2 +- apps/drivers/drv_hrt.h | 4 +- apps/drivers/drv_mag.h | 2 +- apps/drivers/drv_pwm_output.h | 2 +- apps/drivers/drv_sensor.h | 6 +- apps/drivers/drv_tone_alarm.h | 4 +- apps/drivers/hmc5883/hmc5883.cpp | 59 ++++-- apps/drivers/l3gd20/l3gd20.cpp | 30 ++- apps/drivers/mpu6000/mpu6000.cpp | 30 ++- apps/drivers/ms5611/ms5611.cpp | 36 +++- apps/drivers/px4io/px4io.cpp | 36 +++- apps/drivers/px4io/uploader.cpp | 47 ++++- apps/drivers/stm32/drv_hrt.c | 89 +++++---- apps/drivers/stm32/drv_pwm_servo.c | 115 +++++++----- apps/drivers/stm32/tone_alarm/tone_alarm.cpp | 87 +++++---- 27 files changed, 603 insertions(+), 373 deletions(-) (limited to 'apps/drivers') diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp index 8b22b1326..bc4d4b3bf 100644 --- a/apps/drivers/bma180/bma180.cpp +++ b/apps/drivers/bma180/bma180.cpp @@ -289,6 +289,7 @@ BMA180::init() _num_reports = 2; _oldest_report = _next_report = 0; _reports = new struct accel_report[_num_reports]; + if (_reports == nullptr) goto out; @@ -321,13 +322,16 @@ BMA180::init() modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); if (set_range(4)) warnx("Failed setting range"); + if (set_lowpass(75)) warnx("Failed setting lowpass"); if (read_reg(ADDR_CHIP_ID) == CHIP_ID) { ret = OK; + } else { ret = ERROR; } + out: return ret; } @@ -441,6 +445,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_call_interval == 0) return SENSOR_POLLRATE_MANUAL; + return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { @@ -468,7 +473,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGQUEUEDEPTH: - return _num_reports -1; + return _num_reports - 1; case SENSORIOCRESET: /* XXX implement */ @@ -488,12 +493,12 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: /* copy scale in */ - memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale)); + memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); return OK; case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -549,24 +554,30 @@ BMA180::set_range(unsigned max_g) if (max_g == 0) max_g = 16; + if (max_g > 16) return -ERANGE; if (max_g <= 2) { _current_range = 2; rangebits = OFFSET_LSB1_RANGE_2G; + } else if (max_g <= 3) { _current_range = 3; rangebits = OFFSET_LSB1_RANGE_3G; + } else if (max_g <= 4) { _current_range = 4; rangebits = OFFSET_LSB1_RANGE_4G; + } else if (max_g <= 8) { _current_range = 8; rangebits = OFFSET_LSB1_RANGE_8G; + } else if (max_g <= 16) { _current_range = 16; rangebits = OFFSET_LSB1_RANGE_16G; + } else { return -EINVAL; } @@ -586,7 +597,7 @@ BMA180::set_range(unsigned max_g) /* check if wanted value is now in register */ return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) == - (OFFSET_LSB1_RANGE_MASK & rangebits)); + (OFFSET_LSB1_RANGE_MASK & rangebits)); } int @@ -633,7 +644,7 @@ BMA180::set_lowpass(unsigned frequency) /* check if wanted value is now in register */ return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) == - (BW_TCS_BW_MASK & bwbits)); + (BW_TCS_BW_MASK & bwbits)); } void @@ -703,9 +714,9 @@ BMA180::measure() * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+1)) << 8) | (read_reg(ADDR_ACC_X_LSB));// XXX PX4DEV raw_report.x; - report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+3)) << 8) | (read_reg(ADDR_ACC_X_LSB+2));// XXX PX4DEV raw_report.y; - report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+5)) << 8) | (read_reg(ADDR_ACC_X_LSB+4));// XXX PX4DEV raw_report.z; + report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 1)) << 8) | (read_reg(ADDR_ACC_X_LSB)); // XXX PX4DEV raw_report.x; + report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 3)) << 8) | (read_reg(ADDR_ACC_X_LSB + 2)); // XXX PX4DEV raw_report.y; + report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 5)) << 8) | (read_reg(ADDR_ACC_X_LSB + 4)); // XXX PX4DEV raw_report.z; /* discard two non-value bits in the 16 bit measurement */ report->x_raw = (report->x_raw >> 2); @@ -781,17 +792,21 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -809,16 +824,18 @@ test() /* get the driver */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) - err(1, "%s open failed (try 'bma180 start' if the driver is not running)", - ACCEL_DEVICE_PATH); + err(1, "%s open failed (try 'bma180 start' if the driver is not running)", + ACCEL_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) err(1, "reset to manual polling"); - + /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); + if (sz != sizeof(a_report)) err(1, "immediate acc read failed"); @@ -831,7 +848,7 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); + (double)(a_report.range_m_s2 / 9.81f)); /* XXX add poll-rate tests here too */ @@ -846,10 +863,13 @@ void reset() { int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); diff --git a/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c index 2ea855955..987894163 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_adc.c +++ b/apps/drivers/boards/px4fmu/px4fmu_adc.c @@ -99,25 +99,22 @@ int adc_devinit(void) /* Check if we have already initialized */ - if (!initialized) - { + if (!initialized) { char name[11]; - for (i = 0; i < ADC3_NCHANNELS; i++) - { - stm32_configgpio(g_pinlist[i]); + for (i = 0; i < ADC3_NCHANNELS; i++) { + stm32_configgpio(g_pinlist[i]); } - for (i = 0; i < 1; i++) - { + for (i = 0; i < 1; i++) { /* Configure the pins as analog inputs for the selected channels */ //stm32_configgpio(g_pinlist[i]); /* Call stm32_adcinitialize() to get an instance of the ADC interface */ //multiple channels only supported with dma! adc[i] = stm32_adcinitialize(3, (g_chanlist), 4); - if (adc == NULL) - { + + if (adc == NULL) { adbg("ERROR: Failed to get ADC interface\n"); return -ENODEV; } @@ -126,8 +123,8 @@ int adc_devinit(void) /* Register the ADC driver at "/dev/adc0" */ sprintf(name, "/dev/adc%d", i); ret = adc_register(name, adc[i]); - if (ret < 0) - { + + if (ret < 0) { adbg("adc_register failed for adc %s: %d\n", name, ret); return ret; } diff --git a/apps/drivers/boards/px4fmu/px4fmu_can.c b/apps/drivers/boards/px4fmu/px4fmu_can.c index 92d96e558..0d0b5fcd3 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_can.c +++ b/apps/drivers/boards/px4fmu/px4fmu_can.c @@ -107,36 +107,35 @@ int can_devinit(void) { - static bool initialized = false; - struct can_dev_s *can; - int ret; + static bool initialized = false; + struct can_dev_s *can; + int ret; - /* Check if we have already initialized */ + /* Check if we have already initialized */ - if (!initialized) - { - /* Call stm32_caninitialize() to get an instance of the CAN interface */ + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ - can = stm32_caninitialize(CAN_PORT); - if (can == NULL) - { - candbg("ERROR: Failed to get CAN interface\n"); - return -ENODEV; - } + can = stm32_caninitialize(CAN_PORT); - /* Register the CAN driver at "/dev/can0" */ + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } - ret = can_register("/dev/can0", can); - if (ret < 0) - { - candbg("ERROR: can_register failed: %d\n", ret); - return ret; - } + /* Register the CAN driver at "/dev/can0" */ - /* Now we are initialized */ + ret = can_register("/dev/can0", can); - initialized = true; - } + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } - return OK; + /* Now we are initialized */ + + initialized = true; + } + + return OK; } diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index ba6cdd764..e5ded7328 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -34,7 +34,7 @@ /** * @file px4fmu_init.c * - * PX4FMU-specific early startup code. This file implements the + * PX4FMU-specific early startup code. This file implements the * nsh_archinitialize() function that is called early by nsh during startup. * * Code here is run before the rcS script is invoked; it should start required @@ -115,11 +115,11 @@ extern int adc_devinit(void); __EXPORT void stm32_boardinitialize(void) { - /* configure SPI interfaces */ - stm32_spiinitialize(); + /* configure SPI interfaces */ + stm32_spiinitialize(); - /* configure LEDs */ - up_ledinit(); + /* configure LEDs */ + up_ledinit(); } /**************************************************************************** @@ -139,171 +139,179 @@ static struct i2c_dev_s *i2c3; #include #ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) { - return 1; +__EXPORT int matherr(struct __exception *e) +{ + return 1; } #else -__EXPORT int matherr(struct exception *e) { - return 1; +__EXPORT int matherr(struct exception *e) +{ + return 1; } #endif __EXPORT int nsh_archinitialize(void) { - int result; + int result; - /* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */ + /* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */ - /* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */ + /* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */ - /* configure the high-resolution time/callout interface */ + /* configure the high-resolution time/callout interface */ #ifdef CONFIG_HRT_TIMER - hrt_init(); + hrt_init(); #endif - /* configure CPU load estimation */ - #ifdef CONFIG_SCHED_INSTRUMENTATION - cpuload_initialize_once(); - #endif + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif - /* set up the serial DMA polling */ + /* set up the serial DMA polling */ #ifdef SERIAL_HAVE_DMA - { - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - } + { + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + } #endif - message("\r\n"); + message("\r\n"); + + up_ledoff(LED_BLUE); + up_ledoff(LED_AMBER); - up_ledoff(LED_BLUE); - up_ledoff(LED_AMBER); + up_ledon(LED_BLUE); - up_ledon(LED_BLUE); + /* Configure user-space led driver */ + px4fmu_led_init(); - /* Configure user-space led driver */ - px4fmu_led_init(); + /* Configure SPI-based devices */ - /* Configure SPI-based devices */ + spi1 = up_spiinitialize(1); - spi1 = up_spiinitialize(1); - if (!spi1) - { - message("[boot] FAILED to initialize SPI port 1\r\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } - // Default SPI1 to 1MHz and de-assert the known chip selects. - SPI_SETFREQUENCY(spi1, 10000000); - SPI_SETBITS(spi1, 8); - SPI_SETMODE(spi1, SPIDEV_MODE3); - SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); - SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); - up_udelay(20); + // Default SPI1 to 1MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); - message("[boot] Successfully initialized SPI port 1\r\n"); + message("[boot] Successfully initialized SPI port 1\r\n"); - /* initialize I2C2 bus */ + /* initialize I2C2 bus */ - i2c2 = up_i2cinitialize(2); - if (!i2c2) { - message("[boot] FAILED to initialize I2C bus 2\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } + i2c2 = up_i2cinitialize(2); - /* set I2C2 speed */ - I2C_SETFREQUENCY(i2c2, 400000); + if (!i2c2) { + message("[boot] FAILED to initialize I2C bus 2\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + /* set I2C2 speed */ + I2C_SETFREQUENCY(i2c2, 400000); - i2c3 = up_i2cinitialize(3); - if (!i2c3) { - message("[boot] FAILED to initialize I2C bus 3\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - /* set I2C3 speed */ - I2C_SETFREQUENCY(i2c3, 400000); + i2c3 = up_i2cinitialize(3); - /* try to attach, don't fail if device is not responding */ - (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS, - FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, - FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, - FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); + if (!i2c3) { + message("[boot] FAILED to initialize I2C bus 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* set I2C3 speed */ + I2C_SETFREQUENCY(i2c3, 400000); + + /* try to attach, don't fail if device is not responding */ + (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS, + FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, + FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, + FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); #if defined(CONFIG_STM32_SPI3) - /* Get the SPI port */ - - message("[boot] Initializing SPI port 3\n"); - spi3 = up_spiinitialize(3); - if (!spi3) - { - message("[boot] FAILED to initialize SPI port 3\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - message("[boot] Successfully initialized SPI port 3\n"); - - /* Now bind the SPI interface to the MMCSD driver */ - result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); - if (result != OK) - { - message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); + /* Get the SPI port */ + + message("[boot] Initializing SPI port 3\n"); + spi3 = up_spiinitialize(3); + + if (!spi3) { + message("[boot] FAILED to initialize SPI port 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully initialized SPI port 3\n"); + + /* Now bind the SPI interface to the MMCSD driver */ + result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); + + if (result != OK) { + message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); #endif /* SPI3 */ - /* initialize I2C1 bus */ + /* initialize I2C1 bus */ - i2c1 = up_i2cinitialize(1); - if (!i2c1) { - message("[boot] FAILED to initialize I2C bus 1\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } + i2c1 = up_i2cinitialize(1); - /* set I2C1 speed */ - I2C_SETFREQUENCY(i2c1, 400000); + if (!i2c1) { + message("[boot] FAILED to initialize I2C bus 1\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } - /* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */ + /* set I2C1 speed */ + I2C_SETFREQUENCY(i2c1, 400000); - /* Get board information if available */ + /* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */ - /* Initialize the user GPIOs */ - px4fmu_gpio_init(); + /* Get board information if available */ + + /* Initialize the user GPIOs */ + px4fmu_gpio_init(); #ifdef CONFIG_ADC - int adc_state = adc_devinit(); - if (adc_state != OK) - { - /* Try again */ - adc_state = adc_devinit(); - if (adc_state != OK) - { - /* Give up */ - message("[boot] FAILED adc_devinit: %d\n", adc_state); - return -ENODEV; - } - } + int adc_state = adc_devinit(); + + if (adc_state != OK) { + /* Try again */ + adc_state = adc_devinit(); + + if (adc_state != OK) { + /* Give up */ + message("[boot] FAILED adc_devinit: %d\n", adc_state); + return -ENODEV; + } + } + #endif - return OK; + return OK; } diff --git a/apps/drivers/boards/px4fmu/px4fmu_spi.c b/apps/drivers/boards/px4fmu/px4fmu_spi.c index fbb6191d6..70245a3ec 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_spi.c +++ b/apps/drivers/boards/px4fmu/px4fmu_spi.c @@ -95,21 +95,24 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_MPU, selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected); break; + case PX4_SPIDEV_ACCEL: /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected); stm32_gpiowrite(GPIO_SPI_CS_MPU, selected); stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected); break; + case PX4_SPIDEV_MPU: /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected); stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected); stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; + default: break; - + } } diff --git a/apps/drivers/boards/px4fmu/px4fmu_usb.c b/apps/drivers/boards/px4fmu/px4fmu_usb.c index d635e719b..b0b669fbe 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_usb.c +++ b/apps/drivers/boards/px4fmu/px4fmu_usb.c @@ -77,16 +77,16 @@ __EXPORT void stm32_usbinitialize(void) { - /* The OTG FS has an internal soft pull-up */ + /* The OTG FS has an internal soft pull-up */ - /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ #ifdef CONFIG_STM32_OTGFS - stm32_configgpio(GPIO_OTGFS_VBUS); - /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); - */ + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ #endif } @@ -103,6 +103,6 @@ __EXPORT void stm32_usbinitialize(void) __EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) { - ulldbg("resume: %d\n", resume); + ulldbg("resume: %d\n", resume); } diff --git a/apps/drivers/device/device.h b/apps/drivers/device/device.h index 9af678465..01692c315 100644 --- a/apps/drivers/device/device.h +++ b/apps/drivers/device/device.h @@ -54,7 +54,7 @@ /** * Namespace encapsulating all device framework classes, functions and data. */ -namespace device __EXPORT +namespace device __EXPORT { /** @@ -276,14 +276,14 @@ public: */ virtual int poll(struct file *filp, struct pollfd *fds, bool setup); - /** - * Test whether the device is currently open. - * - * This can be used to avoid tearing down a device that is still active. - * - * @return True if the device is currently open. - */ - bool is_open() { return _open_count > 0; } + /** + * Test whether the device is currently open. + * + * This can be used to avoid tearing down a device that is still active. + * + * @return True if the device is currently open. + */ + bool is_open() { return _open_count > 0; } protected: /** diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index c4b2aa944..4b832b548 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -121,7 +121,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len unsigned tries = 0; do { - // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); msgs = 0; @@ -144,7 +144,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len if (msgs == 0) return -EINVAL; - /* + /* * I2C architecture means there is an unavoidable race here * if there are any devices on the bus with a different frequency * preference. Really, this is pointless. @@ -154,7 +154,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len if (ret == OK) break; - + // reset the I2C bus to unwedge on error up_i2creset(_dev); diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 7c5a14d6b..eb1b6cb05 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -42,7 +42,7 @@ #include -namespace device __EXPORT +namespace device __EXPORT { /** diff --git a/apps/drivers/device/spi.cpp b/apps/drivers/device/spi.cpp index a1761b769..528333e04 100644 --- a/apps/drivers/device/spi.cpp +++ b/apps/drivers/device/spi.cpp @@ -134,6 +134,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* do common setup */ if (!up_interrupt_context()) SPI_LOCK(_dev, true); + SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); SPI_SETBITS(_dev, 8); @@ -144,6 +145,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); + if (!up_interrupt_context()) SPI_LOCK(_dev, false); diff --git a/apps/drivers/device/spi.h b/apps/drivers/device/spi.h index b2a111562..e8c8e2c5e 100644 --- a/apps/drivers/device/spi.h +++ b/apps/drivers/device/spi.h @@ -84,7 +84,7 @@ protected: * If called from interrupt context, this interface does not lock * the bus and may interfere with non-interrupt-context callers. * - * Clients in a mixed interrupt/non-interrupt configuration must + * Clients in a mixed interrupt/non-interrupt configuration must * ensure appropriate interlocking. * * At least one of send or recv must be non-null. diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h index 6d0c8c545..3834795e0 100644 --- a/apps/drivers/drv_accel.h +++ b/apps/drivers/drv_accel.h @@ -83,7 +83,7 @@ ORB_DECLARE(sensor_accel); /* * ioctl() definitions * - * Accelerometer drivers also implement the generic sensor driver + * Accelerometer drivers also implement the generic sensor driver * interfaces from drv_sensor.h */ diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h index 5c646f243..0dbf05c5b 100644 --- a/apps/drivers/drv_gyro.h +++ b/apps/drivers/drv_gyro.h @@ -58,7 +58,7 @@ struct gyro_report { float temperature; /**< temperature in degrees celcius */ float range_rad_s; float scaling; - + int16_t x_raw; int16_t y_raw; int16_t z_raw; diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h index a6d501f53..3b493a81a 100644 --- a/apps/drivers/drv_hrt.h +++ b/apps/drivers/drv_hrt.h @@ -62,7 +62,7 @@ typedef uint64_t hrt_abstime; * they are serialised with respect to each other, and must not * block. */ -typedef void (* hrt_callout)(void *arg); +typedef void (* hrt_callout)(void *arg); /* * Callout record. @@ -113,7 +113,7 @@ __EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, h __EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg); /* - * If this returns true, the entry has been invoked and removed from the callout list, + * If this returns true, the entry has been invoked and removed from the callout list, * or it has never been entered. * * Always returns false for repeating callouts. diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 7ba9dd695..114bcb646 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -59,7 +59,7 @@ struct mag_report { float z; float range_ga; float scaling; - + int16_t x_raw; int16_t y_raw; int16_t z_raw; diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index fc458e9b0..97033f2cd 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -136,7 +136,7 @@ __EXPORT extern void up_pwm_servo_deinit(void); * * When disarmed, servos output no pulse. * - * @bug This function should, but does not, guarantee that any pulse + * @bug This function should, but does not, guarantee that any pulse * currently in progress is cleanly completed. * * @param armed If true, outputs are armed; if false they diff --git a/apps/drivers/drv_sensor.h b/apps/drivers/drv_sensor.h index 325bd42bf..3a89cab9d 100644 --- a/apps/drivers/drv_sensor.h +++ b/apps/drivers/drv_sensor.h @@ -52,7 +52,7 @@ #define _SENSORIOC(_n) (_IOC(_SENSORIOCBASE, _n)) /** - * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE + * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE * constants */ #define SENSORIOCSPOLLRATE _SENSORIOC(0) @@ -68,8 +68,8 @@ #define SENSOR_POLLRATE_MAX 1000002 /**< poll at device maximum rate */ #define SENSOR_POLLRATE_DEFAULT 1000003 /**< poll at driver normal rate */ -/** - * Set the internal queue depth to (arg) entries, must be at least 1 +/** + * Set the internal queue depth to (arg) entries, must be at least 1 * * This sets the upper bound on the number of readings that can be * read from the driver. diff --git a/apps/drivers/drv_tone_alarm.h b/apps/drivers/drv_tone_alarm.h index 27b146de9..0c6afc64c 100644 --- a/apps/drivers/drv_tone_alarm.h +++ b/apps/drivers/drv_tone_alarm.h @@ -34,8 +34,8 @@ /* * Driver for the PX4 audio alarm port, /dev/tone_alarm. * - * The tone_alarm driver supports a set of predefined "alarm" - * patterns and one user-supplied pattern. Patterns are ordered by + * The tone_alarm driver supports a set of predefined "alarm" + * patterns and one user-supplied pattern. Patterns are ordered by * priority, with a higher-priority pattern interrupting any * lower-priority pattern that might be playing. * diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index bfe79302d..7943012cc 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -332,13 +332,16 @@ HMC5883::init() /* allocate basic report buffers */ _num_reports = 2; _reports = new struct mag_report[_num_reports]; + if (_reports == nullptr) goto out; + _oldest_report = _next_report = 0; /* get a publish handle on the mag topic */ memset(&_reports[0], 0, sizeof(_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); + if (_mag_topic < 0) debug("failed to create sensor_mag object"); @@ -358,30 +361,37 @@ int HMC5883::set_range(unsigned range) range_bits = 0x00; _range_scale = 1.0f / 1370.0f; _range_ga = 0.88f; + } else if (range <= 1) { range_bits = 0x01; _range_scale = 1.0f / 1090.0f; _range_ga = 1.3f; + } else if (range <= 2) { range_bits = 0x02; _range_scale = 1.0f / 820.0f; _range_ga = 1.9f; + } else if (range <= 3) { range_bits = 0x03; _range_scale = 1.0f / 660.0f; _range_ga = 2.5f; + } else if (range <= 4) { range_bits = 0x04; _range_scale = 1.0f / 440.0f; _range_ga = 4.0f; + } else if (range <= 4.7f) { range_bits = 0x05; _range_scale = 1.0f / 390.0f; _range_ga = 4.7f; + } else if (range <= 5.6f) { range_bits = 0x06; _range_scale = 1.0f / 330.0f; _range_ga = 5.6f; + } else { range_bits = 0x07; _range_scale = 1.0f / 230.0f; @@ -413,10 +423,12 @@ HMC5883::probe() uint8_t data[3] = {0, 0, 0}; _retries = 10; + if (read_reg(ADDR_ID_A, data[0]) || read_reg(ADDR_ID_B, data[1]) || read_reg(ADDR_ID_C, data[2])) debug("read_reg fail"); + _retries = 1; if ((data[0] != ID_A_WHO_AM_I) || @@ -552,6 +564,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; + return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { @@ -666,7 +679,7 @@ HMC5883::cycle() if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) { /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, + work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, @@ -684,7 +697,7 @@ HMC5883::cycle() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, + work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, @@ -850,7 +863,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) warnx("starting mag scale calibration"); /* do a simple demand read */ - sz = read(filp, (char*)&report, sizeof(report)); + sz = read(filp, (char *)&report, sizeof(report)); + if (sz != sizeof(report)) { warn("immediate read failed"); ret = 1; @@ -920,6 +934,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) if (sz != sizeof(report)) { warn("periodic read failed"); goto out; + } else { avg_excited[0] += report.x; avg_excited[1] += report.y; @@ -946,7 +961,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) scaling[2] = fabsf(1.08f / avg_excited[2]); warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - + /* set back to normal mode */ /* Set to 1.1 Gauss */ if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { @@ -971,12 +986,15 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = OK; - out: - if (ret == OK) { - warnx("mag scale calibration successfully finished."); - } else { - warnx("mag scale calibration failed."); - } +out: + + if (ret == OK) { + warnx("mag scale calibration successfully finished."); + + } else { + warnx("mag scale calibration failed."); + } + return ret; } @@ -986,16 +1004,22 @@ int HMC5883::set_excitement(unsigned enable) /* arm the excitement strap */ uint8_t conf_reg; ret = read_reg(ADDR_CONF_A, conf_reg); + if (OK != ret) perf_count(_comms_errors); + if (((int)enable) < 0) { conf_reg |= 0x01; + } else if (enable > 0) { conf_reg |= 0x02; + } else { conf_reg &= ~0x03; } + ret = write_reg(ADDR_CONF_A, conf_reg); + if (OK != ret) perf_count(_comms_errors); @@ -1087,17 +1111,22 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -1114,11 +1143,13 @@ test() int ret; int fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); + if (sz != sizeof(report)) err(1, "immediate read failed"); @@ -1167,7 +1198,7 @@ test() * Basic idea: * * output = (ext field +- 1.1 Ga self-test) * scale factor - * + * * and consequently: * * 1.1 Ga = (excited - normal) * scale factor @@ -1207,6 +1238,7 @@ int calibrate() int ret; int fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); @@ -1218,6 +1250,7 @@ int calibrate() if (ret == OK) { errx(0, "PASS"); + } else { errx(1, "FAIL"); } @@ -1230,10 +1263,13 @@ void reset() { int fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); @@ -1290,6 +1326,7 @@ hmc5883_main(int argc, char *argv[]) if (!strcmp(argv[1], "calibrate")) { if (hmc5883::calibrate() == 0) { errx(0, "calibration successful"); + } else { errx(1, "calibration failed"); } diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index 4606cd654..4915b81e3 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -317,6 +317,7 @@ L3GD20::init() _num_reports = 2; _oldest_report = _next_report = 0; _reports = new struct gyro_report[_num_reports]; + if (_reports == nullptr) goto out; @@ -330,7 +331,7 @@ L3GD20::init() write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); - + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ @@ -451,6 +452,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_call_interval == 0) return SENSOR_POLLRATE_MANUAL; + return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { @@ -478,7 +480,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGQUEUEDEPTH: - return _num_reports -1; + return _num_reports - 1; case SENSORIOCRESET: /* XXX implement */ @@ -497,12 +499,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -562,12 +564,15 @@ L3GD20::set_range(unsigned max_dps) if (max_dps <= 250) { _current_range = 250; bits |= RANGE_250DPS; + } else if (max_dps <= 500) { _current_range = 500; bits |= RANGE_500DPS; + } else if (max_dps <= 2000) { _current_range = 2000; bits |= RANGE_2000DPS; + } else { return -EINVAL; } @@ -590,15 +595,19 @@ L3GD20::set_samplerate(unsigned frequency) if (frequency <= 95) { _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; + } else if (frequency <= 190) { _current_rate = 190; bits |= RATE_190HZ_LP_25HZ; + } else if (frequency <= 380) { _current_rate = 380; bits |= RATE_380HZ_LP_30HZ; + } else if (frequency <= 760) { _current_rate = 760; bits |= RATE_760HZ_LP_30HZ; + } else { return -EINVAL; } @@ -746,17 +755,21 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(GYRO_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -774,15 +787,17 @@ test() /* get the driver */ fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + if (fd_gyro < 0) err(1, "%s open failed", GYRO_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) err(1, "reset to manual polling"); - + /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); + if (sz != sizeof(g_report)) err(1, "immediate gyro read failed"); @@ -793,7 +808,7 @@ test() warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f)); + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); /* XXX add poll-rate tests here too */ @@ -808,10 +823,13 @@ void reset() { int fd = open(GYRO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 18b139b32..90786886a 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -569,6 +569,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_call_interval == 0) return SENSOR_POLLRATE_MANUAL; + return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: @@ -592,12 +593,12 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: /* copy scale in */ - memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale)); + memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); return OK; case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -639,12 +640,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -976,17 +977,21 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -1006,21 +1011,24 @@ test() /* get the driver */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) - err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - ACCEL_DEVICE_PATH); + err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", + ACCEL_DEVICE_PATH); /* get the driver */ fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + if (fd_gyro < 0) err(1, "%s open failed", GYRO_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) err(1, "reset to manual polling"); - + /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); + if (sz != sizeof(a_report)) err(1, "immediate acc read failed"); @@ -1033,10 +1041,11 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); + (double)(a_report.range_m_s2 / 9.81f)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); + if (sz != sizeof(g_report)) err(1, "immediate gyro read failed"); @@ -1047,7 +1056,7 @@ test() warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f)); + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); @@ -1066,10 +1075,13 @@ void reset() { int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 893ef6c37..699cd36d2 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -303,6 +303,7 @@ MS5611::init() /* allocate basic report buffers */ _num_reports = 2; _reports = new struct baro_report[_num_reports]; + if (_reports == nullptr) goto out; @@ -311,6 +312,7 @@ MS5611::init() /* get a publish handle on the baro topic */ memset(&_reports[0], 0, sizeof(_reports[0])); _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); + if (_baro_topic < 0) debug("failed to create sensor_baro object"); @@ -323,9 +325,10 @@ int MS5611::probe() { _retries = 10; - if((OK == probe_address(MS5611_ADDRESS_1)) || - (OK == probe_address(MS5611_ADDRESS_2))) { - _retries = 1; + + if ((OK == probe_address(MS5611_ADDRESS_1)) || + (OK == probe_address(MS5611_ADDRESS_2))) { + _retries = 1; return OK; } @@ -484,6 +487,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; + return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { @@ -518,9 +522,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case BAROIOCSMSLPRESSURE: + /* range-check for sanity */ if ((arg < 80000) || (arg > 120000)) return -EINVAL; + _msl_pressure = arg; return OK; @@ -688,7 +694,7 @@ MS5611::collect() int64_t SENS2 = 5 * f >> 2; if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); + int64_t f2 = POW2(_TEMP + 1500); OFF2 += 7 * f2; SENS2 += 11 * f2 >> 1; } @@ -697,6 +703,7 @@ MS5611::collect() _OFF -= OFF2; _SENS -= SENS2; } + } else { /* pressure calculation, result in Pa */ @@ -814,8 +821,8 @@ MS5611::read_prom() uint16_t w; } cvt; - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are * called immediately after reset. */ usleep(3000); @@ -941,17 +948,22 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -968,11 +980,13 @@ test() int ret; int fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); + if (sz != sizeof(report)) err(1, "immediate read failed"); @@ -1025,10 +1039,13 @@ void reset() { int fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); @@ -1061,6 +1078,7 @@ calibrate(unsigned altitude) float p1; int fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); @@ -1070,6 +1088,7 @@ calibrate(unsigned altitude) /* average a few measurements */ pressure = 0.0f; + for (unsigned i = 0; i < 20; i++) { struct pollfd fds; int ret; @@ -1091,6 +1110,7 @@ calibrate(unsigned altitude) pressure += report.pressure; } + pressure /= 20; /* average */ pressure /= 10; /* scale from millibar to kPa */ @@ -1108,8 +1128,10 @@ calibrate(unsigned altitude) /* save as integer Pa */ p1 *= 1000.0f; + if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) err(1, "BAROIOCSMSLPRESSURE"); + exit(0); } @@ -1150,7 +1172,7 @@ ms5611_main(int argc, char *argv[]) errx(1, "missing altitude"); long altitude = strtol(argv[2], nullptr, 10); - + ms5611::calibrate(altitude); } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 66db1c360..995c9393f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -110,7 +110,7 @@ private: bool _primary_pwm_device; ///< true if we are the default PWM output volatile bool _switch_armed; ///< PX4IO switch armed state - // XXX how should this work? + // XXX how should this work? bool _send_needed; ///< If true, we need to send a packet to IO @@ -149,13 +149,13 @@ private: * group/index during mixing. */ static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); }; -namespace +namespace { PX4IO *g_dev; @@ -190,6 +190,7 @@ PX4IO::~PX4IO() /* spin waiting for the thread to stop */ unsigned i = 10; + do { /* wait 50ms - it should wake every 100ms or so worst-case */ usleep(50000); @@ -223,11 +224,13 @@ PX4IO::init() /* do regular cdev init */ ret = CDev::init(); + if (ret != OK) return ret; /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == OK) { log("default PWM output device"); _primary_pwm_device = true; @@ -235,6 +238,7 @@ PX4IO::init() /* start the IO interface task */ _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + if (_task < 0) { debug("task start failed: %d", errno); return -errno; @@ -256,6 +260,7 @@ PX4IO::task_main() /* open the serial port */ _serial_fd = ::open("/dev/ttyS2", O_RDWR); + if (_serial_fd < 0) { debug("failed to open serial port for IO: %d", errno); _task = -1; @@ -343,6 +348,7 @@ PX4IO::task_main() _send_needed = true; } } + if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls); @@ -364,9 +370,9 @@ PX4IO::task_main() int PX4IO::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -458,13 +464,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): + /* fake an update to the selected servo channel */ if ((arg >= 900) && (arg <= 2100)) { _outputs.output[cmd - PWM_SERVO_SET(0)] = arg; _send_needed = true; + } else { ret = -EINVAL; } + break; case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): @@ -481,6 +490,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) delete _mixers; _mixers = nullptr; } + break; case MIXERIOCADDSIMPLE: { @@ -519,6 +529,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* allocate a new mixer group and load it from the file */ newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (newmixers->load_from_file(path) != 0) { delete newmixers; ret = -EINVAL; @@ -528,6 +539,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) if (_mixers != nullptr) { delete _mixers; } + _mixers = newmixers; } @@ -537,6 +549,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* not a recognised value */ ret = -ENOTTY; } + unlock(); return ret; @@ -576,6 +589,7 @@ px4io_main(int argc, char *argv[]) if (argc > 2) { fn[0] = argv[2]; fn[1] = nullptr; + } else { fn[0] = "/fs/microsd/px4io.bin"; fn[1] = "/etc/px4io.bin"; @@ -589,18 +603,24 @@ px4io_main(int argc, char *argv[]) switch (ret) { case OK: break; + case -ENOENT: errx(1, "PX4IO firmware file not found"); + case -EEXIST: case -EIO: errx(1, "error updating PX4IO - check that bootloader mode is enabled"); + case -EINVAL: errx(1, "verify failed - retry the update"); + case -ETIMEDOUT: errx(1, "timed out waiting for bootloader - power-cycle and try again"); + default: errx(1, "unexpected error %d", ret); } + return ret; } diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp index 0fbbac839..5669aeb01 100644 --- a/apps/drivers/px4io/uploader.cpp +++ b/apps/drivers/px4io/uploader.cpp @@ -67,6 +67,7 @@ PX4IO_Uploader::upload(const char *filenames[]) int ret; _io_fd = open("/dev/ttyS2", O_RDWR); + if (_io_fd < 0) { log("could not open interface"); return -errno; @@ -74,6 +75,7 @@ PX4IO_Uploader::upload(const char *filenames[]) /* look for the bootloader */ ret = sync(); + if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); @@ -87,17 +89,20 @@ PX4IO_Uploader::upload(const char *filenames[]) log("failed to open %s", filenames[i]); continue; } + log("using firmware from %s", filenames[i]); break; } + if (_fw_fd == -1) return -ENOENT; /* do the usual program thing - allow for failure */ for (unsigned retries = 0; retries < 1; retries++) { if (retries > 0) { - log("retrying update..."); + log("retrying update..."); ret = sync(); + if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); @@ -106,25 +111,33 @@ PX4IO_Uploader::upload(const char *filenames[]) } ret = erase(); + if (ret != OK) { log("erase failed"); continue; } + ret = program(); + if (ret != OK) { log("program failed"); continue; } + ret = verify(); + if (ret != OK) { log("verify failed"); continue; } + ret = reboot(); + if (ret != OK) { log("reboot failed"); return ret; } + log("update complete"); ret = OK; @@ -145,6 +158,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) /* wait 100 ms for a character */ int ret = ::poll(&fds[0], 1, timeout); + if (ret < 1) { //log("poll timeout %d", ret); return -ETIMEDOUT; @@ -160,9 +174,11 @@ PX4IO_Uploader::recv(uint8_t *p, unsigned count) { while (count--) { int ret = recv(*p++); + if (ret != OK) return ret; } + return OK; } @@ -175,7 +191,7 @@ PX4IO_Uploader::drain() do { ret = recv(c, 10); //log("discard 0x%02x", c); - } while(ret == OK); + } while (ret == OK); } int @@ -184,6 +200,7 @@ PX4IO_Uploader::send(uint8_t c) //log("send 0x%02x", c); if (write(_io_fd, &c, 1) != 1) return -errno; + return OK; } @@ -192,9 +209,11 @@ PX4IO_Uploader::send(uint8_t *p, unsigned count) { while (count--) { int ret = send(*p++); + if (ret != OK) return ret; } + return OK; } @@ -205,15 +224,20 @@ PX4IO_Uploader::get_sync(unsigned timeout) int ret; ret = recv(c[0], timeout); + if (ret != OK) return ret; + ret = recv(c[1], timeout); + if (ret != OK) return ret; + if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { log("bad sync 0x%02x,0x%02x", c[0], c[1]); return -EIO; } + return OK; } @@ -221,9 +245,11 @@ int PX4IO_Uploader::sync() { drain(); + /* complete any pending program operation */ for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) send(0); + send(PROTO_GET_SYNC); send(PROTO_EOC); return get_sync(); @@ -239,8 +265,10 @@ PX4IO_Uploader::get_info(int param, uint32_t &val) send(PROTO_EOC); ret = recv((uint8_t *)&val, sizeof(val)); + if (ret != OK) return ret; + return get_sync(); } @@ -267,10 +295,13 @@ PX4IO_Uploader::program() /* get more bytes to program */ //log(" %d", (int)lseek(_fw_fd, 0, SEEK_CUR)); count = read(_fw_fd, file_buf, sizeof(file_buf)); + if (count == 0) return OK; + if (count < 0) return -errno; + ASSERT((count % 4) == 0); send(PROTO_PROG_MULTI); @@ -279,6 +310,7 @@ PX4IO_Uploader::program() send(PROTO_EOC); ret = get_sync(1000); + if (ret != OK) return ret; } @@ -297,6 +329,7 @@ PX4IO_Uploader::verify() send(PROTO_CHIP_VERIFY); send(PROTO_EOC); ret = get_sync(); + if (ret != OK) return ret; @@ -304,19 +337,24 @@ PX4IO_Uploader::verify() /* get more bytes to verify */ int base = (int)lseek(_fw_fd, 0, SEEK_CUR); count = read(_fw_fd, file_buf, sizeof(file_buf)); + if (count == 0) break; + if (count < 0) return -errno; + ASSERT((count % 4) == 0); send(PROTO_READ_MULTI); send(count); send(PROTO_EOC); + for (ssize_t i = 0; i < count; i++) { uint8_t c; ret = recv(c); + if (ret != OK) { log("%d: got %d waiting for bytes", base + i, ret); return ret; @@ -327,12 +365,15 @@ PX4IO_Uploader::verify() return -EINVAL; } } + ret = get_sync(); + if (ret != OK) { log("timeout waiting for post-verify sync"); return ret; } } + return OK; } @@ -358,6 +399,7 @@ PX4IO_Uploader::compare(bool &identical) send(PROTO_CHIP_VERIFY); send(PROTO_EOC); ret = get_sync(); + if (ret != OK) return ret; @@ -365,6 +407,7 @@ PX4IO_Uploader::compare(bool &identical) send(sizeof(fw_vectors)); send(PROTO_EOC); ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors)); + if (ret != OK) return ret; diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 11b98fd1b..1ac90b16d 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -30,7 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - + /** * @file drv_hrt.c * @@ -41,7 +41,7 @@ * Note that really, this could use systick too, but that's * monopolised by NuttX and stealing it would just be awkward. * - * We don't use the NuttX STM32 driver per se; rather, we + * We don't use the NuttX STM32 driver per se; rather, we * claim the timer and then drive it directly. */ @@ -264,10 +264,10 @@ static void hrt_latency_update(void); /* callout list manipulation */ static void hrt_call_internal(struct hrt_call *entry, - hrt_abstime deadline, - hrt_abstime interval, - hrt_callout callout, - void *arg); + hrt_abstime deadline, + hrt_abstime interval, + hrt_callout callout, + void *arg); static void hrt_call_enter(struct hrt_call *entry); static void hrt_call_reschedule(void); static void hrt_call_invoke(void); @@ -372,39 +372,39 @@ static void hrt_tim_init(void) { /* clock/power on our timer */ - modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); + modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); - /* claim our interrupt vector */ - irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); + /* claim our interrupt vector */ + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); - /* disable and configure the timer */ - rCR1 = 0; - rCR2 = 0; - rSMCR = 0; - rDIER = DIER_HRT | DIER_PPM; - rCCER = 0; /* unlock CCMR* registers */ - rCCMR1 = CCMR1_PPM; - rCCMR2 = CCMR2_PPM; - rCCER = CCER_PPM; - rDCR = 0; + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_HRT | DIER_PPM; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PPM; + rCCMR2 = CCMR2_PPM; + rCCER = CCER_PPM; + rDCR = 0; - /* configure the timer to free-run at 1MHz */ - rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ + /* configure the timer to free-run at 1MHz */ + rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ - /* run the full span of the counter */ - rARR = 0xffff; + /* run the full span of the counter */ + rARR = 0xffff; - /* set an initial capture a little ways off */ - rCCR_HRT = 1000; + /* set an initial capture a little ways off */ + rCCR_HRT = 1000; - /* generate an update event; reloads the counter, all registers */ - rEGR = GTIM_EGR_UG; + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; - /* enable the timer */ - rCR1 = GTIM_CR1_CEN; + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; - /* enable interrupts */ - up_enable_irq(HRT_TIMER_VECTOR); + /* enable interrupts */ + up_enable_irq(HRT_TIMER_VECTOR); } #ifdef CONFIG_HRT_PPM @@ -412,7 +412,7 @@ hrt_tim_init(void) * Handle the PPM decoder state machine. */ static void -hrt_ppm_decode(uint32_t status) +hrt_ppm_decode(uint32_t status) { uint16_t count = rCCR_PPM; uint16_t width; @@ -428,10 +428,11 @@ hrt_ppm_decode(uint32_t status) ppm.last_edge = count; ppm_edge_history[ppm_edge_next++] = width; + if (ppm_edge_next >= 32) ppm_edge_next = 0; - /* + /* * if this looks like a start pulse, then push the last set of values * and reset the state machine */ @@ -441,6 +442,7 @@ hrt_ppm_decode(uint32_t status) if (ppm.next_channel > 4) { for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++) ppm_buffer[i] = ppm_temp_buffer[i]; + ppm_decoded_channels = i; ppm_last_valid_decode = hrt_absolute_time(); @@ -461,10 +463,11 @@ hrt_ppm_decode(uint32_t status) return; case ARM: + /* we expect a pulse giving us the first mark */ if (width > PPM_MAX_PULSE_WIDTH) goto error; /* pulse was too long */ - + /* record the mark timing, expect an inactive edge */ ppm.last_mark = count; ppm.phase = INACTIVE; @@ -481,6 +484,7 @@ hrt_ppm_decode(uint32_t status) ppm.last_mark = count; ppm_pulse_history[ppm_pulse_next++] = interval; + if (ppm_pulse_next >= 32) ppm_pulse_next = 0; @@ -493,7 +497,7 @@ hrt_ppm_decode(uint32_t status) ppm_temp_buffer[ppm.next_channel++] = interval; ppm.phase = INACTIVE; - return; + return; } @@ -526,9 +530,11 @@ hrt_tim_isr(int irq, void *context) rSR = ~status; #ifdef CONFIG_HRT_PPM + /* was this a PPM edge? */ if (status & (SR_INT_PPM | SR_OVF_PPM)) hrt_ppm_decode(status); + #endif /* was this a timer tick? */ @@ -640,7 +646,7 @@ hrt_init(void) void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) { - hrt_call_internal(entry, + hrt_call_internal(entry, hrt_absolute_time() + delay, 0, callout, @@ -730,9 +736,11 @@ hrt_call_enter(struct hrt_call *entry) //lldbg("call enter at head, reschedule\n"); /* we changed the next deadline, reschedule the timer event */ hrt_call_reschedule(); + } else { do { next = (struct hrt_call *)sq_next(&call->link); + if ((next == NULL) || (entry->deadline < next->deadline)) { //lldbg("call enter after head\n"); sq_addafter(&call->link, &entry->link, &callout_queue); @@ -755,8 +763,10 @@ hrt_call_invoke(void) hrt_abstime now = hrt_absolute_time(); call = (struct hrt_call *)sq_peek(&callout_queue); + if (call == NULL) break; + if (call->deadline > now) break; @@ -764,7 +774,7 @@ hrt_call_invoke(void) //lldbg("call pop\n"); /* save the intended deadline for periodic calls */ - deadline = call->deadline; + deadline = call->deadline; /* zero the deadline, as the call has occurred */ call->deadline = 0; @@ -804,7 +814,7 @@ hrt_call_reschedule() * we want. * * It is important for accurate timekeeping that the compare - * interrupt fires sufficiently often that the base_time update in + * interrupt fires sufficiently often that the base_time update in * hrt_absolute_time runs at least once per timer period. */ if (next != NULL) { @@ -813,11 +823,13 @@ hrt_call_reschedule() //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ deadline = now + HRT_INTERVAL_MIN; + } else if (next->deadline < deadline) { //lldbg("due soon\n"); deadline = next->deadline; } } + //lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); /* set the new compare value and remember it for latency tracking */ @@ -837,6 +849,7 @@ hrt_latency_update(void) return; } } + /* catch-all at the end */ latency_counters[index]++; } diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index 708aa8884..be8934492 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -197,26 +197,29 @@ pwm_channel_init(unsigned channel) /* configure the channel */ switch (pwm_channels[channel].timer_channel) { - case 1: - rCCMR1(timer) |= (6 << 4); - rCCR1(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 0); - break; - case 2: - rCCMR1(timer) |= (6 << 12); - rCCR2(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 4); - break; - case 3: - rCCMR2(timer) |= (6 << 4); - rCCR3(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 8); - break; - case 4: - rCCMR2(timer) |= (6 << 12); - rCCR4(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 12); - break; + case 1: + rCCMR1(timer) |= (6 << 4); + rCCR1(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 0); + break; + + case 2: + rCCMR1(timer) |= (6 << 12); + rCCR2(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 4); + break; + + case 3: + rCCMR2(timer) |= (6 << 4); + rCCR3(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 8); + break; + + case 4: + rCCMR2(timer) |= (6 << 12); + rCCR4(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 12); + break; } } @@ -238,22 +241,28 @@ up_pwm_servo_set(unsigned channel, servo_position_t value) /* configure the channel */ if (value > 0) value--; + switch (pwm_channels[channel].timer_channel) { - case 1: - rCCR1(timer) = value; - break; - case 2: - rCCR2(timer) = value; - break; - case 3: - rCCR3(timer) = value; - break; - case 4: - rCCR4(timer) = value; - break; - default: - return -1; + case 1: + rCCR1(timer) = value; + break; + + case 2: + rCCR2(timer) = value; + break; + + case 3: + rCCR3(timer) = value; + break; + + case 4: + rCCR4(timer) = value; + break; + + default: + return -1; } + return 0; } @@ -275,19 +284,23 @@ up_pwm_servo_get(unsigned channel) /* configure the channel */ switch (pwm_channels[channel].timer_channel) { - case 1: - value = rCCR1(timer); - break; - case 2: - value = rCCR2(timer); - break; - case 3: - value = rCCR3(timer); - break; - case 4: - value = rCCR4(timer); - break; + case 1: + value = rCCR1(timer); + break; + + case 2: + value = rCCR2(timer); + break; + + case 3: + value = rCCR3(timer); + break; + + case 4: + value = rCCR4(timer); + break; } + return value; } @@ -303,9 +316,10 @@ up_pwm_servo_init(uint32_t channel_mask) /* now init channels */ for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { /* don't do init for disabled channels; this leaves the pin configs alone */ - if (((1< (_max_pattern_len * sizeof(tone_note))) return -EFBIG; + if ((len % sizeof(tone_note)) || (len == 0)) return -EIO; + if (!check((tone_note *)buffer)) return -EIO; @@ -479,6 +487,7 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len) debug("starting user pattern"); next(); } + irqrestore(flags); return len; @@ -511,18 +520,22 @@ ToneAlarm::next(void) /* find the note to play */ if (_current_pattern == _pattern_user) { np = &_user_pattern[_next_note]; + } else { np = &_patterns[_current_pattern - 1][_next_note]; } /* work out which note is next */ _next_note++; + if (_next_note >= _note_max) { /* hit the end of the pattern, stop */ _current_pattern = _pattern_none; + } else if (np[1].duration == DURATION_END) { /* hit the end of the pattern, stop */ _current_pattern = _pattern_none; + } else if (np[1].duration == DURATION_REPEAT) { /* next note is a repeat, rewind in preparation */ _next_note = 0; @@ -534,11 +547,11 @@ ToneAlarm::next(void) /* set reload based on the pitch */ rARR = _notes[np->pitch]; - /* force an update, reloads the counter and all registers */ - rEGR = GTIM_EGR_UG; + /* force an update, reloads the counter and all registers */ + rEGR = GTIM_EGR_UG; - /* enable the output */ - rCCER |= TONE_CCER; + /* enable the output */ + rCCER |= TONE_CCER; } /* arrange a callback when the note/rest is done */ @@ -554,6 +567,7 @@ ToneAlarm::check(tone_note *pattern) if ((i == 0) && ((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT))) return false; + if (pattern[i].duration == DURATION_END) break; @@ -561,6 +575,7 @@ ToneAlarm::check(tone_note *pattern) if (pattern[i].pitch >= _note_max) return false; } + return true; } @@ -592,13 +607,16 @@ play_pattern(unsigned pattern) int fd, ret; fd = open("/dev/tone_alarm", 0); + if (fd < 0) err(1, "/dev/tone_alarm"); warnx("playing pattern %u", pattern); ret = ioctl(fd, TONE_SET_ALARM, pattern); + if (ret != 0) err(1, "TONE_SET_ALARM"); + exit(0); } @@ -615,6 +633,7 @@ tone_alarm_main(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "couldn't allocate the ToneAlarm driver"); + if (g_dev->init() != OK) { delete g_dev; errx(1, "ToneAlarm init failed"); @@ -623,8 +642,10 @@ tone_alarm_main(int argc, char *argv[]) if ((argc > 1) && !strcmp(argv[1], "start")) play_pattern(1); + if ((argc > 1) && !strcmp(argv[1], "stop")) play_pattern(0); + if ((pattern = strtol(argv[1], nullptr, 10)) != 0) play_pattern(pattern); -- cgit v1.2.3