diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-12 00:43:27 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-12 00:43:27 +0200 |
commit | 0308f399d98f53a634206705c57eed208f5ad8a3 (patch) | |
tree | 540fc6df847cf61f2ac1676adc9d72bd99ab65e2 /src | |
parent | 8d497b58f95d910b7ae6d8447d8c5dccf77c565a (diff) | |
parent | 5e6d3604a377ab56bb0f40384fffb9370dbe0d74 (diff) | |
download | px4-firmware-0308f399d98f53a634206705c57eed208f5ad8a3.tar.gz px4-firmware-0308f399d98f53a634206705c57eed208f5ad8a3.tar.bz2 px4-firmware-0308f399d98f53a634206705c57eed208f5ad8a3.zip |
Merge branch 'master' of github.com:PX4/Firmware
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 13 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 19 | ||||
-rw-r--r-- | src/drivers/ms5611/ms5611_spi.cpp | 9 | ||||
-rw-r--r-- | src/modules/px4iofirmware/controls.c | 8 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.h | 4 | ||||
-rw-r--r-- | src/modules/px4iofirmware/sbus.c | 12 |
6 files changed, 47 insertions, 18 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e6d765e13..970e8cf4b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -377,9 +377,12 @@ out: int L3GD20::probe() { + irqstate_t flags = irqsave(); /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); + bool success = false; + /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { @@ -390,15 +393,21 @@ L3GD20::probe() #else #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 #endif - return OK; + + success = true; } if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; - return OK; + success = true; } + irqrestore(flags); + + if (success) + return OK; + return -EIO; } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 05d6f1881..35904cc4d 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -404,7 +404,7 @@ public: LSM303D_mag(LSM303D *parent); ~LSM303D_mag(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); protected: @@ -498,8 +498,10 @@ LSM303D::init() int mag_ret; /* do SPI init (and probe) first */ - if (SPI::init() != OK) + if (SPI::init() != OK) { + warnx("SPI init failed"); goto out; + } /* allocate basic report buffers */ _num_accel_reports = 2; @@ -541,6 +543,7 @@ out: void LSM303D::reset() { + irqstate_t flags = irqsave(); /* enable accel*/ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); @@ -555,6 +558,7 @@ LSM303D::reset() mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); + irqrestore(flags); _accel_read = 0; _mag_read = 0; @@ -563,11 +567,16 @@ LSM303D::reset() int LSM303D::probe() { + irqstate_t flags = irqsave(); /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); + + irqrestore(flags); + + if (success) return OK; return -EIO; @@ -1470,8 +1479,10 @@ start() /* create the driver */ g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); - if (g_dev == nullptr) + if (g_dev == nullptr) { + warnx("failed instantiating LSM303D obj"); goto fail; + } if (OK != g_dev->init()) goto fail; diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index f6c624340..21caed2ff 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -134,6 +134,7 @@ int MS5611_SPI::init() { int ret; + irqstate_t flags; ret = SPI::init(); if (ret != OK) { @@ -141,15 +142,23 @@ MS5611_SPI::init() goto out; } + /* disable interrupts, make this section atomic */ + flags = irqsave(); /* send reset command */ ret = _reset(); + /* re-enable interrupts */ + irqrestore(flags); if (ret != OK) { debug("reset failed"); goto out; } + /* disable interrupts, make this section atomic */ + flags = irqsave(); /* read PROM */ ret = _read_prom(); + /* re-enable interrupts */ + irqrestore(flags); if (ret != OK) { debug("prom readout failed"); goto out; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 796c6cd9f..617b536f4 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -108,9 +108,11 @@ controls_tick() { perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); - if (sbus_updated) + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + r_raw_rc_count = 8; + } perf_end(c_gather_sbus); /* @@ -124,8 +126,6 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; perf_end(c_gather_ppm); - ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS); - /* * In some cases we may have received a frame, but input has still * been lost. diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index dea67043e..11f4d053d 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -51,7 +51,7 @@ */ #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 +#define PX4IO_INPUT_CHANNELS 8 // XXX this should be 18 channels /* * Debug logging @@ -200,7 +200,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 073ddeaba..c523df6ca 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -66,7 +66,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels); int sbus_init(const char *device) @@ -97,7 +97,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values) +sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -154,7 +154,7 @@ sbus_input(uint16_t *values, uint16_t *num_values) * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values); + return sbus_decode(now, values, num_values, max_channels); } /* @@ -194,7 +194,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { @@ -214,8 +214,8 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_values; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { |