ICM20602 and ICM20608-G: new standalone driver (8 kHz gyro, 4 kHz accel)

- uses the FIFO and SPI DMA - currently implemented on px4_fmu-v4 only - clip counters - new low pass filter that operates on an array of data (LowPassFilter2pArray) - new sensor messages for better visibility - sensor_{accel, gyro}_fifo: full raw data for optional logging and analysis - sensor_{accel, gyro}_status: metadata, clipping, etc - sensor_{accel, gyro}_integrated: delta angles and delta velocities - eventually these will replace the existing sensor_{accel, gyro} monolith
PX4 · Nov 5, 2019 · 0ef4725 · 0ef4725
1 parent b999581
commit 0ef4725
Show file tree

Hide file tree

Showing 57 changed files with 3,545 additions and 122 deletions.
diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake
@@ -30,7 +30,8 @@ px4_add_board(
 		#magnetometer # all available magnetometer drivers
 		magnetometer/hmc5883
 		#mkblctrl
-		optical_flow/px4flow
+		#optical_flow/px4flow
+		osd
 		#pca9685
 		#pwm_input
 		#pwm_out_sim
@@ -40,18 +41,18 @@ px4_add_board(
 		#telemetry # all available telemetry drivers
 		telemetry/frsky_telemetry
 		#test_ppm
-		osd
 
 	MODULES
+		#airspeed_selector
 		attitude_estimator_q
+		battery_status
 		#camera_feedback
 		commander
 		dataman
 		ekf2
 		events
 		#fw_att_control
 		#fw_pos_control_l1
-		#rover_pos_control
 		land_detector
 		landing_target_estimator
 		load_mon
@@ -61,17 +62,16 @@ px4_add_board(
 		mc_att_control
 		mc_pos_control
 		navigator
-		battery_status
+		#rover_pos_control
 		sensors
 		sih
 		#vmount
 		#vtol_att_control
-		#airspeed_selector
 
 	SYSTEMCMDS
 		#bl_update
-		dmesg
 		config
+		dmesg
 		dumpfile
 		esc_calib
 		hardfault_log

diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake
@@ -47,7 +47,7 @@ px4_add_board(
 		magnetometer/hmc5883
 		#mkblctrl
 		#optical_flow # all available optical flow drivers
-		optical_flow/px4flow
+		#optical_flow/px4flow
 		#pca9685
 		#protocol_splitter
 		#pwm_input
@@ -62,15 +62,16 @@ px4_add_board(
 		#uavcan
 
 	MODULES
+		#airspeed_selector
 		#attitude_estimator_q
+		battery_status
 		camera_feedback
 		commander
 		dataman
 		ekf2
 		events
 		fw_att_control
 		fw_pos_control_l1
-		#rover_pos_control
 		land_detector
 		#landing_target_estimator
 		load_mon
@@ -80,11 +81,10 @@ px4_add_board(
 		mc_att_control
 		mc_pos_control
 		navigator
-		battery_status
+		#rover_pos_control
 		sensors
 		vmount
 		vtol_att_control
-		#airspeed_selector
 
 	SYSTEMCMDS
 		bl_update
@@ -110,7 +110,7 @@ px4_add_board(
 		tune_control
 		#usb_connected
 		ver
-		work_queue
+		#work_queue
 
 	EXAMPLES
 		#bottle_drop # OBC challenge

diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake
@@ -33,13 +33,14 @@ px4_add_board(
 		irlock
 		lights/rgbled
 		magnetometer/hmc5883
-		optical_flow/px4flow
+		#optical_flow/px4flow
 		px4fmu
 		px4io
 		tone_alarm
 
 	MODULES
 		#attitude_estimator_q
+		battery_status
 		camera_feedback
 		commander
 		dataman
@@ -54,7 +55,6 @@ px4_add_board(
 		mc_att_control
 		mc_pos_control
 		navigator
-		battery_status
 		sensors
 		vmount
 

diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake
@@ -26,7 +26,10 @@ px4_add_board(
 		dshot
 		gps
 		heater
-		imu # all available imu drivers
+		#imu # all available imu drivers
+		imu/invensense/icm20602
+		imu/invensense/icm20608-g
+		imu/mpu9250
 		irlock
 		lights/blinkm
 		lights/rgbled

diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors
@@ -48,10 +48,10 @@ fi
 # will prevent the incorrect driver from a successful initialization.
 
 # ICM20602 internal SPI bus ICM-20608-G is rotated 90 deg yaw
-if ! mpu6000 -R 2 -T 20602 start
+if ! icm20602 -R 8 start
 then
 	# ICM20608 internal SPI bus ICM-20602-G is rotated 90 deg yaw
-	if ! mpu6000 -R 2 -T 20608 start
+	if ! icm20608g -R 8 start
 	then
 		# BMI055 accel internal SPI bus
 		bmi055 -A start

diff --git a/boards/px4/fmu-v4/nuttx-config/include/board.h b/boards/px4/fmu-v4/nuttx-config/include/board.h
@@ -269,6 +269,10 @@
 #define GPIO_SPI1_MOSI  (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
 #define GPIO_SPI1_SCK   (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
 
+#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1
+#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2
+
+
 #define GPIO_SPI2_MISO  (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz)
 #define GPIO_SPI2_MOSI  (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
 #define GPIO_SPI2_SCK   (GPIO_SPI2_SCK_1|GPIO_SPEED_50MHz)

diff --git a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig
@@ -184,8 +184,10 @@ CONFIG_STM32_SDIO_CARD=y
 CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
 CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32_SPI1=y
+CONFIG_STM32_SPI1_DMA=y
 CONFIG_STM32_SPI2=y
 CONFIG_STM32_SPI4=y
+CONFIG_STM32_SPI_DMA=y
 CONFIG_STM32_TIM10=y
 CONFIG_STM32_TIM11=y
 CONFIG_STM32_TIM1=y

diff --git a/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig
@@ -1,3 +1,10 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
 # CONFIG_DISABLE_OS_API is not set
 # CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
 # CONFIG_MMCSD_HAVE_CARDDETECT is not set
@@ -178,8 +185,10 @@ CONFIG_STM32_SDIO_CARD=y
 CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
 CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32_SPI1=y
+CONFIG_STM32_SPI1_DMA=y
 CONFIG_STM32_SPI2=y
 CONFIG_STM32_SPI4=y
+CONFIG_STM32_SPI_DMA=y
 CONFIG_STM32_TIM10=y
 CONFIG_STM32_TIM11=y
 CONFIG_STM32_TIM1=y

diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h
@@ -352,7 +352,7 @@
 #define BOARD_HAS_PWM    DIRECT_PWM_OUTPUT_CHANNELS
 
 /* This board provides a DMA pool and APIs. */
-#define BOARD_DMA_ALLOC_POOL_SIZE    5120
+#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 1008)	// 5120 fat + 512 + 1008 spi
 
 #define BOARD_HAS_ON_RESET 1
 

diff --git a/boards/px4/fmu-v4/src/timer_config.c b/boards/px4/fmu-v4/src/timer_config.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake
@@ -29,11 +29,9 @@ px4_add_board(
 		gps
 		#heater
 		#imu # all available imu drivers
-		imu/bma180
-		imu/bmi160
-		imu/l3gd20
-		imu/mpu6000
 		imu/mpu9250
+		imu/invensense/icm20602
+		imu/invensense/icm20608-g
 		irlock
 		lights/blinkm
 		lights/rgbled

diff --git a/boards/px4/fmu-v4pro/init/rc.board_sensors b/boards/px4/fmu-v4pro/init/rc.board_sensors
@@ -4,10 +4,10 @@
 #------------------------------------------------------------------------------
 
 # Internal SPI bus ICM-20608-G
-mpu6000 -R 2 -T 20608 start
+icm20608-g -R 8 start
 
 # Internal SPI bus ICM-20602
-mpu6000 -R 2 -T 20602 start
+icm20602 -R 8 start
 
 # Internal SPI bus mpu9250
 mpu9250 -R 2 start

diff --git a/boards/px4/fmu-v4pro/nuttx-config/include/board.h b/boards/px4/fmu-v4pro/nuttx-config/include/board.h
@@ -383,6 +383,8 @@
 #define GPIO_SPI1_MOSI  GPIO_SPI1_MOSI_1
 #define GPIO_SPI1_SCK   GPIO_SPI1_SCK_1
 
+#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1
+#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2
 
 #define GPIO_SPI2_MISO  GPIO_SPI2_MISO_1
 #define GPIO_SPI2_MOSI  GPIO_SPI2_MOSI_1

diff --git a/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig
@@ -188,9 +188,11 @@ CONFIG_STM32_SDIO_CARD=y
 CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
 CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32_SPI1=y
+CONFIG_STM32_SPI1_DMA=y
 CONFIG_STM32_SPI2=y
 CONFIG_STM32_SPI5=y
 CONFIG_STM32_SPI6=y
+CONFIG_STM32_SPI_DMA=y
 CONFIG_STM32_TIM10=y
 CONFIG_STM32_TIM11=y
 CONFIG_STM32_TIM1=y

diff --git a/boards/px4/fmu-v4pro/src/board_config.h b/boards/px4/fmu-v4pro/src/board_config.h
@@ -331,9 +331,8 @@
 
 #define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS
 
-/* This board provides a DMA pool and APIs */
-
-#define BOARD_DMA_ALLOC_POOL_SIZE 5120
+/* This board provides a DMA pool and APIs. */
+#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 1008)	// 5120 fat + 512 + 1008 spi
 
 #define BOARD_HAS_ON_RESET 1
 

diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake
@@ -50,9 +50,9 @@ px4_add_board(
 		rc_input
 		#roboclaw
 		safety_button
-		tap_esc
+		#tap_esc
 		telemetry # all available telemetry drivers
-		test_ppm
+		#test_ppm
 		tone_alarm
 		#uavcan
 

diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
@@ -42,8 +42,8 @@ set(msg_files
 	battery_status.msg
 	camera_capture.msg
 	camera_trigger.msg
-	collision_report.msg
 	collision_constraints.msg
+	collision_report.msg
 	commander_state.msg
 	cpuload.msg
 	debug_array.msg
@@ -101,12 +101,20 @@ set(msg_files
 	safety.msg
 	satellite_info.msg
 	sensor_accel.msg
+	sensor_accel_control.msg
+	sensor_accel_fifo.msg
+	sensor_accel_integrated.msg
+	sensor_accel_status.msg
 	sensor_baro.msg
 	sensor_bias.msg
 	sensor_combined.msg
 	sensor_correction.msg
 	sensor_gyro.msg
 	sensor_gyro_control.msg
+	sensor_gyro_fifo.msg
+	sensor_gyro_integrated.msg
+	sensor_gyro_status.msg
+	sensor_imu.msg
 	sensor_mag.msg
 	sensor_preflight.msg
 	sensor_selection.msg

diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg
@@ -7,14 +7,16 @@ float32 x		# acceleration in the NED X board axis in m/s^2
 float32 y		# acceleration in the NED Y board axis in m/s^2
 float32 z		# acceleration in the NED Z board axis in m/s^2
 
-uint32 integral_dt	# integration time  (microseconds)
+uint32 integral_dt	# integration time (microseconds)
+uint8 integral_samples	# number of samples integrated
 float32 x_integral	# delta velocity in the NED X board axis in m/s over the integration time frame (integral_dt)
 float32 y_integral	# delta velocity in the NED Y board axis in m/s over the integration time frame (integral_dt)
 float32 z_integral	# delta velocity in the NED Z board axis in m/s over the integration time frame (integral_dt)
+uint8 integral_clip_count	# total clip count per integration period on any axis
 
 float32 temperature	# temperature in degrees celsius
 
-float32 scaling		# scaling from raw to m/s^s
+float32 scaling		# scaling from raw to m/s^2
 int16 x_raw
 int16 y_raw
 int16 z_raw
diff --git a/msg/sensor_accel_control.msg b/msg/sensor_accel_control.msg
@@ -0,0 +1,11 @@
+
+# WARNING: Not recommend for custom development.
+#  This topic is part of an incremental refactoring of the sensor pipeline and will likely disappear post PX4 release v1.10
+
+uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint64 timestamp	# time since system start (microseconds)
+
+uint64 timestamp_sample	# time the raw data was sampled (microseconds)
+
+float32[3] xyz		# filtered acceleration in the board axis in m/s/s
diff --git a/msg/sensor_accel_fifo.msg b/msg/sensor_accel_fifo.msg
@@ -0,0 +1,13 @@
+uint64 timestamp	# time since system start (microseconds)
+uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint64 timestamp_sample	# time since system start (microseconds)
+
+float32 dt		# delta time between samples (microseconds)
+float32 scale
+
+uint8 samples		# number of valid samples
+
+int16[4] x		# acceleration in the NED X board axis in m/s/s
+int16[4] y		# acceleration in the NED Y board axis in m/s/s
+int16[4] z		# acceleration in the NED Z board axis in m/s/s
diff --git a/msg/sensor_accel_integrated.msg b/msg/sensor_accel_integrated.msg
@@ -0,0 +1,11 @@
+uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp_sample	# time since system start (microseconds)
+
+uint16 dt	# integration time (microseconds)
+uint8 samples	# number of samples integrated
+
+float32[3] delta_velocity	# delta velocity in the NED frame in m/s over the integration time frame (dt)
+
+uint8 clip_count	# total clip count during the integration period across all axes
diff --git a/msg/sensor_accel_status.msg b/msg/sensor_accel_status.msg
@@ -0,0 +1,18 @@
+uint64 timestamp	# time since system start (microseconds)
+uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint64 error_count
+
+float32 temperature
+
+uint8 rotation
+
+# clipping per axis?
+uint64[3] clipping
+
+uint16 measure_rate
+uint16 sample_rate
+
+float32 full_scale_range
+
+float32 high_frequency_vibration # high frequency vibration level in the IMU delta angle data (rad)
diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
@@ -13,3 +13,5 @@ uint32 gyro_integral_dt		# gyro measurement sampling period in us
 int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
 float32[3] accelerometer_m_s2		# average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period
 uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in us
+uint8 accelerometer_integral_samples	# number of integrated samples
+uint8 accelerometer_clip_count		# total accelerometer clip count during the integration period across all axes