crazyflie: add probe() to vl53lxx driver and set imu sampling rate to 200 Hz for this board

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

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

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)
 

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);
 
-	// set I2C to standard mode
-	writeRegister(0x88, 0x00);
+	if (ret != OK) {
+		ret = PX4_ERROR;
+		return ret;
+	}
 
-	writeRegister(0x80, 0x01);
-	writeRegister(0xFF, 0x01);
-	writeRegister(0x00, 0x00);
-	readRegister(0x91, val);
-	writeRegister(0x00, 0x01);
-	writeRegister(0xFF, 0x00);
-	writeRegister(0x80, 0x00);
+	ret |= writeRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val | 0x01);
+
+	// set I2C to standard mode
+	ret |= writeRegister(0x88, 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,

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),

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 */