aboutsummaryrefslogtreecommitdiff
path: root/apps/drivers
diff options
context:
space:
mode:
authorpx4dev <px4@purgatory.org>2012-10-23 23:38:45 -0700
committerpx4dev <px4@purgatory.org>2012-10-23 23:51:13 -0700
commit2fc10320697ecaa9c4e0c52d4d047424e41e6336 (patch)
tree4f18f494ab811e29dc55452f92a63fff9d271dda /apps/drivers
parent34f99c7dca1995f8ddd9e8d61c4cbd7289f40e99 (diff)
downloadpx4-firmware-2fc10320697ecaa9c4e0c52d4d047424e41e6336.tar.gz
px4-firmware-2fc10320697ecaa9c4e0c52d4d047424e41e6336.tar.bz2
px4-firmware-2fc10320697ecaa9c4e0c52d4d047424e41e6336.zip
Major formatting/whitespace cleanup
Diffstat (limited to 'apps/drivers')
-rw-r--r--apps/drivers/bma180/bma180.cpp44
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_adc.c19
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_can.c47
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_init.c264
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_spi.c5
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_usb.c16
-rw-r--r--apps/drivers/device/device.h18
-rw-r--r--apps/drivers/device/i2c.cpp6
-rw-r--r--apps/drivers/device/i2c.h2
-rw-r--r--apps/drivers/device/spi.cpp2
-rw-r--r--apps/drivers/device/spi.h2
-rw-r--r--apps/drivers/drv_accel.h2
-rw-r--r--apps/drivers/drv_gyro.h2
-rw-r--r--apps/drivers/drv_hrt.h4
-rw-r--r--apps/drivers/drv_mag.h2
-rw-r--r--apps/drivers/drv_pwm_output.h2
-rw-r--r--apps/drivers/drv_sensor.h6
-rw-r--r--apps/drivers/drv_tone_alarm.h4
-rw-r--r--apps/drivers/hmc5883/hmc5883.cpp59
-rw-r--r--apps/drivers/l3gd20/l3gd20.cpp30
-rw-r--r--apps/drivers/mpu6000/mpu6000.cpp30
-rw-r--r--apps/drivers/ms5611/ms5611.cpp36
-rw-r--r--apps/drivers/px4io/px4io.cpp36
-rw-r--r--apps/drivers/px4io/uploader.cpp47
-rw-r--r--apps/drivers/stm32/drv_hrt.c89
-rw-r--r--apps/drivers/stm32/drv_pwm_servo.c115
-rw-r--r--apps/drivers/stm32/tone_alarm/tone_alarm.cpp87
27 files changed, 603 insertions, 373 deletions
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 <math.h>
#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 <nuttx/i2c.h>
-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<<i) & channel_mask) && (pwm_channels[i].gpio != 0))
+ if (((1 << i) & channel_mask) && (pwm_channels[i].gpio != 0))
pwm_channel_init(i);
}
+
return OK;
}
@@ -326,17 +340,18 @@ up_pwm_servo_set_rate(unsigned rate)
if (pwm_timers[i].base != 0)
pwm_timer_set_rate(i, rate);
}
+
return OK;
}
void
up_pwm_servo_arm(bool armed)
{
- /*
+ /*
* XXX this is inelgant and in particular will either jam outputs at whatever level
* they happen to be at at the time the timers stop or generate runts.
- * The right thing is almost certainly to kill auto-reload on the timers so that
- * they just stop at the end of their count for disable, and to reset/restart them
+ * The right thing is almost certainly to kill auto-reload on the timers so that
+ * they just stop at the end of their count for disable, and to reset/restart them
* for enable.
*/
diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
index 00f225dd5..03cf3ee5d 100644
--- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -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.
*
@@ -244,7 +244,8 @@ const tone_note ToneAlarm::_patterns[_max_pattern][_max_pattern_len] = {
{{TONE_NOTE_C7, 100}},
{{TONE_NOTE_D7, 100}},
{{TONE_NOTE_E7, 100}},
- { //This is tetris ;)
+ {
+ //This is tetris ;)
{TONE_NOTE_C6, 40},
{TONE_NOTE_G5, 20},
{TONE_NOTE_G5S, 20},
@@ -361,6 +362,7 @@ ToneAlarm::init()
int ret;
ret = CDev::init();
+
if (ret != OK)
return ret;
@@ -368,34 +370,34 @@ ToneAlarm::init()
stm32_configgpio(GPIO_TONE_ALARM);
/* clock/power on our timer */
- modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
+ modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
/* initialise the timer */
- rCR1 = 0;
- rCR2 = 0;
- rSMCR = 0;
- rDIER = 0;
- rCCER &= TONE_CCER; /* unlock CCMR* registers */
- rCCMR1 = TONE_CCMR1;
- rCCMR2 = TONE_CCMR2;
- rCCER = TONE_CCER;
- rDCR = 0;
-
- /* toggle the CC output each time the count passes 1 */
- TONE_rCCR = 1;
-
- /*
- * Configure the timebase to free-run at half max frequency.
- * XXX this should be more flexible in order to get a better
- * frequency range, but for the F4 with the APB1 timers based
- * at 42MHz, this gets us down to ~320Hz or so.
- */
- rPSC = 1;
-
- /* make sure the timer is running */
- rCR1 = GTIM_CR1_CEN;
-
- debug("ready");
+ rCR1 = 0;
+ rCR2 = 0;
+ rSMCR = 0;
+ rDIER = 0;
+ rCCER &= TONE_CCER; /* unlock CCMR* registers */
+ rCCMR1 = TONE_CCMR1;
+ rCCMR2 = TONE_CCMR2;
+ rCCER = TONE_CCER;
+ rDCR = 0;
+
+ /* toggle the CC output each time the count passes 1 */
+ TONE_rCCR = 1;
+
+ /*
+ * Configure the timebase to free-run at half max frequency.
+ * XXX this should be more flexible in order to get a better
+ * frequency range, but for the F4 with the APB1 timers based
+ * at 42MHz, this gets us down to ~320Hz or so.
+ */
+ rPSC = 1;
+
+ /* make sure the timer is running */
+ rCR1 = GTIM_CR1_CEN;
+
+ debug("ready");
return OK;
}
@@ -413,6 +415,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
switch (cmd) {
case TONE_SET_ALARM:
debug("TONE_SET_ALARM %u", arg);
+
if (new_pattern == 0) {
/* cancel any current alarm */
_current_pattern = _pattern_none;
@@ -431,10 +434,13 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
/* and start playing it */
next();
+
} else {
/* current pattern is higher priority than the new pattern, ignore */
}
+
break;
+
default:
result = -ENOTTY;
break;
@@ -457,8 +463,10 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len)
/* sanity-check the size of the write */
if (len > (_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);