diff options
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 68 | ||||
-rw-r--r-- | src/drivers/device/spi.cpp | 34 | ||||
-rw-r--r-- | src/drivers/device/spi.h | 11 | ||||
-rw-r--r-- | src/drivers/drv_pwm_output.h | 3 | ||||
-rw-r--r-- | src/drivers/drv_tone_alarm.h | 2 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 4 | ||||
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 3 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 6 | ||||
-rw-r--r-- | src/drivers/ms5611/ms5611_spi.cpp | 16 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 161 | ||||
-rw-r--r-- | src/drivers/stm32/tone_alarm/tone_alarm.cpp | 10 |
11 files changed, 222 insertions, 96 deletions
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 135767b26..ae2a645f7 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -58,6 +58,7 @@ #include <nuttx/sdio.h> #include <nuttx/mmcsd.h> #include <nuttx/analog/adc.h> +#include <nuttx/gran.h> #include <stm32.h> #include "board_config.h" @@ -69,6 +70,7 @@ #include <drivers/drv_led.h> #include <systemlib/cpuload.h> +#include <systemlib/perf_counter.h> /**************************************************************************** * Pre-Processor Definitions @@ -96,10 +98,70 @@ * Protected Functions ****************************************************************************/ +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + /**************************************************************************** * Public Functions ****************************************************************************/ +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + /************************************************************************************ * Name: stm32_boardinitialize * @@ -110,7 +172,8 @@ * ************************************************************************************/ -__EXPORT void stm32_boardinitialize(void) +__EXPORT void +stm32_boardinitialize(void) { /* configure SPI interfaces */ stm32_spiinitialize(); @@ -170,6 +233,9 @@ __EXPORT int nsh_archinitialize(void) /* configure the high-resolution time/callout interface */ hrt_init(); + /* configure the DMA allocator */ + dma_alloc_init(); + /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 8fffd60cb..fa6b78d64 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -67,6 +67,7 @@ SPI::SPI(const char *name, CDev(name, devname, irq), // public // protected + locking_mode(LOCK_PREEMPTION), // private _bus(bus), _device(device), @@ -132,13 +133,25 @@ SPI::probe() int SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { + irqstate_t state; if ((send == nullptr) && (recv == nullptr)) return -EINVAL; - /* do common setup */ - if (!up_interrupt_context()) - SPI_LOCK(_dev, true); + /* lock the bus as required */ + if (!up_interrupt_context()) { + switch (locking_mode) { + default: + case LOCK_PREEMPTION: + state = irqsave(); + break; + case LOCK_THREADS: + SPI_LOCK(_dev, true); + break; + case LOCK_NONE: + break; + } + } SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); @@ -151,8 +164,19 @@ 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); + if (!up_interrupt_context()) { + switch (locking_mode) { + default: + case LOCK_PREEMPTION: + irqrestore(state); + break; + case LOCK_THREADS: + SPI_LOCK(_dev, false); + break; + case LOCK_NONE: + break; + } + } return OK; } diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index e0122372a..9103dca2e 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -101,6 +101,17 @@ protected: */ int transfer(uint8_t *send, uint8_t *recv, unsigned len); + /** + * Locking modes supported by the driver. + */ + enum LockMode { + LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ + LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ + LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ + }; + + LockMode locking_mode; /**< selected locking mode */ + private: int _bus; enum spi_dev_e _device; diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index ec9d4ca09..94e923d71 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -118,6 +118,9 @@ ORB_DECLARE(output_pwm); /** start DSM bind */ #define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) +#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ +#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ + /** Power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index f0b860620..caf2b0f6e 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -60,6 +60,8 @@ #include <sys/ioctl.h> +#define TONEALARM_DEVICE_PATH "/dev/tone_alarm" + #define _TONE_ALARM_BASE 0x7400 #define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 58a1593ed..5e891d7bb 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1292,10 +1292,6 @@ test() if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); - /* set the queue depth to 10 */ - if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) - errx(1, "failed to set queue depth"); - /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4c3b0ce51..748809d3f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -367,7 +367,6 @@ 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); @@ -393,8 +392,6 @@ L3GD20::probe() success = true; } - irqrestore(flags); - if (success) return OK; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a90cd0a3d..2c56b6035 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -525,7 +525,6 @@ 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); @@ -540,7 +539,6 @@ 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; @@ -549,15 +547,12 @@ 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 */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - irqrestore(flags); - if (success) return OK; @@ -1013,6 +1008,7 @@ LSM303D::accel_set_range(unsigned max_g) _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); return OK; diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 21caed2ff..e547c913b 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -142,23 +142,15 @@ 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; @@ -270,13 +262,7 @@ MS5611_SPI::_reg16(unsigned reg) int MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) { - irqstate_t flags = irqsave(); - - int ret = transfer(send, recv, len); - - irqrestore(flags); - - return ret; + return transfer(send, recv, len); } #endif /* PX4_SPIDEV_BARO */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 78d1d3e63..5fff1feac 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -241,7 +241,8 @@ private: volatile int _task; ///<worker task id volatile bool _task_should_exit; ///<worker terminate flag - int _mavlink_fd; ///<mavlink file descriptor + int _mavlink_fd; ///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. + int _thread_mavlink_fd; ///<mavlink file descriptor for thread. perf_counter_t _perf_update; ///<local performance counter @@ -254,6 +255,7 @@ private: int _t_actuator_armed; ///< system armed control topic int _t_vehicle_control_mode;///< vehicle control mode topic int _t_param; ///< parameter update topic + int _t_vehicle_command; ///< vehicle command topic /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io @@ -274,6 +276,7 @@ private: bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power + /** * Trampoline to the worker task */ @@ -389,7 +392,7 @@ private: /** * Send mixer definition text to IO */ - int mixer_send(const char *buf, unsigned buflen); + int mixer_send(const char *buf, unsigned buflen, unsigned retries=3); /** * Handle a status update from IO. @@ -409,6 +412,13 @@ private: */ int io_handle_alarms(uint16_t alarms); + /** + * Handle issuing dsm bind ioctl to px4io. + * + * @param dsmMode 0:dsm2, 1:dsmx + */ + void dsm_bind_ioctl(int dsmMode); + }; @@ -433,6 +443,7 @@ PX4IO::PX4IO(device::Device *interface) : _task(-1), _task_should_exit(false), _mavlink_fd(-1), + _thread_mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _status(0), _alarms(0), @@ -440,6 +451,7 @@ PX4IO::PX4IO(device::Device *interface) : _t_actuator_armed(-1), _t_vehicle_control_mode(-1), _t_param(-1), + _t_vehicle_command(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), @@ -710,10 +722,10 @@ void PX4IO::task_main() { hrt_abstime last_poll_time = 0; - int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); log("starting"); + _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); /* * Subscribe to the appropriate PWM output topic based on whether we are the @@ -732,16 +744,20 @@ PX4IO::task_main() _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); + orb_set_interval(_t_param, 1000); /* 1Hz update rate max. */ + if ((_t_actuators < 0) || (_t_actuator_armed < 0) || (_t_vehicle_control_mode < 0) || - (_t_param < 0)) { + (_t_param < 0) || + (_t_vehicle_command < 0)) { log("subscription(s) failed"); goto out; } /* poll descriptor */ - pollfd fds[4]; + pollfd fds[5]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_actuator_armed; @@ -750,8 +766,10 @@ PX4IO::task_main() fds[2].events = POLLIN; fds[3].fd = _t_param; fds[3].events = POLLIN; + fds[4].fd = _t_vehicle_command; + fds[4].events = POLLIN; - debug("ready"); + log("ready"); /* lock against the ioctl handler */ lock(); @@ -791,6 +809,16 @@ PX4IO::task_main() if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) io_set_arming_state(); + /* if we have a vehicle command, handle it */ + if (fds[4].revents & POLLIN) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); + // Check for a DSM pairing command + if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { + dsm_bind_ioctl((int)cmd.param2); + } + } + /* * If it's time for another tick of the polling status machine, * try it now. @@ -825,20 +853,11 @@ PX4IO::task_main() int32_t dsm_bind_val; param_t dsm_bind_param; - // See if bind parameter has been set, and reset it to 0 + // See if bind parameter has been set, and reset it to -1 param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); - if (dsm_bind_val) { - if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { - if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) { - mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x'); - ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7); - } else { - mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected"); - } - } else { - mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); - } - dsm_bind_val = 0; + if (dsm_bind_val >= 0) { + dsm_bind_ioctl(dsm_bind_val); + dsm_bind_val = -1; param_set(dsm_bind_param, &dsm_bind_val); } @@ -1145,6 +1164,23 @@ PX4IO::io_handle_status(uint16_t status) return ret; } +void +PX4IO::dsm_bind_ioctl(int dsmMode) +{ + if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { + /* 0: dsm2, 1:dsmx */ + if ((dsmMode >= 0) && (dsmMode <= 1)) { + mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); + } else { + mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); + } + } else { + mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); + } +} + + int PX4IO::io_handle_alarms(uint16_t alarms) { @@ -1432,61 +1468,70 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t } int -PX4IO::mixer_send(const char *buf, unsigned buflen) +PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) { - uint8_t frame[_max_transfer]; - px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; - unsigned max_len = _max_transfer - sizeof(px4io_mixdata); - msg->f2i_mixer_magic = F2I_MIXER_MAGIC; - msg->action = F2I_MIXER_ACTION_RESET; + uint8_t frame[_max_transfer]; do { - unsigned count = buflen; - if (count > max_len) - count = max_len; + px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; + unsigned max_len = _max_transfer - sizeof(px4io_mixdata); - if (count > 0) { - memcpy(&msg->text[0], buf, count); - buf += count; - buflen -= count; - } + msg->f2i_mixer_magic = F2I_MIXER_MAGIC; + msg->action = F2I_MIXER_ACTION_RESET; - /* - * We have to send an even number of bytes. This - * will only happen on the very last transfer of a - * mixer, and we are guaranteed that there will be - * space left to round up as _max_transfer will be - * even. - */ - unsigned total_len = sizeof(px4io_mixdata) + count; - if (total_len % 1) { - msg->text[count] = '\0'; - total_len++; - } + do { + unsigned count = buflen; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + if (count > max_len) + count = max_len; - if (ret) { - log("mixer send error %d", ret); - return ret; - } + if (count > 0) { + memcpy(&msg->text[0], buf, count); + buf += count; + buflen -= count; + } - msg->action = F2I_MIXER_ACTION_APPEND; + /* + * We have to send an even number of bytes. This + * will only happen on the very last transfer of a + * mixer, and we are guaranteed that there will be + * space left to round up as _max_transfer will be + * even. + */ + unsigned total_len = sizeof(px4io_mixdata) + count; + if (total_len % 1) { + msg->text[count] = '\0'; + total_len++; + } - } while (buflen > 0); + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + log("mixer send error %d", ret); + return ret; + } + + msg->action = F2I_MIXER_ACTION_APPEND; + + } while (buflen > 0); + + retries--; + + log("mixer sent"); + + } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK))); /* check for the mixer-OK flag */ if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { - debug("mixer upload OK"); mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); return 0; - } else { - debug("mixer rejected by IO"); - mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); } + log("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); + /* load must have failed for some reason */ return -EINVAL; } @@ -1999,9 +2044,9 @@ bind(int argc, char *argv[]) errx(0, "needs argument, use dsm2 or dsmx"); if (!strcmp(argv[2], "dsm2")) - pulses = 3; + pulses = DSM2_BIND_PULSES; else if (!strcmp(argv[2], "dsmx")) - pulses = 7; + pulses = DSMX_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a582ece17..930809036 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -317,7 +317,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : - CDev("tone_alarm", "/dev/tone_alarm"), + CDev("tone_alarm", TONEALARM_DEVICE_PATH), _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), @@ -820,10 +820,10 @@ play_tune(unsigned tune) { int fd, ret; - fd = open("/dev/tone_alarm", 0); + fd = open(TONEALARM_DEVICE_PATH, 0); if (fd < 0) - err(1, "/dev/tone_alarm"); + err(1, TONEALARM_DEVICE_PATH); ret = ioctl(fd, TONE_SET_ALARM, tune); close(fd); @@ -839,10 +839,10 @@ play_string(const char *str, bool free_buffer) { int fd, ret; - fd = open("/dev/tone_alarm", O_WRONLY); + fd = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (fd < 0) - err(1, "/dev/tone_alarm"); + err(1, TONEALARM_DEVICE_PATH); ret = write(fd, str, strlen(str) + 1); close(fd); |