ICM20602: 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 · Oct 6, 2019 · 92a81bb · 92a81bb
1 parent 941a325
commit 92a81bb
Show file tree

Hide file tree

Showing 33 changed files with 1,786 additions and 94 deletions.
diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake
@@ -25,7 +25,10 @@ px4_add_board(
 		distance_sensor # all available distance sensor drivers
 		gps
 		heater
-		imu # all available imu drivers
+		#imu # all available imu drivers
+		imu/icm20602
+		imu/mpu6000
+		imu/mpu9250
 		irlock
 		lights/blinkm
 		lights/rgbled
@@ -46,6 +49,7 @@ px4_add_board(
 		uavcan
 
 	MODULES
+		airspeed_selector
 		attitude_estimator_q
 		camera_feedback
 		commander
@@ -54,7 +58,6 @@ px4_add_board(
 		events
 		fw_att_control
 		fw_pos_control_l1
-		rover_pos_control
 		land_detector
 		landing_target_estimator
 		load_mon
@@ -64,11 +67,11 @@ px4_add_board(
 		mc_att_control
 		mc_pos_control
 		navigator
+		rover_pos_control
 		sensors
 		sih
 		vmount
 		vtol_att_control
-		airspeed_selector
 
 	SYSTEMCMDS
 		bl_update

diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors
@@ -48,7 +48,7 @@ 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 2 start
 then
 	# ICM20608 internal SPI bus ICM-20602-G is rotated 90 deg yaw
 	if ! mpu6000 -R 2 -T 20608 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/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 + 1024)	// 5120 fat + 1024 spi
 
 #define BOARD_HAS_ON_RESET 1
 __BEGIN_DECLS

diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake
@@ -21,8 +21,8 @@ px4_add_board(
 		adc
 		barometer # all available barometer drivers
 		batt_smbus
-		camera_capture
-		camera_trigger
+		#camera_capture
+		#camera_trigger
 		differential_pressure # all available differential pressure drivers
 		distance_sensor # all available distance sensor drivers
 		gps

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
@@ -100,12 +100,19 @@ 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_mag.msg
 	sensor_preflight.msg
 	sensor_selection.msg

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 angular velocity in the board axis in rad/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[8] x		# angular velocity in the NED X board axis in rad/s
+int16[8] y		# angular velocity in the NED Y board axis in rad/s
+int16[8] z		# angular velocity in the NED Z board axis in rad/s
diff --git a/msg/sensor_accel_integrated.msg b/msg/sensor_accel_integrated.msg
@@ -0,0 +1,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 since system start (microseconds)
+
+uint16 dt	# integration time (microseconds)
+
+float32[3] delta_velocity	# delta velocity in the NED frame in m/s over the integration time frame (dt)
+
+bool clipping
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_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg
@@ -0,0 +1,13 @@
+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)
+
+float32 dt		# delta time between samples (microseconds)
+float32 scale
+
+uint8 samples		# number of valid samples
+
+int16[32] x		# angular velocity in the NED X board axis in rad/s
+int16[32] y		# angular velocity in the NED Y board axis in rad/s
+int16[32] z		# angular velocity in the NED Z board axis in rad/s
diff --git a/msg/sensor_gyro_integrated.msg b/msg/sensor_gyro_integrated.msg
@@ -0,0 +1,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 since system start (microseconds)
+
+uint16 dt	# integration time (microseconds)
+
+float32[3] delta_angle	# delta angle in the NED frame in rad/s over the integration time frame (integral_dt)
+
+bool clipping
diff --git a/msg/sensor_gyro_status.msg b/msg/sensor_gyro_status.msg
@@ -0,0 +1,20 @@
+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 coning_vibration	# Level of coning vibration in the IMU delta angles (rad^2)
+
+float32 high_frequency_vibration # high frequency vibration level in the IMU delta angle data (rad)
diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml
@@ -258,10 +258,24 @@ rtps:
     id: 112
   - msg: vehicle_acceleration
     id: 113
-  - msg: sensor_gyro_control
-    id: 114
   - msg: airspeed_validated
+    id: 114
+  - msg: sensor_accel_control
     id: 115
+  - msg: sensor_accel_fifo
+    id: 116
+  - msg: sensor_accel_integrated
+    id: 117
+  - msg: sensor_accel_status
+    id: 118
+  - msg: sensor_gyro_control
+    id: 119
+  - msg: sensor_gyro_fifo
+    id: 120
+  - msg: sensor_gyro_integrated
+    id: 121
+  - msg: sensor_gyro_status
+    id: 122
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
@@ -50,7 +50,7 @@ namespace wq_configurations
 {
 static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority
 
-static constexpr wq_config_t SPI1{"wq:SPI1", 1400, -1};
+static constexpr wq_config_t SPI1{"wq:SPI1", 2100, -1};
 static constexpr wq_config_t SPI2{"wq:SPI2", 1400, -2};
 static constexpr wq_config_t SPI3{"wq:SPI3", 1400, -3};
 static constexpr wq_config_t SPI4{"wq:SPI4", 1400, -4};
@@ -65,7 +65,7 @@ static constexpr wq_config_t I2C4{"wq:I2C4", 1250, -10};
 static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 6600, -11}; // PX4 att/pos controllers, highest priority after sensors
 
 static constexpr wq_config_t hp_default{"wq:hp_default", 1500, -12};
-static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50};
+static constexpr wq_config_t lp_default{"wq:lp_default", 2000, -50};
 
 static constexpr wq_config_t test1{"wq:test1", 800, 0};
 static constexpr wq_config_t test2{"wq:test2", 800, 0};

diff --git a/src/drivers/boards/common/board_dma_alloc.h b/src/drivers/boards/common/board_dma_alloc.h
@@ -44,6 +44,8 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+__BEGIN_DECLS
+
 /************************************************************************************
  * Name: board_dma_alloc_init
  *
@@ -112,7 +114,7 @@ __EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16
 #if defined(BOARD_DMA_ALLOC_POOL_SIZE)
 __EXPORT void *board_dma_alloc(size_t size);
 #else
-#define board_dma_alloc(size) (NULL)
+#define board_dma_alloc(size) malloc(size)
 #endif
 
 /************************************************************************************
@@ -129,5 +131,7 @@ __EXPORT void *board_dma_alloc(size_t size);
 #if defined(BOARD_DMA_ALLOC_POOL_SIZE)
 __EXPORT void board_dma_free(FAR void *memory, size_t size);
 #else
-#define board_dma_free(memory, size) ()
+#define board_dma_free(memory, size) free(memory)
 #endif
+
+__END_DECLS
diff --git a/src/drivers/imu/CMakeLists.txt b/src/drivers/imu/CMakeLists.txt
@@ -39,6 +39,7 @@ add_subdirectory(bmi055)
 add_subdirectory(bmi160)
 add_subdirectory(fxas21002c)
 add_subdirectory(fxos8701cq)
+add_subdirectory(icm20602)
 add_subdirectory(icm20948)
 add_subdirectory(l3gd20)
 add_subdirectory(lsm303d)

diff --git a/src/drivers/imu/icm20602/CMakeLists.txt b/src/drivers/imu/icm20602/CMakeLists.txt
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__imu__icm20602
+	MAIN icm20602
+	COMPILE_FLAGS
+	SRCS
+		ICM20602.cpp
+		ICM20602.hpp
+		icm20602_main.cpp
+	DEPENDS
+		drivers_accelerometer
+		drivers_gyroscope
+		px4_work_queue
+	)