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 +++-- apps/systemcmds/bl_update/bl_update.c | 8 + apps/systemcmds/eeprom/24xxxx_mtd.c | 533 +++++++++++++-------------- apps/systemcmds/eeprom/eeprom.c | 11 +- apps/systemcmds/led/led.c | 7 +- apps/systemcmds/mixer/mixer.c | 1 + apps/systemcmds/param/param.c | 13 +- apps/systemcmds/top/top.c | 6 +- apps/systemlib/bson/tinybson.c | 24 +- apps/systemlib/bson/tinybson.h | 27 +- apps/systemlib/conversions.c | 14 +- apps/systemlib/cpuload.c | 88 +++-- apps/systemlib/err.c | 16 +- apps/systemlib/err.h | 38 +- apps/systemlib/geo/geo.c | 120 +++--- apps/systemlib/geo/geo.h | 14 +- apps/systemlib/mixer/mixer.cpp | 2 +- apps/systemlib/mixer/mixer_group.cpp | 20 +- apps/systemlib/mixer/mixer_multirotor.cpp | 6 +- apps/systemlib/param/param.c | 8 +- apps/systemlib/pid/pid.c | 28 +- apps/systemlib/ppm_decode.c | 24 +- apps/systemlib/ppm_decode.h | 2 +- apps/systemlib/systemlib.c | 12 +- 50 files changed, 1150 insertions(+), 848 deletions(-) (limited to 'apps') 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); diff --git a/apps/systemcmds/bl_update/bl_update.c b/apps/systemcmds/bl_update/bl_update.c index 752c01986..745f76852 100644 --- a/apps/systemcmds/bl_update/bl_update.c +++ b/apps/systemcmds/bl_update/bl_update.c @@ -67,10 +67,12 @@ bl_update_main(int argc, char *argv[]) setopt(); int fd = open(argv[1], O_RDONLY); + if (fd < 0) err(1, "open %s", argv[1]); struct stat s; + if (stat(argv[1], &s) < 0) err(1, "stat %s", argv[1]); @@ -79,14 +81,17 @@ bl_update_main(int argc, char *argv[]) errx(1, "%s: file too large", argv[1]); uint8_t *buf = malloc(s.st_size); + if (buf == NULL) errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size); if (read(fd, buf, s.st_size) != s.st_size) err(1, "firmware read error"); + close(fd); uint32_t *hdr = (uint32_t *)buf; + if ((hdr[0] < 0x20000000) || /* stack not below RAM */ (hdr[0] > (0x20000000 + (128 * 1024))) || /* stack not above RAM */ (hdr[1] < 0x08000000) || /* entrypoint not below flash */ @@ -123,6 +128,7 @@ bl_update_main(int argc, char *argv[]) /* wait for the operation to complete */ while (*sr & 0x1000) { } + if (*sr & 0xf2) { warnx("WARNING: erase error 0x%02x", *sr); goto flash_end; @@ -148,6 +154,7 @@ bl_update_main(int argc, char *argv[]) /* wait for the operation to complete */ while (*sr & 0x1000) { } + if (*sr & 0xf2) { warnx("WARNING: program error 0x%02x", *sr); goto flash_end; @@ -203,6 +210,7 @@ setopt(void) if ((*optcr & opt_mask) == opt_bits) errx(0, "option bits set"); + errx(1, "option bits setting failed; readback 0x%04x", *optcr); } \ No newline at end of file diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c index 3cded52fa..79149caa0 100644 --- a/apps/systemcmds/eeprom/24xxxx_mtd.c +++ b/apps/systemcmds/eeprom/24xxxx_mtd.c @@ -135,20 +135,19 @@ * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. */ -struct at24c_dev_s -{ - struct mtd_dev_s mtd; /* MTD interface */ - FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ - uint8_t addr; /* I2C address */ - uint16_t pagesize; /* 32, 63 */ - uint16_t npages; /* 128, 256, 512, 1024 */ - - perf_counter_t perf_reads; - perf_counter_t perf_writes; - perf_counter_t perf_resets; - perf_counter_t perf_read_retries; - perf_counter_t perf_read_errors; - perf_counter_t perf_write_errors; +struct at24c_dev_s { + struct mtd_dev_s mtd; /* MTD interface */ + FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ + uint8_t addr; /* I2C address */ + uint16_t pagesize; /* 32, 63 */ + uint16_t npages; /* 128, 256, 512, 1024 */ + + perf_counter_t perf_reads; + perf_counter_t perf_writes; + perf_counter_t perf_resets; + perf_counter_t perf_read_retries; + perf_counter_t perf_read_errors; + perf_counter_t perf_write_errors; }; /************************************************************************************ @@ -159,9 +158,9 @@ struct at24c_dev_s static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buf); + size_t nblocks, FAR uint8_t *buf); static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR const uint8_t *buf); + size_t nblocks, FAR const uint8_t *buf); static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); /************************************************************************************ @@ -180,35 +179,32 @@ static struct at24c_dev_s g_at24c; static int at24c_eraseall(FAR struct at24c_dev_s *priv) { - int startblock = 0; - uint8_t buf[AT24XX_PAGESIZE + 2]; - - struct i2c_msg_s msgv[1] = - { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - memset(&buf[2],0xff,priv->pagesize); - - for (startblock = 0; startblock < priv->npages; startblock++) - { - uint16_t offset = startblock * priv->pagesize; - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - - while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) - { - fvdbg("erase stall\n"); - usleep(10000); - } - } - - return OK; + int startblock = 0; + uint8_t buf[AT24XX_PAGESIZE + 2]; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + memset(&buf[2], 0xff, priv->pagesize); + + for (startblock = 0; startblock < priv->npages; startblock++) { + uint16_t offset = startblock * priv->pagesize; + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + + while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { + fvdbg("erase stall\n"); + usleep(10000); + } + } + + return OK; } /************************************************************************************ @@ -217,9 +213,9 @@ static int at24c_eraseall(FAR struct at24c_dev_s *priv) static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) { - /* EEprom need not erase */ + /* EEprom need not erase */ - return (int)nblocks; + return (int)nblocks; } /************************************************************************************ @@ -227,90 +223,86 @@ static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nbloc ************************************************************************************/ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buffer) + size_t nblocks, FAR uint8_t *buffer) { - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t addr[2]; - int ret; - - struct i2c_msg_s msgv[2] = - { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addr[0], - .length = sizeof(addr), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = 0, - .length = priv->pagesize, - } - }; + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t addr[2]; + int ret; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addr[0], + .length = sizeof(addr), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = 0, + .length = priv->pagesize, + } + }; #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #endif - blocksleft = nblocks; - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - if (startblock >= priv->npages) - { - return 0; - } - - if (startblock + nblocks > priv->npages) - { - nblocks = priv->npages - startblock; - } - - while (blocksleft-- > 0) - { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - addr[1] = offset & 0xff; - addr[0] = (offset >> 8) & 0xff; - msgv[1].buffer = buffer; - - for (;;) - { - - perf_begin(priv->perf_reads); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - if (ret >= 0) - break; - - fvdbg("read stall"); - usleep(1000); - - /* We should normally only be here on the first read after - * a write. - * - * XXX maybe do special first-read handling with optional - * bus reset as well? - */ - perf_count(priv->perf_read_retries); - if (--tries == 0) - { - perf_count(priv->perf_read_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } + blocksleft = nblocks; + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + addr[1] = offset & 0xff; + addr[0] = (offset >> 8) & 0xff; + msgv[1].buffer = buffer; + + for (;;) { + + perf_begin(priv->perf_reads); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret >= 0) + break; + + fvdbg("read stall"); + usleep(1000); + + /* We should normally only be here on the first read after + * a write. + * + * XXX maybe do special first-read handling with optional + * bus reset as well? + */ + perf_count(priv->perf_read_retries); + + if (--tries == 0) { + perf_count(priv->perf_read_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #else - return nblocks; + return nblocks; #endif } @@ -322,81 +314,75 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, ************************************************************************************/ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, - FAR const uint8_t *buffer) + FAR const uint8_t *buffer) { - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t buf[AT24XX_PAGESIZE+2]; - int ret; - - struct i2c_msg_s msgv[1] = - { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t buf[AT24XX_PAGESIZE + 2]; + int ret; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #endif - blocksleft = nblocks; + blocksleft = nblocks; - if (startblock >= priv->npages) - { - return 0; - } + if (startblock >= priv->npages) { + return 0; + } - if (startblock + nblocks > priv->npages) - { - nblocks = priv->npages - startblock; - } + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - while (blocksleft-- > 0) - { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - memcpy(&buf[2], buffer, priv->pagesize); + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + memcpy(&buf[2], buffer, priv->pagesize); - for (;;) - { + for (;;) { - perf_begin(priv->perf_writes); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); - perf_end(priv->perf_writes); + perf_begin(priv->perf_writes); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); + perf_end(priv->perf_writes); - if (ret >= 0) - break; + if (ret >= 0) + break; - fvdbg("write stall"); - usleep(1000); + fvdbg("write stall"); + usleep(1000); - /* We expect to see a number of retries per write cycle as we - * poll for write completion. - */ - if (--tries == 0) - { - perf_count(priv->perf_write_errors); - return ERROR; - } - } + /* We expect to see a number of retries per write cycle as we + * poll for write completion. + */ + if (--tries == 0) { + perf_count(priv->perf_write_errors); + return ERROR; + } + } - startblock++; - buffer += priv->pagesize; - } + startblock++; + buffer += priv->pagesize; + } #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #else - return nblocks; + return nblocks; #endif } @@ -406,67 +392,65 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) { - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - int ret = -EINVAL; /* Assume good command with bad parameters */ - - fvdbg("cmd: %d \n", cmd); - - switch (cmd) - { - case MTDIOC_GEOMETRY: - { - FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); - if (geo) - { - /* Populate the geometry structure with information need to know - * the capacity and how to access the device. - * - * NOTE: that the device is treated as though it where just an array - * of fixed size blocks. That is most likely not true, but the client - * will expect the device logic to do whatever is necessary to make it - * appear so. - * - * blocksize: - * May be user defined. The block size for the at24XX devices may be - * larger than the page size in order to better support file systems. - * The read and write functions translate BLOCKS to pages for the - * small flash devices - * erasesize: - * It has to be at least as big as the blocksize, bigger serves no - * purpose. - * neraseblocks - * Note that the device size is in kilobits and must be scaled by - * 1024 / 8 - */ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + fvdbg("cmd: %d \n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to know + * the capacity and how to access the device. + * + * NOTE: that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the client + * will expect the device logic to do whatever is necessary to make it + * appear so. + * + * blocksize: + * May be user defined. The block size for the at24XX devices may be + * larger than the page size in order to better support file systems. + * The read and write functions translate BLOCKS to pages for the + * small flash devices + * erasesize: + * It has to be at least as big as the blocksize, bigger serves no + * purpose. + * neraseblocks + * Note that the device size is in kilobits and must be scaled by + * 1024 / 8 + */ #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; #else - geo->blocksize = priv->pagesize; - geo->erasesize = priv->pagesize; - geo->neraseblocks = priv->npages; + geo->blocksize = priv->pagesize; + geo->erasesize = priv->pagesize; + geo->neraseblocks = priv->npages; #endif - ret = OK; + ret = OK; - fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", - geo->blocksize, geo->erasesize, geo->neraseblocks); - } - } - break; + fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; - case MTDIOC_BULKERASE: - ret=at24c_eraseall(priv); - break; + case MTDIOC_BULKERASE: + ret = at24c_eraseall(priv); + break; - case MTDIOC_XIPBASE: - default: - ret = -ENOTTY; /* Bad command */ - break; - } + case MTDIOC_XIPBASE: + default: + ret = -ENOTTY; /* Bad command */ + break; + } - return ret; + return ret; } /************************************************************************************ @@ -483,46 +467,45 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) * ************************************************************************************/ -FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) -{ - FAR struct at24c_dev_s *priv; - - fvdbg("dev: %p\n", dev); - - /* Allocate a state structure (we allocate the structure instead of using - * a fixed, static allocation so that we can handle multiple FLASH devices. - * The current implementation would handle only one FLASH part per I2C - * device (only because of the SPIDEV_FLASH definition) and so would have - * to be extended to handle multiple FLASH parts on the same I2C bus. - */ - - priv = &g_at24c; - if (priv) - { - /* Initialize the allocated structure */ - - priv->addr = CONFIG_AT24XX_ADDR; - priv->pagesize = AT24XX_PAGESIZE; - priv->npages = AT24XX_NPAGES; - - priv->mtd.erase = at24c_erase; - priv->mtd.bread = at24c_bread; - priv->mtd.bwrite = at24c_bwrite; - priv->mtd.ioctl = at24c_ioctl; - priv->dev = dev; - - priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); - priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); - priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); - priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); - priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); - priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); - } - - /* Return the implementation-specific state structure as the MTD device */ - - fvdbg("Return %p\n", priv); - return (FAR struct mtd_dev_s *)priv; +FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { + FAR struct at24c_dev_s *priv; + + fvdbg("dev: %p\n", dev); + + /* Allocate a state structure (we allocate the structure instead of using + * a fixed, static allocation so that we can handle multiple FLASH devices. + * The current implementation would handle only one FLASH part per I2C + * device (only because of the SPIDEV_FLASH definition) and so would have + * to be extended to handle multiple FLASH parts on the same I2C bus. + */ + + priv = &g_at24c; + + if (priv) { + /* Initialize the allocated structure */ + + priv->addr = CONFIG_AT24XX_ADDR; + priv->pagesize = AT24XX_PAGESIZE; + priv->npages = AT24XX_NPAGES; + + priv->mtd.erase = at24c_erase; + priv->mtd.bread = at24c_bread; + priv->mtd.bwrite = at24c_bwrite; + priv->mtd.ioctl = at24c_ioctl; + priv->dev = dev; + + priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); + priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); + priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); + priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); + priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); + priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); + } + + /* Return the implementation-specific state structure as the MTD device */ + + fvdbg("Return %p\n", priv); + return (FAR struct mtd_dev_s *)priv; } /* @@ -530,5 +513,5 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) */ int at24c_nuke(void) { - return at24c_eraseall(&g_at24c); + return at24c_eraseall(&g_at24c); } diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c index fa88fa09e..19a14aa02 100644 --- a/apps/systemcmds/eeprom/eeprom.c +++ b/apps/systemcmds/eeprom/eeprom.c @@ -143,13 +143,13 @@ eeprom_start(void) if (ret < 0) errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); - + /* mount the EEPROM */ ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); if (ret < 0) errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); - + started = true; warnx("mounted EEPROM at /eeprom"); exit(0); @@ -165,6 +165,7 @@ eeprom_erase(void) if (at24c_nuke()) errx(1, "erase failed"); + errx(0, "erase done, reboot now"); } @@ -190,7 +191,7 @@ eeprom_save(const char *name) if (!started) errx(1, "must be started first"); - if (!name) + if (!name) err(1, "missing argument for device name, try '/eeprom/parameters'"); warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); @@ -221,9 +222,9 @@ eeprom_load(const char *name) if (!started) errx(1, "must be started first"); - if (!name) + if (!name) err(1, "missing argument for device name, try '/eeprom/parameters'"); - + warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); int fd = open(name, O_RDONLY); diff --git a/apps/systemcmds/led/led.c b/apps/systemcmds/led/led.c index 7a1faa618..15d448118 100644 --- a/apps/systemcmds/led/led.c +++ b/apps/systemcmds/led/led.c @@ -121,6 +121,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: led {start|stop|status} [-d ]\n\n"); exit(1); } @@ -129,7 +130,7 @@ usage(const char *reason) * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_create(). */ @@ -165,9 +166,11 @@ int led_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tled is running\n"); + } else { printf("\tled not started\n"); } + exit(0); } @@ -187,7 +190,7 @@ int led_thread_main(int argc, char *argv[]) while (!thread_should_exit) { /* swell blue led */ - + /* 200 Hz base loop */ usleep(1000000 / rate); diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index 9d52557e7..3f52bdbf1 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -97,6 +97,7 @@ load(const char *devname, const char *fname) /* tell it to load the file */ ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname); + if (ret != 0) { fprintf(stderr, "failed loading %s\n", fname); } diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index d70d15da4..68dbd822e 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -70,10 +70,13 @@ param_main(int argc, char *argv[]) if (argc >= 2) { if (!strcmp(argv[1], "save")) do_save(); + if (!strcmp(argv[1], "load")) do_load(); + if (!strcmp(argv[1], "import")) do_import(); + if (!strcmp(argv[1], "show")) do_show(); } @@ -154,8 +157,8 @@ do_show_print(void *arg, param_t param) float f; printf("%c %s: ", - param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), - param_name(param)); + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); /* * This case can be expanded to handle printing common structure types. @@ -167,19 +170,25 @@ do_show_print(void *arg, param_t param) printf("%d\n", i); return; } + break; + case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { printf("%4.4f\n", (double)f); return; } + break; + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: printf("\n", 0 + param_type(param), param_size(param)); return; + default: printf("\n", 0 + param_type(param)); return; } + printf("\n", param); } diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c index 7a2a993c2..59d2bc8f1 100644 --- a/apps/systemcmds/top/top.c +++ b/apps/systemcmds/top/top.c @@ -190,13 +190,15 @@ int top_main(int argc, char *argv[]) runtime_spaces = ""; } - unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; unsigned stack_free = 0; uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + while (stack_free < stack_size) { if (*stack_sweeper++ != 0xff) break; + stack_free++; } diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c index 10b736fd6..75578d2ec 100644 --- a/apps/systemlib/bson/tinybson.c +++ b/apps/systemlib/bson/tinybson.c @@ -56,7 +56,7 @@ static int read_int8(bson_decoder_t decoder, int8_t *b) { - return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1; + return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1; } static int @@ -119,12 +119,14 @@ bson_decoder_next(bson_decoder_t decoder) while (decoder->pending > 0) { if (read_int8(decoder, &tbyte)) CODER_KILL(decoder, "read error discarding pending bytes"); + decoder->pending--; } /* get the type byte */ if (read_int8(decoder, &tbyte)) CODER_KILL(decoder, "read error on type byte"); + decoder->node.type = tbyte; decoder->pending = 0; @@ -135,13 +137,17 @@ bson_decoder_next(bson_decoder_t decoder) /* get the node name */ nlen = 0; + for (;;) { if (nlen >= BSON_MAXNAME) CODER_KILL(decoder, "node name overflow"); + if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) CODER_KILL(decoder, "read error on node name"); + if (decoder->node.name[nlen] == '\0') break; + nlen++; } @@ -151,20 +157,28 @@ bson_decoder_next(bson_decoder_t decoder) case BSON_INT: if (read_int32(decoder, &decoder->node.i)) CODER_KILL(decoder, "read error on BSON_INT"); + break; + case BSON_DOUBLE: if (read_double(decoder, &decoder->node.d)) CODER_KILL(decoder, "read error on BSON_DOUBLE"); + break; + case BSON_STRING: if (read_int32(decoder, &decoder->pending)) CODER_KILL(decoder, "read error on BSON_STRING length"); + break; + case BSON_BINDATA: if (read_int32(decoder, &decoder->pending)) CODER_KILL(decoder, "read error on BSON_BINDATA size"); + if (read_int8(decoder, &tbyte)) CODER_KILL(decoder, "read error on BSON_BINDATA subtype"); + decoder->node.subtype = tbyte; break; @@ -186,11 +200,12 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf) CODER_CHECK(decoder); /* if data already copied, return zero bytes */ - if (decoder->pending == 0) + if (decoder->pending == 0) return 0; /* copy bytes per the node size */ result = read(decoder->fd, buf, decoder->pending); + if (result != decoder->pending) CODER_KILL(decoder, "read error on copy_data"); @@ -209,7 +224,7 @@ static int write_int8(bson_encoder_t encoder, int8_t b) { debug("write_int8 %d", b); - return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1; + return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1; } static int @@ -233,6 +248,7 @@ write_name(bson_encoder_t encoder, const char *name) if (len > BSON_MAXNAME) return -1; + debug("write name '%s' len %d", name, len); return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -1; } @@ -300,6 +316,7 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char write_int32(encoder, len) || write(encoder->fd, name, len + 1) != (int)(len + 1)) CODER_KILL(encoder, "write error on BSON_STRING"); + return 0; } @@ -314,5 +331,6 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary write_int8(encoder, subtype) || write(encoder->fd, data, size) != (int)(size)) CODER_KILL(encoder, "write error on BSON_BINDATA"); + return 0; } diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h index 1b9de5cd3..b6229dc50 100644 --- a/apps/systemlib/bson/tinybson.h +++ b/apps/systemlib/bson/tinybson.h @@ -31,14 +31,14 @@ * ****************************************************************************/ - /** - * @file tinybson.h - * - * A simple subset SAX-style BSON parser and generator. See http://bsonspec.org - * - * Some types and defines taken from the standalone BSON parser/generator - * in the Mongo C connector. - */ +/** +* @file tinybson.h +* +* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org +* +* Some types and defines taken from the standalone BSON parser/generator +* in the Mongo C connector. +*/ #ifndef _TINYBSON_H #define _TINYBSON_H @@ -77,8 +77,7 @@ typedef enum bson_binary_subtype { /** * Node structure passed to the callback. */ -typedef struct bson_node_s -{ +typedef struct bson_node_s { char name[BSON_MAXNAME]; bson_type_t type; bson_binary_subtype_t subtype; @@ -96,8 +95,7 @@ typedef struct bson_decoder_s *bson_decoder_t; */ typedef int (* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node); -struct bson_decoder_s -{ +struct bson_decoder_s { int fd; bson_decoder_callback callback; void *private; @@ -143,8 +141,7 @@ __EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder); /** * Encoder state structure. */ -typedef struct bson_encoder_s -{ +typedef struct bson_encoder_s { int fd; } *bson_encoder_t; @@ -169,7 +166,7 @@ __EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, i */ __EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value); -/** +/** * Append a string to the encoded stream. */ __EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string); diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 99ee41508..9105d83cb 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -45,13 +45,13 @@ int16_t int16_t_from_bytes(uint8_t bytes[]) { - union { - uint8_t b[2]; - int16_t w; - } u; + union { + uint8_t b[2]; + int16_t w; + } u; - u.b[1] = bytes[0]; - u.b[0] = bytes[1]; + u.b[1] = bytes[0]; + u.b[0] = bytes[1]; - return u.w; + return u.w; } diff --git a/apps/systemlib/cpuload.c b/apps/systemlib/cpuload.c index 9e6df3fac..20b711fa6 100644 --- a/apps/systemlib/cpuload.c +++ b/apps/systemlib/cpuload.c @@ -79,43 +79,43 @@ void cpuload_initialize_once() { // if (!system_load.initialized) // { - system_load.start_time = hrt_absolute_time(); - int i; - for (i = 0; i < CONFIG_MAX_TASKS; i++) - { - system_load.tasks[i].valid = false; - } - system_load.total_count = 0; - - uint64_t now = hrt_absolute_time(); - - /* initialize idle thread statically */ - system_load.tasks[0].start_time = now; - system_load.tasks[0].total_runtime = 0; - system_load.tasks[0].curr_start_time = 0; - system_load.tasks[0].tcb = sched_gettcb(0); - system_load.tasks[0].valid = true; - system_load.total_count++; - - /* initialize init thread statically */ - system_load.tasks[1].start_time = now; - system_load.tasks[1].total_runtime = 0; - system_load.tasks[1].curr_start_time = 0; - system_load.tasks[1].tcb = sched_gettcb(1); - system_load.tasks[1].valid = true; - /* count init thread */ - system_load.total_count++; - // } + system_load.start_time = hrt_absolute_time(); + int i; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + system_load.tasks[i].valid = false; + } + + system_load.total_count = 0; + + uint64_t now = hrt_absolute_time(); + + /* initialize idle thread statically */ + system_load.tasks[0].start_time = now; + system_load.tasks[0].total_runtime = 0; + system_load.tasks[0].curr_start_time = 0; + system_load.tasks[0].tcb = sched_gettcb(0); + system_load.tasks[0].valid = true; + system_load.total_count++; + + /* initialize init thread statically */ + system_load.tasks[1].start_time = now; + system_load.tasks[1].total_runtime = 0; + system_load.tasks[1].curr_start_time = 0; + system_load.tasks[1].tcb = sched_gettcb(1); + system_load.tasks[1].valid = true; + /* count init thread */ + system_load.total_count++; + // } } -void sched_note_start(FAR _TCB *tcb ) +void sched_note_start(FAR _TCB *tcb) { /* search first free slot */ int i; - for (i = 1; i < CONFIG_MAX_TASKS; i++) - { - if (!system_load.tasks[i].valid) - { + + for (i = 1; i < CONFIG_MAX_TASKS; i++) { + if (!system_load.tasks[i].valid) { /* slot is available */ system_load.tasks[i].start_time = hrt_absolute_time(); system_load.tasks[i].total_runtime = 0; @@ -128,13 +128,12 @@ void sched_note_start(FAR _TCB *tcb ) } } -void sched_note_stop(FAR _TCB *tcb ) +void sched_note_stop(FAR _TCB *tcb) { int i; - for (i = 1; i < CONFIG_MAX_TASKS; i++) - { - if (system_load.tasks[i].tcb->pid == tcb->pid) - { + + for (i = 1; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].tcb->pid == tcb->pid) { /* mark slot as fee */ system_load.tasks[i].valid = false; system_load.tasks[i].total_runtime = 0; @@ -152,26 +151,23 @@ void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb) /* Kind of inefficient: find both tasks and update times */ uint8_t both_found = 0; - for (int i = 0; i < CONFIG_MAX_TASKS; i++) - { + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { /* Task ending its current scheduling run */ - if (system_load.tasks[i].tcb->pid == pFromTcb->pid) - { + if (system_load.tasks[i].tcb->pid == pFromTcb->pid) { //if (system_load.tasks[i].curr_start_time != 0) { system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time; } both_found++; - } - else if (system_load.tasks[i].tcb->pid == pToTcb->pid) - { + + } else if (system_load.tasks[i].tcb->pid == pToTcb->pid) { system_load.tasks[i].curr_start_time = new_time; both_found++; } /* Do only iterate as long as needed */ - if (both_found == 2) - { + if (both_found == 2) { break; } } diff --git a/apps/systemlib/err.c b/apps/systemlib/err.c index f22c5ed0d..3011743a1 100644 --- a/apps/systemlib/err.c +++ b/apps/systemlib/err.c @@ -79,8 +79,10 @@ warnerr_core(int errcode, const char *fmt, va_list args) /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) errcode = -errcode; + if (errcode < NOCODE) fprintf(stderr, ": %s", strerror(errcode)); + fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC lib_lowprintf("%s: ", getprogname()); @@ -89,8 +91,10 @@ warnerr_core(int errcode, const char *fmt, va_list args) /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) errcode = -errcode; + if (errcode < NOCODE) lib_lowprintf(": %s", strerror(errcode)); + lib_lowprintf("\n"); #endif } @@ -144,7 +148,7 @@ verrx(int exitcode, const char *fmt, va_list args) } void -warn(const char *fmt, ...) +warn(const char *fmt, ...) { va_list args; @@ -153,13 +157,13 @@ warn(const char *fmt, ...) } void -vwarn(const char *fmt, va_list args) +vwarn(const char *fmt, va_list args) { warnerr_core(errno, fmt, args); } void -warnc(int errcode, const char *fmt, ...) +warnc(int errcode, const char *fmt, ...) { va_list args; @@ -168,13 +172,13 @@ warnc(int errcode, const char *fmt, ...) } void -vwarnc(int errcode, const char *fmt, va_list args) +vwarnc(int errcode, const char *fmt, va_list args) { warnerr_core(errcode, fmt, args); } void -warnx(const char *fmt, ...) +warnx(const char *fmt, ...) { va_list args; @@ -183,7 +187,7 @@ warnx(const char *fmt, ...) } void -vwarnx(const char *fmt, va_list args) +vwarnx(const char *fmt, va_list args) { warnerr_core(NOCODE, fmt, args); } diff --git a/apps/systemlib/err.h b/apps/systemlib/err.h index f798b97e7..ca13d6265 100644 --- a/apps/systemlib/err.h +++ b/apps/systemlib/err.h @@ -36,30 +36,30 @@ * * Simple error/warning functions, heavily inspired by the BSD functions of * the same names. - * + * * The err() and warn() family of functions display a formatted error * message on the standard error output. In all cases, the last * component of the program name, a colon character, and a space are * output. If the fmt argument is not NULL, the printf(3)-like formatted * error message is output. The output is terminated by a newline * character. - * + * * The err(), errc(), verr(), verrc(), warn(), warnc(), vwarn(), and * vwarnc() functions append an error message obtained from strerror(3) * based on a supplied error code value or the global variable errno, * preceded by another colon and space unless the fmt argument is NULL. - * + * * In the case of the errc(), verrc(), warnc(), and vwarnc() functions, * the code argument is used to look up the error message. - * + * * The err(), verr(), warn(), and vwarn() functions use the global * variable errno to look up the error message. - * + * * The errx() and warnx() functions do not append an error message. - * + * * The err(), verr(), errc(), verrc(), errx(), and verrx() functions do * not return, but exit with the value of the argument eval. - * + * */ #ifndef _SYSTEMLIB_ERR_H @@ -71,18 +71,18 @@ __BEGIN_DECLS __EXPORT const char *getprogname(void); -__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn,format(printf,2, 3))); -__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn,format(printf,2, 0))); -__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn,format(printf,3, 4))); -__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn,format(printf,3, 0))); -__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn,format(printf,2, 3))); -__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn,format(printf,2, 0))); -__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf,1, 2))); -__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf,1, 0))); -__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf,2, 3))); -__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf,2, 0))); -__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf,1, 2))); -__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf,1, 0))); +__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); +__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); +__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn, format(printf, 3, 4))); +__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn, format(printf, 3, 0))); +__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); +__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); +__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf, 1, 2))); +__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); +__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf, 2, 3))); +__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf, 2, 0))); +__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf, 1, 2))); +__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); __END_DECLS diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index bc467bfa3..789368fbd 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -89,7 +89,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) { // This function returns the distance to the nearest point on the track line. Distance is positive if current -// position is right of the track and negative if left of the track as seen from a point on the track line +// position is right of the track and negative if left of the track as seen from a point on the track line // headed towards the end point. crosstrack_error_s return_var; @@ -97,43 +97,46 @@ __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, float bearing_end; float bearing_track; float bearing_diff; - + return_var.error = true; // Set error flag, cleared when valid result calculated. return_var.past_end = false; return_var.distance = 0.0f; return_var.bearing = 0.0f; - + // Return error if arguments are bad - if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var; - + if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var; + bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); bearing_diff = bearing_track - bearing_end; bearing_diff = _wrapPI(bearing_diff); - + // Return past_end = true if past end point of line - if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { + if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { return_var.past_end = true; return_var.error = false; return return_var; } - + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - return_var.distance = (dist_to_end)*sin(bearing_diff); - if(sin(bearing_diff) >=0 ) { + return_var.distance = (dist_to_end) * sin(bearing_diff); + + if (sin(bearing_diff) >= 0) { return_var.bearing = _wrapPI(bearing_track - M_PI_2_F); + } else { return_var.bearing = _wrapPI(bearing_track + M_PI_2_F); } + return_var.error = false; - + return return_var; } -__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep) +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and @@ -146,68 +149,76 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, float bearing_sector_end; float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); bool in_sector; - + return_var.error = true; // Set error flag, cleared when valid result calculated. return_var.past_end = false; return_var.distance = 0.0f; return_var.bearing = 0.0f; - + // Return error if arguments are bad - if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var; - - - if(arc_sweep >= 0) { + if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var; + + + if (arc_sweep >= 0) { bearing_sector_start = arc_start_bearing; bearing_sector_end = arc_start_bearing + arc_sweep; - if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F; + + if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; + } else { bearing_sector_end = arc_start_bearing; bearing_sector_start = arc_start_bearing - arc_sweep; - if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F; + + if (bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F; } + in_sector = false; - + // Case where sector does not span zero - if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; - + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + // Case where sector does span zero - if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true; - + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; + // If in the sector then calculate distance and bearing to closest point - if(in_sector) { + if (in_sector) { return_var.past_end = false; float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); - - if(dist_to_center <= radius) { + + if (dist_to_center <= radius) { return_var.distance = radius - dist_to_center; return_var.bearing = bearing_now + M_PI_F; + } else { return_var.distance = dist_to_center - radius; return_var.bearing = bearing_now; } - - // If out of the sector then calculate dist and bearing to start or end point + + // If out of the sector then calculate dist and bearing to start or end point + } else { - - // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) - // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to - // calculate the position of the start and end points. We should not be doing this often - // as this function generally will not be called repeatedly when we are out of the sector. - - // TO DO - this is messed up and won't compile + + // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) + // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to + // calculate the position of the start and end points. We should not be doing this often + // as this function generally will not be called repeatedly when we are out of the sector. + + // TO DO - this is messed up and won't compile float start_disp_x = radius * sin(arc_start_bearing); float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep)); - float lon_start = lon_now + start_disp_x/111111.0d; - float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d; - float lon_end = lon_now + end_disp_x/111111.0d; - float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d; + float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); + float lon_start = lon_now + start_disp_x / 111111.0d; + float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; + float lon_end = lon_now + end_disp_x / 111111.0d; + float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - if(dist_to_start < dist_to_end) { + + if (dist_to_start < dist_to_end) { return_var.distance = dist_to_start; return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + } else { return_var.past_end = true; return_var.distance = dist_to_end; @@ -215,10 +226,10 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, } } - + return_var.bearing = _wrapPI(return_var.bearing); return_var.error = false; - return return_var; + return return_var; } float _wrapPI(float bearing) @@ -227,21 +238,25 @@ float _wrapPI(float bearing) while (bearing > M_PI_F) { bearing = bearing - M_TWOPI_F; } + while (bearing <= -M_PI_F) { bearing = bearing + M_TWOPI_F; } + return bearing; } - + float _wrap2PI(float bearing) { while (bearing >= M_TWOPI_F) { bearing = bearing - M_TWOPI_F; } + while (bearing < 0.0f) { bearing = bearing + M_TWOPI_F; } + return bearing; } @@ -251,23 +266,26 @@ float _wrap180(float bearing) while (bearing > 180.0f) { bearing = bearing - 360.0f; } + while (bearing <= -180.0f) { bearing = bearing + 360.0f; } + return bearing; } - + float _wrap360(float bearing) { while (bearing >= 360.0f) { bearing = bearing - 360.0f; } + while (bearing < 0.0f) { bearing = bearing + 360.0f; } + return bearing; } - - \ No newline at end of file + diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h index 205afc79e..c98b4c85d 100644 --- a/apps/systemlib/geo/geo.h +++ b/apps/systemlib/geo/geo.h @@ -44,8 +44,8 @@ * @author Lorenz Meier * Additional functions - @author Doug Weibel */ - - + + #include typedef struct { @@ -59,14 +59,14 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -// +// __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); -__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep); - +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); + float _wrap180(float bearing); -float _wrap360(float bearing); +float _wrap360(float bearing); float _wrapPI(float bearing); float _wrap2PI(float bearing); \ No newline at end of file diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index b56226c03..6c1bbe469 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -63,7 +63,7 @@ float Mixer::get_control(uint8_t group, uint8_t index) { float value; - + _control_cb(_cb_handle, group, index, value); return value; diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 44a33bf8b..004151235 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -224,29 +224,35 @@ mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, co if (!strcmp(geomname, "4+")) { geometry = MultirotorMixer::QUAD_PLUS; + } else if (!strcmp(geomname, "4x")) { geometry = MultirotorMixer::QUAD_X; + } else if (!strcmp(geomname, "6+")) { geometry = MultirotorMixer::HEX_PLUS; + } else if (!strcmp(geomname, "6x")) { geometry = MultirotorMixer::HEX_X; + } else if (!strcmp(geomname, "8+")) { geometry = MultirotorMixer::OCTA_PLUS; + } else if (!strcmp(geomname, "8x")) { geometry = MultirotorMixer::OCTA_X; + } else { debug("unrecognised geometry '%s'", geomname); return nullptr; } return new MultirotorMixer( - control_cb, - cb_handle, - geometry, - s[0] / 10000.0f, - s[1] / 10000.0f, - s[2] / 10000.0f, - s[3] / 10000.0f); + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f); } int diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index e2576a848..0e714ed50 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -57,7 +57,7 @@ #define CW (-1.0f) #define CCW (1.0f) -namespace +namespace { /* @@ -167,17 +167,21 @@ MultirotorMixer::mix(float *outputs, unsigned space) pitch * _rotors[i].pitch_scale + yaw * _rotors[i].yaw_scale + thrust; + if (tmp > max) max = tmp; + outputs[i] = tmp; } /* scale values into the -1.0 - 1.0 range */ if (max > 1.0f) { fixup_scale = 2.0f / max; + } else { fixup_scale = 2.0f; } + for (unsigned i = 0; i < _rotor_count; i++) outputs[i] = -1.0f + (outputs[i] * fixup_scale); diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 6dacb1635..eeb867f11 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -189,6 +189,7 @@ param_notify_changes(void) */ if (param_topic == -1) { param_topic = orb_advertise(ORB_ID(parameter_update), &pup); + } else { orb_publish(ORB_ID(parameter_update), param_topic, &pup); } @@ -455,6 +456,7 @@ param_reset(param_t param) utarray_erase(param_values, pos, 1); } } + param_unlock(); if (s != NULL) @@ -560,8 +562,7 @@ out: return result; } -struct param_import_state -{ +struct param_import_state { bool mark_saved; }; @@ -689,9 +690,10 @@ param_import_internal(int fd, bool mark_saved) do { result = bson_decoder_next(&decoder); - } while(result > 0); + } while (result > 0); out: + if (result < 0) debug("BSON error decoding parameters"); diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index f9b50d030..7e277cdc7 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -43,7 +43,7 @@ #include __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode) + float limit, uint8_t mode) { pid->kp = kp; pid->ki = ki; @@ -65,30 +65,35 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float if (isfinite(kp)) { pid->kp = kp; + } else { ret = 1; } if (isfinite(ki)) { pid->ki = ki; + } else { ret = 1; } if (isfinite(kd)) { pid->kd = kd; + } else { ret = 1; } if (isfinite(intmax)) { pid->intmax = intmax; + } else { ret = 1; } - + if (isfinite(limit)) { pid->limit = limit; + } else { ret = 1; } @@ -121,17 +126,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo goto start */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) - { + if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { return pid->last_output; } float i, d; pid->sp = sp; - + // Calculated current error value float error = pid->sp - val; - + if (isfinite(error)) { // Why is this necessary? DEW pid->error_previous = error; } @@ -140,30 +144,36 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo if (pid->mode == PID_MODE_DERIVATIV_CALC) { d = (error - pid->error_previous) / dt; + } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; + } else { d = 0.0f; } // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if( fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabs(i) > pid->intmax ) - { + + if (fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || + fabs(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; + } else { if (!isfinite(i)) { i = 0; } + pid->integral = i; pid->saturated = 0; } // Calculate the output. Limit output magnitude to pid->limit float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); + if (output > pid->limit) output = pid->limit; + if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { diff --git a/apps/systemlib/ppm_decode.c b/apps/systemlib/ppm_decode.c index dd6d43a77..a5d2f738d 100644 --- a/apps/systemlib/ppm_decode.c +++ b/apps/systemlib/ppm_decode.c @@ -30,7 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - + /** * @file ppm_decode.c * @@ -51,7 +51,7 @@ * * The PPM decoder works as follows. * - * Initially, the decoder waits in the UNSYNCH state for two edges + * Initially, the decoder waits in the UNSYNCH state for two edges * separated by PPM_MIN_START. Once the second edge is detected, * the decoder moves to the ARM state. * @@ -64,9 +64,9 @@ * * The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when * received calculates the time from the previous mark and records - * this time as the value for the next channel. + * this time as the value for the next channel. * - * If at any time waiting for an edge, the delay from the previous edge + * If at any time waiting for an edge, the delay from the previous edge * exceeds PPM_MIN_START the frame is deemed to have ended and the recorded * values are advertised to clients. */ @@ -132,21 +132,23 @@ ppm_input_decode(bool reset, unsigned count) /* how long since the last edge? */ width = count - ppm.last_edge; + if (count < ppm.last_edge) width += ppm.count_max; /* handle wrapped count */ + ppm.last_edge = count; - /* + /* * If this looks like a start pulse, then push the last set of values * and reset the state machine. * * Note that this is not a "high performance" design; it implies a whole - * frame of latency between the pulses being received and their being + * frame of latency between the pulses being received and their being * considered valid. */ if (width >= PPM_MIN_START) { - /* + /* * If the number of channels changes unexpectedly, we don't want * to just immediately jump on the new count as it may be a result * of noise or dropped edges. Instead, take a few frames to settle. @@ -169,11 +171,13 @@ ppm_input_decode(bool reset, unsigned count) ppm_decoded_channels = new_channel_count; new_channel_count = 0; } + } else { /* frame channel count matches expected, let's use it */ if (ppm.next_channel > PPM_MIN_CHANNELS) { for (i = 0; i < ppm.next_channel; i++) ppm_buffer[i] = ppm_temp_buffer[i]; + ppm_last_valid_decode = hrt_absolute_time(); } } @@ -193,10 +197,11 @@ ppm_input_decode(bool reset, unsigned count) 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; @@ -211,6 +216,7 @@ ppm_input_decode(bool reset, unsigned count) return; case ACTIVE: + /* we expect a well-formed pulse */ if (width > PPM_MAX_PULSE_WIDTH) goto error; /* pulse was too long */ @@ -228,7 +234,7 @@ ppm_input_decode(bool reset, unsigned count) ppm_temp_buffer[ppm.next_channel++] = interval; ppm.phase = INACTIVE; - return; + return; } diff --git a/apps/systemlib/ppm_decode.h b/apps/systemlib/ppm_decode.h index 681cbe830..c2b24321a 100644 --- a/apps/systemlib/ppm_decode.h +++ b/apps/systemlib/ppm_decode.h @@ -30,7 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - + /** * @file ppm_decode.h * diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c index 94a5283f0..50ac62464 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -131,7 +131,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma pid = task_create(name, priority, stack_size, entry, argv); if (pid > 0) { - + /* configure the scheduler */ struct sched_param param; @@ -140,6 +140,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma /* XXX do any other private task accounting here before the task starts */ } + sched_unlock(); return pid; @@ -157,15 +158,18 @@ int fmu_get_board_info(struct fmu_board_info_s *info) statres = stat("/dev/bma180", &sb); if (statres == OK) { - /* BMA180 indicates a v1.5-v1.6 board */ - strcpy(info->board_name, "FMU v1.6"); - info->board_version = 16; + /* BMA180 indicates a v1.5-v1.6 board */ + strcpy(info->board_name, "FMU v1.6"); + info->board_version = 16; + } else { statres = stat("/dev/accel", &sb); + if (statres == OK) { /* MPU-6000 indicates a v1.7+ board */ strcpy(info->board_name, "FMU v1.7"); info->board_version = 17; + } else { /* If no BMA and no MPU is present, it is a v1.3 board */ strcpy(info->board_name, "FMU v1.3"); -- cgit v1.2.3