crazyflie: increase imu reading rate

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

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
 

src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp

@@ -73,10 +73,10 @@
 #include <board_config.h>
 
 /* Configuration Constants */
-#ifdef PX4_I2C_BUS_EXPANSION3
+#ifdef PX4_I2C_BUS_EXPANSION
-#define VL53LXX_BUS 				PX4_I2C_BUS_EXPANSION3		// I2C port (I2C4) on fmu-v5
+#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_CONVERSION_INTERVAL 						1000 		/*  1ms */
-#define VL53LXX_SAMPLE_RATE									20000 	/* 20ms */
+#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")),
+	_sample_perf(perf_alloc(PC_ELAPSED, "vl53lxx_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "tr1_com_err"))
+	_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
+		writeRegister(SYSRANGE_START_REG, 0x01);
-		// in continuous mode
 
 		readRegister(SYSRANGE_START_REG, system_start);
 
-		while ((system_start & 0x01) == 1) {
+		if ((system_start & 0x01) == 1) {
-			readRegister(SYSRANGE_START_REG, system_start);
+			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 = distance_m;
-		report.current_distance = 2.0f;
-
-	} else {
-		report.current_distance = distance_m;
-	}
 
 	report.min_distance = VL53LXX_MIN_RANGING_DISTANCE;
 	report.max_distance = VL53LXX_MAX_RANGING_DISTANCE;

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),
+	_accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE / 2),
-	_gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true),
+	_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

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

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,