From 533f42adb8bc3f40d6751f2c916cf64c6b6248c9 Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Mon, 14 May 2018 18:26:27 +0200 Subject: [PATCH] crazyflie: add probe() to vl53lxx driver and set imu sampling rate to 200 Hz for this board --- .../configs/nuttx_px4fmu-v4pro_default.cmake | 1 - cmake/configs/nuttx_px4fmu-v5_default.cmake | 1 - src/drivers/boards/crazyflie/board_config.h | 2 +- .../distance_sensor/vl53lxx/vl53lxx.cpp | 63 +++++++++++++------ src/drivers/imu/mpu9250/mpu9250.cpp | 15 +++-- src/drivers/imu/mpu9250/mpu9250.h | 11 ++-- 6 files changed, 57 insertions(+), 36 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake index ea76f0f64b..8985fdfd0c 100644 --- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake @@ -25,7 +25,6 @@ set(config_module_list drivers/imu/mpu6000 drivers/imu/mpu9250 drivers/oreoled - drivers/pmw3901 drivers/pwm_input drivers/pwm_out_sim drivers/px4flow diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake index 9c645b329b..f866340b56 100644 --- a/cmake/configs/nuttx_px4fmu-v5_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake @@ -25,7 +25,6 @@ set(config_module_list drivers/imu/mpu6000 drivers/imu/mpu9250 drivers/oreoled - drivers/pmw3901 drivers/pwm_input drivers/pwm_out_sim drivers/px4flow diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h index 5153db41fa..1cb3b1bf27 100644 --- a/src/drivers/boards/crazyflie/board_config.h +++ b/src/drivers/boards/crazyflie/board_config.h @@ -118,7 +118,7 @@ #define PX4_FLOW_BUS_CS_GPIO { GPIO_SPI1_CS0_EXT, GPIO_SPI1_CS1_EXT, GPIO_SPI1_CS2_EXT } /* SPI1 Devices */ -#define PX4_SPIDEV_EXPANSION_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 0) +#define PX4_SPIDEV_EXPANSION_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 0) // SD CARD BREAKOUT #define PX4_SPIDEV_EXPANSION_2 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 1) // OPTICAL FLOW BREAKOUT #define PX4_SPIDEV_EXPANSION_3 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 2) diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index 25a9f1daa7..a4c5b1eee3 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -107,6 +107,9 @@ #define VL53LXX_MAX_RANGING_DISTANCE 2.0f #define VL53LXX_MIN_RANGING_DISTANCE 0.0f +#define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0 +#define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA + #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif @@ -130,6 +133,9 @@ public: */ void print_info(); +protected: + virtual int probe(); + private: uint8_t _rotation; work_s _work; @@ -253,24 +259,35 @@ int VL53LXX::sensorInit() { uint8_t val = 0; - int ret; + int ret = OK; float rate_limit; uint8_t rate_limit_split[2]; // I2C at 2.8V on sensor side of level shifter - readRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val); - writeRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val | 0x01); + ret |= readRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val); + + if (ret != OK) { + ret = PX4_ERROR; + return ret; + } + + ret |= writeRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val | 0x01); // set I2C to standard mode - writeRegister(0x88, 0x00); + ret |= writeRegister(0x88, 0x00); - writeRegister(0x80, 0x01); - writeRegister(0xFF, 0x01); - writeRegister(0x00, 0x00); - readRegister(0x91, val); - writeRegister(0x00, 0x01); - writeRegister(0xFF, 0x00); - writeRegister(0x80, 0x00); + ret |= writeRegister(0x80, 0x01); + ret |= writeRegister(0xFF, 0x01); + ret |= writeRegister(0x00, 0x00); + ret |= readRegister(0x91, val); + ret |= writeRegister(0x00, 0x01); + ret |= writeRegister(0xFF, 0x00); + ret |= writeRegister(0x80, 0x00); + + if (ret != OK) { + ret = PX4_ERROR; + return ret; + } stop_variable_ = val; @@ -306,8 +323,6 @@ VL53LXX::init() goto out; } - sensorInit(); - /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); @@ -339,6 +354,16 @@ out: return ret; } +int +VL53LXX::probe() +{ + if (sensorInit() == OK) { + return OK; + } + + // not found on any address + return -EIO; +} int VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg) @@ -602,7 +627,7 @@ VL53LXX::measure() readRegister(SYSRANGE_START_REG, system_start); if ((system_start & 0x01) == 1) { - work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, + work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000); // reschedule every 1 ms until measurement is ready ret = OK; return ret; @@ -618,7 +643,7 @@ VL53LXX::measure() readRegister(SYSRANGE_START_REG, system_start); if ((system_start & 0x01) == 1) { - work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, + work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000); // reschedule every 1 ms until measurement is ready ret = OK; return ret; @@ -631,7 +656,7 @@ VL53LXX::measure() readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement); if ((wait_for_measurement & 0x07) == 0) { - work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, + work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000); // reschedule every 1 ms until measurement is ready ret = OK; return ret; @@ -716,7 +741,7 @@ VL53LXX::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000); + work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000); /* notify about state change */ struct subsystem_info_s info = {}; @@ -741,7 +766,7 @@ VL53LXX::start() void VL53LXX::stop() { - work_cancel(HPWORK, &_work); + work_cancel(LPWORK, &_work); } @@ -766,7 +791,7 @@ VL53LXX::cycle() collect(); - work_queue(HPWORK, + work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 1e949dac21..50a3381e07 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -139,12 +139,11 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), -// #ifdef CRAZYFLIE -// _sample_rate(250), -// #else -// _sample_rate(1000), -// #endif - _sample_rate(250), +#ifdef CONFIG_ARCH_BOARD_CRAZYFLIE + _sample_rate(200), +#else + _sample_rate(1000), +#endif _accel_reads(perf_alloc(PC_COUNT, "mpu9250_acc_read")), _gyro_reads(perf_alloc(PC_COUNT, "mpu9250_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu9250_read")), @@ -161,8 +160,8 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE / 4), - _gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE / 4, true), + _accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE), + _gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true), _rotation(rotation), _checked_next(0), _last_temperature(0), diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 963db6ba21..e82248a1e5 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -181,12 +181,11 @@ #define MPU_WHOAMI_6500 0x70 #define MPU9250_ACCEL_DEFAULT_RATE 1000 -// #ifdef CRAZYFLIE -// #define MPU9250_ACCEL_MAX_OUTPUT_RATE 4*280 -// #else -// #define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 -// #endif -#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 // remove this when using the previous #ifdef +#ifdef CONFIG_ARCH_BOARD_CRAZYFLIE +#define MPU9250_ACCEL_MAX_OUTPUT_RATE 1000 +#else +#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 +#endif #define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 #define MPU9250_GYRO_DEFAULT_RATE 1000 /* rates need to be the same between accel and gyro */