ST ISM330DLC IMU driver

PX4 · Nov 22, 2019 · b4d8b78 · b4d8b78
1 parent f271efa
commit b4d8b78
Show file tree

Hide file tree

Showing 29 changed files with 1,688 additions and 101 deletions.
diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake
@@ -48,7 +48,7 @@ px4_add_board(
 		magnetometer/hmc5883
 		#mkblctrl
 		#optical_flow # all available optical flow drivers
-		optical_flow/px4flow
+		#optical_flow/px4flow
 		#osd
 		#pca9685
 		#protocol_splitter

diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake
@@ -32,8 +32,8 @@ px4_add_board(
 		imu/adis16477
 		imu/adis16497
 		imu/bmi088
-# TBD		imu/ism330dlc - needs bus selection
 		imu/mpu6000
+		imu/st/ism330dlc
 		irlock
 		lights/blinkm
 		lights/rgbled

diff --git a/boards/px4/fmu-v5x/src/spi.cpp b/boards/px4/fmu-v5x/src/spi.cpp
@@ -335,9 +335,7 @@ __EXPORT void board_spi_reset(int mask_ms)
 		stm32_configgpio(GPIO_SPI2_SCK_OFF);
 		stm32_configgpio(GPIO_SPI2_MISO_OFF);
 		stm32_configgpio(GPIO_SPI2_MOSI_OFF);
-#if BOARD_USE_DRDY
 		stm32_configgpio(GPIO_DRDY_OFF_SPI2_DRDY1_ISM330);
-#endif
 		/* set the sensor rail off */
 		stm32_gpiowrite(GPIO_VDD_3V3_SENSORS2_EN, 0);
 	}
@@ -412,9 +410,7 @@ __EXPORT void board_spi_reset(int mask_ms)
 		stm32_configgpio(GPIO_SPI2_SCK);
 		stm32_configgpio(GPIO_SPI2_MISO);
 		stm32_configgpio(GPIO_SPI2_MOSI);
-#if BOARD_USE_DRDY
 		stm32_configgpio(GPIO_SPI2_DRDY1_ISM330);
-#endif
 	}
 
 	if (mask & 4) {

diff --git a/cmake/bloaty.cmake b/cmake/bloaty.cmake
@@ -34,7 +34,7 @@
 find_program(BLOATY_PROGRAM bloaty)
 if (BLOATY_PROGRAM)
 
-	set(BLOATY_OPTS --demangle=short --domain=vm -s vm -n 100 -w)
+	set(BLOATY_OPTS --demangle=full --domain=vm -s vm -n 100 -w)
 
 	# bloaty compilation units
 	add_custom_target(bloaty_compileunits

diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
@@ -45,8 +45,8 @@ set(msg_files
 	camera_capture.msg
 	camera_trigger.msg
 	cellular_status.msg
-	collision_report.msg
 	collision_constraints.msg
+	collision_report.msg
 	commander_state.msg
 	cpuload.msg
 	debug_array.msg
@@ -104,12 +104,16 @@ set(msg_files
 	safety.msg
 	satellite_info.msg
 	sensor_accel.msg
+	sensor_accel_fifo.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_status.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_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		# acceleration in the NED X board axis in m/s/s
+int16[8] y		# acceleration in the NED Y board axis in m/s/s
+int16[8] z		# acceleration in the NED Z board axis in m/s/s
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.msg b/msg/sensor_gyro.msg
@@ -1,6 +1,5 @@
-uint32 device_id	# unique device ID for the sensor that does not change between power cycles
-
 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
 
@@ -9,9 +8,11 @@ float32 y		# angular velocity in the NED Y board axis in rad/s
 float32 z		# angular velocity in the NED Z board axis in rad/s
 
 uint32 integral_dt	# integration time (microseconds)
+uint8 integral_samples	# number of samples integrated
 float32 x_integral	# delta angle in the NED X board axis in rad/s over the integration time frame (integral_dt)
 float32 y_integral	# delta angle in the NED Y board axis in rad/s over the integration time frame (integral_dt)
 float32 z_integral	# delta angle in the NED Z board axis in rad/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
 

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[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_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
@@ -265,6 +265,14 @@ rtps:
     id: 116
   - msg: cellular_status
     id: 117
+  - msg: sensor_accel_fifo
+    id: 118
+  - msg: sensor_accel_status
+    id: 119
+  - msg: sensor_gyro_fifo
+    id: 120
+  - msg: sensor_gyro_status
+    id: 121
   ########## 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,13 +50,13 @@ 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 SPI0{"wq:SPI0", 1400, -1};
-static constexpr wq_config_t SPI1{"wq:SPI1", 1400, -2};
-static constexpr wq_config_t SPI2{"wq:SPI2", 1400, -3};
-static constexpr wq_config_t SPI3{"wq:SPI3", 1400, -4};
-static constexpr wq_config_t SPI4{"wq:SPI4", 1400, -5};
-static constexpr wq_config_t SPI5{"wq:SPI5", 1400, -6};
-static constexpr wq_config_t SPI6{"wq:SPI6", 1400, -7};
+static constexpr wq_config_t SPI0{"wq:SPI0", 1600, -1};
+static constexpr wq_config_t SPI1{"wq:SPI1", 1600, -2};
+static constexpr wq_config_t SPI2{"wq:SPI2", 1600, -3};
+static constexpr wq_config_t SPI3{"wq:SPI3", 1600, -4};
+static constexpr wq_config_t SPI4{"wq:SPI4", 1600, -5};
+static constexpr wq_config_t SPI5{"wq:SPI5", 1600, -6};
+static constexpr wq_config_t SPI6{"wq:SPI6", 1600, -7};
 
 static constexpr wq_config_t I2C0{"wq:I2C0", 1400, -8};
 static constexpr wq_config_t I2C1{"wq:I2C1", 1400, -9};

diff --git a/platforms/nuttx/Debug/launch.json.in b/platforms/nuttx/Debug/launch.json.in
@@ -10,6 +10,7 @@
             "type": "cortex-debug",
             "servertype": "jlink",
             "interface": "swd",
+            //"ipAddress": "server:port",
             "cwd": "${workspaceRoot}",
             "internalConsoleOptions": "openOnSessionStart",
             "preLaunchCommands": [

diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h
@@ -46,6 +46,8 @@
 
 #include <board_config.h>
 
+__BEGIN_DECLS
+
 /************************************************************************************
  * Name: board_dma_alloc_init
  *
@@ -114,7 +116,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
 
 /************************************************************************************
@@ -131,5 +133,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/distance_sensor/tfmini/CMakeLists.txt b/src/drivers/distance_sensor/tfmini/CMakeLists.txt
@@ -40,4 +40,6 @@ px4_add_module(
 		tfmini_parser.cpp
 	MODULE_CONFIG
 		module.yaml
+	DEPENDS
+		drivers_rangefinder
 	)
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
@@ -116,6 +116,7 @@
 #define DRV_BARO_DEVTYPE_BAROSIM	0x65
 #define DRV_DEVTYPE_BMI088		0x66
 #define DRV_DEVTYPE_BMP388		0x67
+#define DRV_DEVTYPE_ST_ISM330DLC	0x68
 
 /*
  * ioctl() definitions

diff --git a/src/drivers/imu/st/ism330dlc/CMakeLists.txt b/src/drivers/imu/st/ism330dlc/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__st__ism330dlc
+	MAIN ism330dlc
+	SRCS
+		ism330dlc_main.cpp
+		ISM330DLC.cpp
+		ISM330DLC.hpp
+		ST_ISM330DLC_Registers.hpp
+	DEPENDS
+		drivers_accelerometer
+		drivers_gyroscope
+		px4_work_queue
+	)