From f49fd2acc75608bc9962897435ab84f4d57e35c6 Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Wed, 9 May 2018 10:34:08 +0200 Subject: [PATCH] crazyflie: increase imu reading rate --- ROMFS/px4fmu_common/init.d/rc.sensors | 5 ++ ROMFS/px4fmu_common/init.d/rcS | 4 -- .../distance_sensor/vl53lxx/vl53lxx.cpp | 65 ++++++++++++------- src/drivers/imu/mpu9250/mpu9250.cpp | 10 ++- src/drivers/imu/mpu9250/mpu9250.h | 5 ++ src/drivers/pmw3901/pmw3901.cpp | 19 ++++-- 6 files changed, 71 insertions(+), 37 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index ede1115da8..315108a6af 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -252,6 +252,11 @@ then # I2C bypass of mpu lps25h start + + # Optical flow deck + vl53lxx start + #pmw3901 start + fi if ver hwcmp AEROFC_V1 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ece2efd91c..e5a77a0c32 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -988,10 +988,6 @@ fi sh /etc/init.d/rc.logging - #vl53lxx start - #pmw3901 start - - # End of autostart fi diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index 8a3464629e..25a9f1daa7 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -73,10 +73,10 @@ #include /* Configuration Constants */ -#ifdef PX4_I2C_BUS_EXPANSION3 -#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION3 // I2C port (I2C4) on fmu-v5 +#ifdef PX4_I2C_BUS_EXPANSION +#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION #else -#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION // I2C on all others +#define VL53LXX_BUS 0 #endif #define VL53LXX_BASEADDR 0b0101001 // 7-bit address @@ -101,8 +101,8 @@ #define SYSTEM_INTERRUPT_CLEAR_REG 0x0B #define RESULT_RANGE_STATUS_REG 0x14 -#define VL53LXX_CONVERSION_INTERVAL 1000 /* 1ms */ -#define VL53LXX_SAMPLE_RATE 20000 /* 20ms */ +#define VL53LXX_CONVERSION_INTERVAL 1000 /* 1ms */ +#define VL53LXX_SAMPLE_RATE 50000 /* 50ms */ #define VL53LXX_MAX_RANGING_DISTANCE 2.0f #define VL53LXX_MIN_RANGING_DISTANCE 0.0f @@ -138,6 +138,7 @@ private: int _measure_ticks; bool _collect_phase; bool _new_measurement; + bool _measurement_started; int _class_instance; int _orb_class_instance; @@ -200,21 +201,22 @@ private: extern "C" __EXPORT int vl53lxx_main(int argc, char *argv[]); VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) : - I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 400000), // 400 kHz only for Crazyflie (other boards use max 100 kHz) + I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 400000), _rotation(rotation), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), _new_measurement(true), + _measurement_started(false), _class_instance(-1), _orb_class_instance(-1), _distance_sensor_topic(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "tr1_read")), - _comms_errors(perf_alloc(PC_COUNT, "tr1_com_err")) + _sample_perf(perf_alloc(PC_ELAPSED, "vl53lxx_read")), + _comms_errors(perf_alloc(PC_COUNT, "vl53lxx_com_err")) { // up the retries since the device misses the first measure attempts - I2C::_retries = 3; + I2C::_retries = 2; // enable debug() calls _debug_enabled = false; @@ -233,6 +235,10 @@ VL53LXX::~VL53LXX() delete _reports; } + if (_distance_sensor_topic != nullptr) { + orb_unadvertise(_distance_sensor_topic); + } + if (_class_instance != -1) { unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); } @@ -469,9 +475,6 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value) return ret; } - /* wait for it to complete */ - usleep(VL53LXX_CONVERSION_INTERVAL); - /* read from the sensor */ ret = transfer(nullptr, 0, &val, 1); @@ -501,9 +504,6 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length) return ret; } - /* wait for it to complete */ - usleep(VL53LXX_CONVERSION_INTERVAL); - /* read from the sensor */ ret = transfer(nullptr, 0, &val[0], length); @@ -597,17 +597,37 @@ VL53LXX::measure() writeRegister(0xFF, 0x00); writeRegister(0x80, 0x00); - writeRegister(SYSRANGE_START_REG, 0x01); // maybe could be removed by putting sensor - // in continuous mode + writeRegister(SYSRANGE_START_REG, 0x01); readRegister(SYSRANGE_START_REG, system_start); - while ((system_start & 0x01) == 1) { - readRegister(SYSRANGE_START_REG, system_start); + if ((system_start & 0x01) == 1) { + work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, + 1000); // reschedule every 1 ms until measurement is ready + ret = OK; + return ret; + + } else { + _measurement_started = true; } } + if (!_collect_phase && !_measurement_started) { + + readRegister(SYSRANGE_START_REG, system_start); + + if ((system_start & 0x01) == 1) { + work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, + 1000); // reschedule every 1 ms until measurement is ready + ret = OK; + return ret; + + } else { + _measurement_started = true; + } + } + readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement); if ((wait_for_measurement & 0x07) == 0) { @@ -662,12 +682,7 @@ VL53LXX::collect() report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.orientation = _rotation; - if (distance_m > 2.0f) { - report.current_distance = 2.0f; - - } else { - report.current_distance = distance_m; - } + report.current_distance = distance_m; report.min_distance = VL53LXX_MIN_RANGING_DISTANCE; report.max_distance = VL53LXX_MAX_RANGING_DISTANCE; diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index ab045158a7..0ab770e75b 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -139,7 +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 _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")), @@ -156,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), - _gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true), + _accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE / 2), + _gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE / 2, true), _rotation(rotation), _checked_next(0), _last_temperature(0), @@ -1157,7 +1161,7 @@ MPU9250::cycle() &_work, (worker_t)&MPU9250::cycle_trampoline, this, - USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION)); + USEC2TICK(900)); } } #endif diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 15f11927ae..08f721bd7d 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -181,6 +181,11 @@ #define MPU_WHOAMI_6500 0x70 #define MPU9250_ACCEL_DEFAULT_RATE 1000 +// #ifdef CRAZYFLIE +// #define MPU9250_ACCEL_MAX_OUTPUT_RATE 3*280 +// #else +// #define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 +// #endif #define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 #define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 #define MPU9250_GYRO_DEFAULT_RATE 1000 diff --git a/src/drivers/pmw3901/pmw3901.cpp b/src/drivers/pmw3901/pmw3901.cpp index d717d3ba9c..8bfd9c30f4 100644 --- a/src/drivers/pmw3901/pmw3901.cpp +++ b/src/drivers/pmw3901/pmw3901.cpp @@ -78,11 +78,15 @@ /* Configuration Constants */ #ifdef PX4_SPI_BUS_EXPANSION -#define PMW3901_BUS PX4_SPI_BUS_EXPANSION /* fmu-v4pro: PX4_SPI_BUS_EXT1 | fmu-v5: PX4_SPI_BUS_EXTERNAL1 */ +#define PMW3901_BUS PX4_SPI_BUS_EXPANSION +#else +#define PMW3901_BUS 0 #endif #ifdef PX4_SPIDEV_EXPANSION_2 -#define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2 /* fmu-v4pro: PX4_SPIDEV_EXT0 | fmu-v5: PX4_SPIDEV_EXTERNAL1_1 */ +#define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2 +#else +#define PMW3901_SPIDEV 0 #endif #define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz @@ -235,7 +239,8 @@ PMW3901::sensorInit() uint8_t data[5]; // initialize pmw3901 flow sensor - readRegister(0x00, &data[0], 1); // chip idreadRegister(0x5F, &data[1], 1); // inverse chip id + readRegister(0x00, &data[0], 1); // chip id + readRegister(0x5F, &data[1], 1); // inverse chip id // Power on reset writeRegister(0x3A, 0x5A); @@ -331,6 +336,10 @@ PMW3901::sensorInit() writeRegister(0x5A, 0x50); writeRegister(0x40, 0x80); + writeRegister(0x7F, 0x00); + writeRegister(0x5A, 0x10); + writeRegister(0x54, 0x00); + ret = OK; return ret; @@ -643,7 +652,7 @@ PMW3901::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&PMW3901::cycle_trampoline, this, 1000); + work_queue(LPWORK, &_work, (worker_t)&PMW3901::cycle_trampoline, this, 1000); /* notify about state change */ struct subsystem_info_s info = {}; @@ -682,7 +691,7 @@ PMW3901::cycle() collect(); /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, + work_queue(LPWORK, &_work, (worker_t)&PMW3901::cycle_trampoline, this,