From 2c6669b535dafaf50132cbd6fa0aae91bc8c1879 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Nils=20Forss=C3=A9n?= Date: Thu, 5 Jun 2025 18:11:57 +0200 Subject: [PATCH] imu added, simple integration example working --- .gitignore | 3 +- README.md | 3 + build.sh | 2 - cmake_build.sh | 3 + processes/click_reader/6dofimu17.c | 330 +++++++++++++++++++++ processes/click_reader/CMakeLists.txt | 2 +- processes/click_reader/include/6dofimu17.h | 321 ++++++++++++++++++++ processes/click_reader/main.cpp | 142 ++++++++- processes/include/c_util.h | 4 + processes/include/util.h | 4 +- 10 files changed, 794 insertions(+), 20 deletions(-) create mode 100755 cmake_build.sh create mode 100644 processes/click_reader/6dofimu17.c create mode 100644 processes/click_reader/include/6dofimu17.h diff --git a/.gitignore b/.gitignore index bd43a13..848c76e 100755 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,3 @@ build/ bin/ -.vscode -WiringPi-3.6/ \ No newline at end of file +.vscode \ No newline at end of file diff --git a/README.md b/README.md index 634baf0..694d118 100755 --- a/README.md +++ b/README.md @@ -25,3 +25,6 @@ https://github.com/YuechengLi/BMP280_driver (deprecated and not very good, shoul just run wvdial +# TODO + +Fix the spi functions in pressure4. \ No newline at end of file diff --git a/build.sh b/build.sh index ac8ad99..0f588ab 100755 --- a/build.sh +++ b/build.sh @@ -1,6 +1,4 @@ #!/usr/bin/bash cd $(dirname "$0")/build -cmake -G Ninja .. - ninja \ No newline at end of file diff --git a/cmake_build.sh b/cmake_build.sh new file mode 100755 index 0000000..1dc1804 --- /dev/null +++ b/cmake_build.sh @@ -0,0 +1,3 @@ +cd $(dirname "$0")/build +cmake -G Ninja . +ninja \ No newline at end of file diff --git a/processes/click_reader/6dofimu17.c b/processes/click_reader/6dofimu17.c new file mode 100644 index 0000000..ff9a660 --- /dev/null +++ b/processes/click_reader/6dofimu17.c @@ -0,0 +1,330 @@ +/**************************************************************************** +** Copyright (C) 2020 MikroElektronika d.o.o. +** Contact: https://www.mikroe.com/contact +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** The above copyright notice and this permission notice shall be +** included in all copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +** EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES +** OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +** IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +** DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT +** OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +** USE OR OTHER DEALINGS IN THE SOFTWARE. +****************************************************************************/ + +/*! + * @file c6dofimu17.c + * @brief 6DOF IMU 17 Click Driver. + */ + +#include "6dofimu17.h" + +#include + +/** + * @brief Dummy data. + * @details Definition of dummy data. + */ +#define DUMMY 0x00 +#define SPI_READ_MASK 0x80 +#define BIT_MASK_BIT_0 0x01 +#define BIT_MASK_BIT_1 0x02 +#define BIT_MASK_BIT_2 0x04 +#define BIT_MASK_BIT_3 0x08 +#define BIT_MASK_BIT_4 0x10 +#define BIT_MASK_BIT_5 0x20 +#define BIT_MASK_BIT_6 0x40 +#define BIT_MASK_BIT_7 0x80 + + +static void c6dofimu17_config_delay ( void ) ; + +static c6dofimu17_t ctx; + +err_t c6dofimu17_init_comm ( c6dofimu17_master_io_t spi_read_f, c6dofimu17_master_io_t spi_write_f) +{ + ctx.read_f = spi_read_f; + ctx.write_f = spi_write_f; + c6dofimu17_gyro_cfg_t gyro_cfg; + c6dofimu17_accel_cfg_t accel_cfg; + + return OK; +} + +err_t c6dofimu17_cfg ( c6dofimu17_gyro_cfg_t *gyro_cfg, c6dofimu17_accel_cfg_t *accel_cfg ) +{ + c6dofimu17_soft_reset( ); + c6dofimu17_config_delay( ); + + c6dofimu17_temperature_enable( ); + c6dofimu17_config_delay( ); + + c6dofimu17_gyro_enable( ); + c6dofimu17_config_delay( ); + + c6dofimu17_accel_enable( ); + c6dofimu17_config_delay( ); + + + c6dofimu17_set_config_gyro( *gyro_cfg ); + c6dofimu17_config_delay( ); + + c6dofimu17_set_config_accel( *accel_cfg ); + c6dofimu17_config_delay( ); + + return OK; +} + +err_t c6dofimu17_generic_write ( uint8_t reg, uint8_t *data_in, uint8_t len ) +{ + return ctx.write_f( reg, data_in, len ); +} + +err_t c6dofimu17_generic_read ( uint8_t reg, uint8_t *data_out, uint8_t len ) +{ + return ctx.read_f( reg, data_out, len ); +} + +err_t c6dofimu17_bank_selection ( uint8_t bank_sel ) +{ + if ( bank_sel >= C6DOFIMU17_SET_BANK_4 ) + { + return ERR; + } + return c6dofimu17_generic_write( C6DOFIMU17_REG_BANK_SEL, &bank_sel, 1 ); +} + +err_t c6dofimu17_get_device_id ( uint8_t *device_id ) +{ + return c6dofimu17_generic_read( C6DOFIMU17_REG_WHO_AM_I, device_id, 1 ); +} + +err_t c6dofimu17_temperature_enable ( void ) +{ + uint8_t tmp; + + err_t err_flag = c6dofimu17_generic_read( C6DOFIMU17_REG_PWR_MGMT0, &tmp, 1 ); + + tmp &= ~C6DOFIMU17_SET_TEMPERATURE_DISABLED; + + err_flag |= c6dofimu17_generic_write( C6DOFIMU17_REG_PWR_MGMT0, &tmp, 1 ); + + return err_flag; +} + +err_t c6dofimu17_gyro_enable ( void ) +{ + uint8_t tmp; + + err_t err_flag = c6dofimu17_generic_read( C6DOFIMU17_REG_PWR_MGMT0, &tmp, 1 ); + + tmp |= C6DOFIMU17_SET_GYRO_TLOW_NOISE_MODE; + + err_flag |= c6dofimu17_generic_write( C6DOFIMU17_REG_PWR_MGMT0, &tmp, 1 ); + + return err_flag; +} + +err_t c6dofimu17_accel_enable ( void ) +{ + uint8_t tmp; + + err_t err_flag = c6dofimu17_generic_read( C6DOFIMU17_REG_PWR_MGMT0, &tmp, 1 ); + + tmp |= C6DOFIMU17_SET_ACCEL_LOW_NOISE_MODE; + + err_flag |= c6dofimu17_generic_write( C6DOFIMU17_REG_PWR_MGMT0, &tmp, 1 ); + + return err_flag; +} + +err_t c6dofimu17_set_config_gyro ( c6dofimu17_gyro_cfg_t gyro_cfg ) +{ + uint8_t tmp; + + tmp = ( gyro_cfg.gyro_fs_sel & ( BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ) ) << 5; + tmp |= gyro_cfg.gyro_odr & ( BIT_MASK_BIT_3 | BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + + err_t err_flag = c6dofimu17_generic_write( C6DOFIMU17_REG_GYRO_CONFIG0, &tmp, 1 ); + + tmp = ( gyro_cfg.gyro_ui_filt_ord & ( BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ) ) << 2; + tmp |= gyro_cfg.gyro_dec2_m2_ord & ( BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + + err_flag |= c6dofimu17_generic_write( C6DOFIMU17_REG_ACCEL_CONFIG1, &tmp, 1 ); + + err_flag |= c6dofimu17_generic_read( C6DOFIMU17_REG_GYRO_ACCEL_CONFIG0, &tmp, 1 ); + + tmp |= gyro_cfg.gyro_ui_filt_bw & ( BIT_MASK_BIT_3 | BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + + err_flag |= c6dofimu17_generic_write( C6DOFIMU17_REG_GYRO_ACCEL_CONFIG0, &tmp, 1 ); + + return err_flag; +} + +err_t c6dofimu17_get_config_gyro ( c6dofimu17_gyro_cfg_t *gyro_cfg ) +{ + uint8_t tmp; + + err_t err_flag = c6dofimu17_generic_read( C6DOFIMU17_REG_GYRO_CONFIG0, &tmp, 1 ); + + gyro_cfg->gyro_fs_sel = ( tmp >> 5 ) & ( BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + gyro_cfg->gyro_odr = tmp & ( BIT_MASK_BIT_3 | BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + + err_flag |= c6dofimu17_generic_read( C6DOFIMU17_REG_ACCEL_CONFIG1, &tmp, 1 ); + + gyro_cfg->gyro_ui_filt_ord = ( tmp >> 2 ) & ( BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + gyro_cfg->gyro_dec2_m2_ord = tmp & ( BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + + err_flag |= c6dofimu17_generic_read( C6DOFIMU17_REG_GYRO_ACCEL_CONFIG0, &tmp, 1 ); + + gyro_cfg->gyro_dec2_m2_ord = tmp & ( BIT_MASK_BIT_3 | BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + + return err_flag; +} + +err_t c6dofimu17_set_config_accel ( c6dofimu17_accel_cfg_t accel_cfg ) +{ + uint8_t tmp; + + tmp = ( accel_cfg.accel_fs_sel & ( BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ) ) << 5; + tmp |= accel_cfg.accel_odr & ( BIT_MASK_BIT_3 | BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + + err_t err_flag = c6dofimu17_generic_write( C6DOFIMU17_REG_ACCEL_CONFIG0, &tmp, 1 ); + + err_flag |= c6dofimu17_generic_read( C6DOFIMU17_REG_GYRO_ACCEL_CONFIG0, &tmp, 1 ); + + tmp |= ( accel_cfg.accel_ui_filt_bw & ( BIT_MASK_BIT_3 | BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ) ) << 4; + + err_flag |= c6dofimu17_generic_write( C6DOFIMU17_REG_GYRO_ACCEL_CONFIG0, &tmp, 1 ); + + tmp = ( accel_cfg.accel_ui_filt_ord & ( BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ) ) << 3; + tmp |= ( accel_cfg.accel_dec2_m2_ord & ( BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ) ) << 1; + + err_flag |= c6dofimu17_generic_write( C6DOFIMU17_REG_ACCEL_CONFIG1, &tmp, 1 ); + + return err_flag; +} + +err_t c6dofimu17_get_config_accel ( c6dofimu17_accel_cfg_t *accel_cfg ) +{ + uint8_t tmp; + + err_t err_flag = c6dofimu17_generic_read( C6DOFIMU17_REG_ACCEL_CONFIG0, &tmp, 1 ); + + accel_cfg->accel_fs_sel = ( tmp >> 5 ) & ( BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + accel_cfg->accel_odr = tmp & ( BIT_MASK_BIT_3 | BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + + err_flag |= c6dofimu17_generic_read( C6DOFIMU17_REG_GYRO_ACCEL_CONFIG0, &tmp, 1 ); + + accel_cfg->accel_ui_filt_bw = ( tmp >> 4 ) & ( BIT_MASK_BIT_3 | BIT_MASK_BIT_2 | BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + + err_flag |= c6dofimu17_generic_read( C6DOFIMU17_REG_ACCEL_CONFIG1, &tmp, 1 ); + + accel_cfg->accel_ui_filt_ord = ( tmp << 3 ) & ( BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + accel_cfg->accel_dec2_m2_ord = ( tmp << 1 ) & ( BIT_MASK_BIT_1 | BIT_MASK_BIT_0 ); + + return err_flag; +} + +err_t c6dofimu17_soft_reset ( void ) +{ + uint8_t tmp; + + err_t err_flag = c6dofimu17_generic_read( C6DOFIMU17_REG_DEVICE_CONFIG, &tmp, 1 ); + + tmp |= 0x01; + + err_flag |= c6dofimu17_generic_write( C6DOFIMU17_REG_DEVICE_CONFIG, &tmp, 1 ); + c6dofimu17_config_delay( ) ; + + return err_flag; +} + +err_t c6dofimu17_get_accel_data ( c6dofimu17_axis_t *accel_data ) +{ + uint8_t rx_buf[ 6 ]; + uint16_t tmp; + + err_t err_flag = c6dofimu17_generic_read( C6DOFIMU17_REG_ACCEL_DATA_X1_UI, rx_buf, 6 ); + + tmp = rx_buf[ 0 ]; + tmp <<= 8; + tmp |= rx_buf[ 1 ]; + + accel_data->x = ( int16_t ) tmp; + + tmp = rx_buf[ 2 ]; + tmp <<= 8; + tmp |= rx_buf[ 3 ]; + + accel_data->y = ( int16_t ) tmp; + + tmp = rx_buf[ 4 ]; + tmp <<= 8; + tmp |= rx_buf[ 5 ]; + + accel_data->z = ( int16_t ) tmp; + + return err_flag; +} + +err_t c6dofimu17_get_gyro_data ( c6dofimu17_axis_t *gyro_data ) +{ + uint8_t rx_buf[ 6 ]; + uint16_t tmp; + + err_t err_flag = c6dofimu17_generic_read( C6DOFIMU17_REG_GYRO_DATA_X1_UI, rx_buf, 6 ); + + tmp = rx_buf[ 0 ]; + tmp <<= 8; + tmp |= rx_buf[ 1 ]; + + gyro_data->x = ( int16_t ) tmp; + + tmp = rx_buf[ 2 ]; + tmp <<= 8; + tmp |= rx_buf[ 3 ]; + + gyro_data->y = ( int16_t ) tmp; + + tmp = rx_buf[ 4 ]; + tmp <<= 8; + tmp |= rx_buf[ 5 ]; + + gyro_data->z = ( int16_t ) tmp; + + return err_flag; +} + +err_t c6dofimu17_get_temperature ( float *temperature ) +{ + uint8_t rx_buf[ 2 ]; + int16_t tmp; + + err_t err_flag = c6dofimu17_generic_read( C6DOFIMU17_REG_TEMP_DATA1_UI, rx_buf, 2 ); + + tmp = rx_buf[ 0 ]; + tmp <<= 8; + tmp |= rx_buf[ 1 ]; + + *temperature = ( float ) tmp; + *temperature /= 132.48; + *temperature += 25; + + return err_flag; +} + +static void c6dofimu17_config_delay ( void ) +{ + usleep(10 * 1000); // 10 ms +} + +// ------------------------------------------------------------------------ END \ No newline at end of file diff --git a/processes/click_reader/CMakeLists.txt b/processes/click_reader/CMakeLists.txt index d76aadc..5b70712 100644 --- a/processes/click_reader/CMakeLists.txt +++ b/processes/click_reader/CMakeLists.txt @@ -1,5 +1,6 @@ add_executable(click_reader main.cpp + 6dofimu17.c ) target_include_directories(click_reader PRIVATE include) @@ -7,7 +8,6 @@ target_include_directories(click_reader PRIVATE include) target_link_libraries(click_reader PRIVATE MAVLink) target_link_libraries(click_reader PRIVATE spdlog::spdlog) - target_link_libraries(click_reader PRIVATE ${BCM2835_LIB}) target_include_directories(click_reader PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include) diff --git a/processes/click_reader/include/6dofimu17.h b/processes/click_reader/include/6dofimu17.h new file mode 100644 index 0000000..51802c4 --- /dev/null +++ b/processes/click_reader/include/6dofimu17.h @@ -0,0 +1,321 @@ +/**************************************************************************** +** Copyright (C) 2020 MikroElektronika d.o.o. +** Contact: https://www.mikroe.com/contact +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** The above copyright notice and this permission notice shall be +** included in all copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +** EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES +** OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +** IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +** DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT +** OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +** USE OR OTHER DEALINGS IN THE SOFTWARE. +****************************************************************************/ + +/*! + * @file c6dofimu17.h + * @brief This file contains API for 6DOF IMU 17 Click Driver. + */ + +#ifndef C6DOFIMU17_H +#define C6DOFIMU17_H + +#ifdef __cplusplus +extern "C"{ +#endif + +// Register definitions +#define C6DOFIMU17_REG_DEVICE_CONFIG 0x11 +#define C6DOFIMU17_REG_DRIVE_CONFIG 0x13 +#define C6DOFIMU17_REG_INT_CONFIG 0x14 +#define C6DOFIMU17_REG_FIFO_CONFIG 0x16 +#define C6DOFIMU17_REG_TEMP_DATA1_UI 0x1D +#define C6DOFIMU17_REG_TEMP_DATA0_UI 0x1E +#define C6DOFIMU17_REG_ACCEL_DATA_X1_UI 0x1F +#define C6DOFIMU17_REG_ACCEL_DATA_X0_UI 0x20 +#define C6DOFIMU17_REG_ACCEL_DATA_Y1_UI 0x21 +#define C6DOFIMU17_REG_ACCEL_DATA_Y0_UI 0x22 +#define C6DOFIMU17_REG_ACCEL_DATA_Z1_UI 0x23 +#define C6DOFIMU17_REG_ACCEL_DATA_Z0_UI 0x24 +#define C6DOFIMU17_REG_GYRO_DATA_X1_UI 0x25 +#define C6DOFIMU17_REG_GYRO_DATA_X0_UI 0x26 +#define C6DOFIMU17_REG_GYRO_DATA_Y1_UI 0x27 +#define C6DOFIMU17_REG_GYRO_DATA_Y0_UI 0x28 +#define C6DOFIMU17_REG_GYRO_DATA_Z1_UI 0x29 +#define C6DOFIMU17_REG_GYRO_DATA_Z0_UI 0x2A +#define C6DOFIMU17_REG_TMST_FSYNCH 0x2B +#define C6DOFIMU17_REG_TMST_FSYNCL 0x2C +#define C6DOFIMU17_REG_INT_STATUS 0x2D +#define C6DOFIMU17_REG_FIFO_COUNTH 0x2E +#define C6DOFIMU17_REG_FIFO_COUNTL 0x2F +#define C6DOFIMU17_REG_FIFO_DATA 0x30 +#define C6DOFIMU17_REG_APEX_DATA0 0x31 +#define C6DOFIMU17_REG_APEX_DATA1 0x32 +#define C6DOFIMU17_REG_APEX_DATA2 0x33 +#define C6DOFIMU17_REG_APEX_DATA3 0x34 +#define C6DOFIMU17_REG_APEX_DATA4 0x35 +#define C6DOFIMU17_REG_APEX_DATA5 0x36 +#define C6DOFIMU17_REG_INT_STATUS2 0x37 +#define C6DOFIMU17_REG_INT_STATUS3 0x38 +#define C6DOFIMU17_REG_SIGNAL_PATH_RESET 0x4B +#define C6DOFIMU17_REG_INTF_CONFIG0 0x4C +#define C6DOFIMU17_REG_INTF_CONFIG1 0x4D +#define C6DOFIMU17_REG_PWR_MGMT0 0x4E +#define C6DOFIMU17_REG_GYRO_CONFIG0 0x4F +#define C6DOFIMU17_REG_ACCEL_CONFIG0 0x50 +#define C6DOFIMU17_REG_GYRO_CONFIG1 0x51 +#define C6DOFIMU17_REG_GYRO_ACCEL_CONFIG0 0x52 +#define C6DOFIMU17_REG_ACCEL_CONFIG1 0x53 +#define C6DOFIMU17_REG_TMST_CONFIG 0x54 +#define C6DOFIMU17_REG_APEX_CONFIG0 0x55 +#define C6DOFIMU17_REG_SMD_CONFIG 0x56 +#define C6DOFIMU17_REG_FIFO_CONFIG1 0x5F +#define C6DOFIMU17_REG_FIFO_CONFIG2 0x60 +#define C6DOFIMU17_REG_FIFO_CONFIG3 0x61 +#define C6DOFIMU17_REG_FSYNC_CONFIG 0x62 +#define C6DOFIMU17_REG_INT_CONFIG0 0x63 +#define C6DOFIMU17_REG_INT_CONFIG1 0x64 +#define C6DOFIMU17_REG_INT_SOURCE0 0x65 +#define C6DOFIMU17_REG_INT_SOURCE1 0x66 +#define C6DOFIMU17_REG_INT_SOURCE3 0x68 +#define C6DOFIMU17_REG_INT_SOURCE4 0x69 +#define C6DOFIMU17_REG_FIFO_LOST_PKT0 0x6C +#define C6DOFIMU17_REG_FIFO_LOST_PKT1 0x6D +#define C6DOFIMU17_REG_SELF_TEST_CONFIG 0x70 +#define C6DOFIMU17_REG_WHO_AM_I 0x75 +#define C6DOFIMU17_REG_BANK_SEL 0x76 +#define C6DOFIMU17_CHIP_ID 0x6F + +#define C6DOFIMU17_REG_SENSOR_CONFIG0 0x03 +#define C6DOFIMU17_REG_GYRO_CONFIG_STATIC2 0x0B +#define C6DOFIMU17_REG_GYRO_CONFIG_STATIC3 0x0C +#define C6DOFIMU17_REG_GYRO_CONFIG_STATIC4 0x0D +#define C6DOFIMU17_REG_GYRO_CONFIG_STATIC5 0x0E +#define C6DOFIMU17_REG_GYRO_CONFIG_STATIC6 0x0F +#define C6DOFIMU17_REG_GYRO_CONFIG_STATIC7 0x10 +#define C6DOFIMU17_REG_GYRO_CONFIG_STATIC8 0x11 +#define C6DOFIMU17_REG_GYRO_CONFIG_STATIC9 0x12 +#define C6DOFIMU17_REG_GYRO_CONFIG_STATIC10 0x13 +#define C6DOFIMU17_REG_XG_ST_DATA 0x5F +#define C6DOFIMU17_REG_YG_ST_DATA 0x60 +#define C6DOFIMU17_REG_ZG_ST_DATA 0x61 +#define C6DOFIMU17_REG_TMSTVAL0 0x62 +#define C6DOFIMU17_REG_TMSTVAL1 0x63 +#define C6DOFIMU17_REG_TMSTVAL2 0x64 +#define C6DOFIMU17_REG_INTF_CONFIG4 0x7A +#define C6DOFIMU17_REG_INTF_CONFIG5 0x7B +#define C6DOFIMU17_REG_INTF_CONFIG6 0x7C + +#define C6DOFIMU17_REG_ACCEL_CONFIG_STATIC2 0x03 +#define C6DOFIMU17_REG_ACCEL_CONFIG_STATIC3 0x04 +#define C6DOFIMU17_REG_ACCEL_CONFIG_STATIC4 0x05 +#define C6DOFIMU17_REG_XA_ST_DATA 0x3B +#define C6DOFIMU17_REG_YA_ST_DATA 0x3C +#define C6DOFIMU17_REG_ZA_ST_DATA 0x3D + +#define C6DOFIMU17_REG_PU_PD_CONFIG1 0x06 +#define C6DOFIMU17_REG_PU_PD_CONFIG2 0x0E + +#define C6DOFIMU17_REG_FDR_CONFIG 0x09 +#define C6DOFIMU17_REG_APEX_CONFIG1 0x40 +#define C6DOFIMU17_REG_APEX_CONFIG2 0x41 +#define C6DOFIMU17_REG_APEX_CONFIG3 0x42 +#define C6DOFIMU17_REG_APEX_CONFIG4 0x43 +#define C6DOFIMU17_REG_APEX_CONFIG5 0x44 +#define C6DOFIMU17_REG_APEX_CONFIG6 0x45 +#define C6DOFIMU17_REG_APEX_CONFIG7 0x46 +#define C6DOFIMU17_REG_APEX_CONFIG8 0x47 +#define C6DOFIMU17_REG_APEX_CONFIG9 0x48 +#define C6DOFIMU17_REG_APEX_CONFIG10 0x49 +#define C6DOFIMU17_REG_ACCEL_WOM_X_THR 0x4A +#define C6DOFIMU17_REG_ACCEL_WOM_Y_THR 0x4B +#define C6DOFIMU17_REG_ACCEL_WOM_Z_THR 0x4C +#define C6DOFIMU17_REG_INT_SOURCE6 0x4D +#define C6DOFIMU17_REG_INT_SOURCE7 0x4E +#define C6DOFIMU17_REG_INT_SOURCE8 0x4F +#define C6DOFIMU17_REG_INT_SOURCE9 0x50 +#define C6DOFIMU17_REG_INT_SOURCE10 0x51 +#define C6DOFIMU17_REG_OFFSET_USER0 0x77 +#define C6DOFIMU17_REG_OFFSET_USER1 0x78 +#define C6DOFIMU17_REG_OFFSET_USER2 0x79 +#define C6DOFIMU17_REG_OFFSET_USER3 0x7A +#define C6DOFIMU17_REG_OFFSET_USER4 0x7B +#define C6DOFIMU17_REG_OFFSET_USER5 0x7C +#define C6DOFIMU17_REG_OFFSET_USER6 0x7D +#define C6DOFIMU17_REG_OFFSET_USER7 0x7E +#define C6DOFIMU17_REG_OFFSET_USER8 0x7F + +#define C6DOFIMU17_SET_TEMPERATURE_ENABLED 0xDF +#define C6DOFIMU17_SET_TEMPERATURE_DISABLED 0x20 + +#define C6DOFIMU17_SET_GYRO_OFF_MODE 0x00 +#define C6DOFIMU17_SET_GYRO_STANDBY_MODE 0x04 +#define C6DOFIMU17_SET_GYRO_TLOW_NOISE_MODE 0x0C + +#define C6DOFIMU17_SET_ACCEL_OFF_MODE 0x00 +#define C6DOFIMU17_SET_ACCEL_LOW_POWER_MODE 0x02 +#define C6DOFIMU17_SET_ACCEL_LOW_NOISE_MODE 0x03 + +#define C6DOFIMU17_SET_GYRO_FS_SEL_2000_dps 0x00 +#define C6DOFIMU17_SET_GYRO_FS_SEL_1000_dps 0x01 +#define C6DOFIMU17_SET_GYRO_FS_SEL_500_dps 0x02 +#define C6DOFIMU17_SET_GYRO_FS_SEL_250_dps 0x03 +#define C6DOFIMU17_SET_GYRO_FS_SEL_125_dps 0x04 +#define C6DOFIMU17_SET_GYRO_FS_SEL_62_5_dps 0x05 +#define C6DOFIMU17_SET_GYRO_FS_SEL_31_25_dps 0x06 +#define C6DOFIMU17_SET_GYRO_FS_SEL_16_625_dps 0x07 + +#define C6DOFIMU17_SET_GYRO_ODR_32kHz 0x01 +#define C6DOFIMU17_SET_GYRO_ODR_16kHz 0x02 +#define C6DOFIMU17_SET_GYRO_ODR_8kHz 0x03 +#define C6DOFIMU17_SET_GYRO_ODR_4kHz 0x04 +#define C6DOFIMU17_SET_GYRO_ODR_2kHz 0x05 +#define C6DOFIMU17_SET_GYRO_ODR_1kHz 0x06 +#define C6DOFIMU17_SET_GYRO_ODR_200Hz 0x07 +#define C6DOFIMU17_SET_GYRO_ODR_100Hz 0x08 +#define C6DOFIMU17_SET_GYRO_ODR_50Hz 0x09 +#define C6DOFIMU17_SET_GYRO_ODR_25Hz 0x0A +#define C6DOFIMU17_SET_GYRO_ODR_12_5Hz 0x0B + +#define C6DOFIMU17_SET_GYRO_UI_FILT_ORD_1st 0x00 +#define C6DOFIMU17_SET_GYRO_UI_FILT_ORD_2st 0x01 +#define C6DOFIMU17_SET_GYRO_UI_FILT_ORD_3st 0x02 + +#define C6DOFIMU17_SET_GYRO_DEC2_M2_ORD_3st 0x02 + +#define C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_2 0x00 +#define C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_4 0x01 +#define C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_5 0x02 +#define C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_8 0x03 +#define C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_10 0x04 +#define C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_16 0x05 +#define C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_20 0x06 +#define C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_40 0x07 +#define C6DOFIMU17_SET_GYRO_UI_FILT_BW_LOW_LATENCY_0 0x0E +#define C6DOFIMU17_SET_GYRO_UI_FILT_BW_LOW_LATENCY_1 0x0F + +#define C6DOFIMU17_SET_ACCEL_FS_SEL_16g 0x00 +#define C6DOFIMU17_SET_ACCEL_FS_SEL_8g 0x01 +#define C6DOFIMU17_SET_ACCEL_FS_SEL_4g 0x02 +#define C6DOFIMU17_SET_ACCEL_FS_SEL_2g 0x03 + +#define C6DOFIMU17_SET_ACCEL_ODR_32kHz 0x01 +#define C6DOFIMU17_SET_ACCEL_ODR_16kHz 0x02 +#define C6DOFIMU17_SET_ACCEL_ODR_8kHz 0x03 +#define C6DOFIMU17_SET_ACCEL_ODR_4kHz 0x04 +#define C6DOFIMU17_SET_ACCEL_ODR_2kHz 0x05 +#define C6DOFIMU17_SET_ACCEL_ODR_1kHz 0x06 +#define C6DOFIMU17_SET_ACCEL_ODR_200Hz 0x07 +#define C6DOFIMU17_SET_ACCEL_ODR_100Hz 0x08 +#define C6DOFIMU17_SET_ACCEL_ODR_50Hz 0x09 +#define C6DOFIMU17_SET_ACCEL_ODR_25Hz 0x0A +#define C6DOFIMU17_SET_ACCEL_ODR_12_5Hz 0x0B + +#define C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_2 0x00 +#define C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_4 0x01 +#define C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_5 0x02 +#define C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_8 0x03 +#define C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_10 0x04 +#define C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_16 0x05 +#define C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_20 0x06 +#define C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_40 0x07 +#define C6DOFIMU17_SET_ACCEL_UI_FILT_BW_LOW_LATENCY_0 0x0E +#define C6DOFIMU17_SET_ACCEL_UI_FILT_BW_LOW_LATENCY_1 0x0F + +#define C6DOFIMU17_SET_ACCEL_UI_FILT_ORD_1st 0x00 +#define C6DOFIMU17_SET_ACCEL_UI_FILT_ORD_2st 0x01 +#define C6DOFIMU17_SET_ACCEL_UI_FILT_ORD_3st 0x02 + +#define C6DOFIMU17_SET_ACCEL_DEC2_M2_ORD_3st 0x02 + +#define C6DOFIMU17_SET_BANK_0 0x00 +#define C6DOFIMU17_SET_BANK_1 0x01 +#define C6DOFIMU17_SET_BANK_2 0x02 +#define C6DOFIMU17_SET_BANK_3 0x03 +#define C6DOFIMU17_SET_BANK_4 0x04 + +#include + +#include + +typedef err_t ( *c6dofimu17_master_io_t )( uint8_t, uint8_t*, uint8_t ); /**< Driver serial interface. */ + +typedef struct { + c6dofimu17_master_io_t write_f; /**< Master write function. */ + c6dofimu17_master_io_t read_f; /**< Master read function. */ +} c6dofimu17_t; + +typedef struct +{ + int16_t x; + int16_t y; + int16_t z; + +} c6dofimu17_axis_t; + +typedef struct +{ + uint8_t gyro_fs_sel; + uint8_t gyro_odr; + uint8_t gyro_ui_filt_ord; + uint8_t gyro_dec2_m2_ord; + uint8_t gyro_ui_filt_bw; + +} c6dofimu17_gyro_cfg_t; + +typedef struct +{ + uint8_t accel_fs_sel; + uint8_t accel_odr; + uint8_t accel_ui_filt_bw; + uint8_t accel_ui_filt_ord; + uint8_t accel_dec2_m2_ord; + +} c6dofimu17_accel_cfg_t; + +err_t c6dofimu17_init_comm ( c6dofimu17_master_io_t spi_read_f, c6dofimu17_master_io_t spi_write_f) ; + +err_t c6dofimu17_cfg ( c6dofimu17_gyro_cfg_t *gyro_cfg, c6dofimu17_accel_cfg_t *accel_cfg ); + +err_t c6dofimu17_generic_write ( uint8_t reg, uint8_t *data_in, uint8_t len ); + +err_t c6dofimu17_generic_read ( uint8_t reg, uint8_t *data_out, uint8_t len ); + +err_t c6dofimu17_bank_selection ( uint8_t bank_sel ); + +err_t c6dofimu17_get_device_id ( uint8_t *device_id ); + +err_t c6dofimu17_temperature_enable ( void ); + +err_t c6dofimu17_gyro_enable ( void ); + +err_t c6dofimu17_accel_enable ( void ); + +err_t c6dofimu17_set_config_gyro ( c6dofimu17_gyro_cfg_t gyro_cfg ); + +err_t c6dofimu17_get_config_gyro ( c6dofimu17_gyro_cfg_t *gyro_cfg ); + +err_t c6dofimu17_set_config_accel ( c6dofimu17_accel_cfg_t accel_cfg ); + +err_t c6dofimu17_get_config_accel ( c6dofimu17_accel_cfg_t *accel_cfg ); + +err_t c6dofimu17_soft_reset ( void ); + +err_t c6dofimu17_get_accel_data ( c6dofimu17_axis_t *accel_data ); + +err_t c6dofimu17_get_gyro_data ( c6dofimu17_axis_t *gyro_data ); + +err_t c6dofimu17_get_temperature ( float *temperature ); + +#ifdef __cplusplus +} +#endif +#endif // C6DOFIMU17_H diff --git a/processes/click_reader/main.cpp b/processes/click_reader/main.cpp index 9e2b188..e706f9a 100644 --- a/processes/click_reader/main.cpp +++ b/processes/click_reader/main.cpp @@ -2,6 +2,7 @@ #include #include #include +#include // Pipes #include @@ -11,6 +12,7 @@ // Libs #include +#include #include #include #include @@ -18,9 +20,17 @@ // Local #include "pipe_def.h" #include "util.h" +#include "6dofimu17.h" static std::shared_ptr logger; +static err_t spi_init(); +static err_t spi_write_alias(uint8_t reg, uint8_t* data_out, uint8_t len); +static err_t spi_read_alias(uint8_t reg, uint8_t* data_in, uint8_t len); + +static float accel_to_g(int, unsigned int = 16); +static float gyro_to_dps(int, unsigned int = 1000); + int main(int argc, char* argv[]) { @@ -30,13 +40,35 @@ int main(int argc, char* argv[]) logger->debug("Opening sendpipe for writing."); pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY); - if (!bcm2835_init()) + logger->info("Initializing BCM2835 and SPI!"); + if (spi_init() != OK) { - logger->error("bcm2835_init failed. Are you running as root?"); - return 1; + logger->error("spi_init() failed. Are you running as root?"); + return 1; } -/* + logger->debug("Initializing 6DOF IMU 17 Click."); + c6dofimu17_init_comm(*spi_read_alias, *spi_write_alias); + + auto gyro_cfg = std::make_unique(); + auto accel_cfg = std::make_unique(); + + accel_cfg->accel_fs_sel = C6DOFIMU17_SET_ACCEL_FS_SEL_16g; // Drones typically between 0-15g? + accel_cfg->accel_odr = C6DOFIMU17_SET_ACCEL_ODR_1kHz; + accel_cfg->accel_dec2_m2_ord = C6DOFIMU17_SET_ACCEL_DEC2_M2_ORD_3st; + accel_cfg->accel_ui_filt_ord = C6DOFIMU17_SET_ACCEL_UI_FILT_ORD_3st; + accel_cfg->accel_ui_filt_bw = C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_20; // ~50Hz + + gyro_cfg->gyro_fs_sel = C6DOFIMU17_SET_GYRO_FS_SEL_1000_dps; // Drones typically between 0-500 deg/s? + gyro_cfg->gyro_odr = C6DOFIMU17_SET_GYRO_ODR_1kHz; + gyro_cfg->gyro_ui_filt_ord = C6DOFIMU17_SET_GYRO_UI_FILT_ORD_3st; + gyro_cfg->gyro_dec2_m2_ord = C6DOFIMU17_SET_GYRO_DEC2_M2_ORD_3st; + gyro_cfg->gyro_ui_filt_bw = C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_20; // ~50Hz + + c6dofimu17_cfg(gyro_cfg.get(), accel_cfg.get()); + + + /* mavlink_message_t message; const uint8_t system_id = 42; const uint8_t base_mode = 0; @@ -59,21 +91,103 @@ int main(int argc, char* argv[]) uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; const int len = mavlink_msg_to_send_buffer(buffer, &message); - while(true) - { - write(sendpipe_fd, buffer, len); - THREAD_SLEEP(3s); - } -*/ + */ - logger->info("Initializing serial communication!"); + auto accel_data = std::make_unique(); + auto gyro_data = std::make_unique(); + struct { + float x = 0.0; + float y = 0.0; + float z = 0.0; + } angle; + auto t_start = std::chrono::high_resolution_clock::now(); + auto t_end = std::chrono::high_resolution_clock::now(); + float time_d_ms = 0.0; + + // Control loop while (true) { - THREAD_SLEEP(1s); + THREAD_SLEEP(100ms); + int res = 0; + res = c6dofimu17_get_gyro_data(gyro_data.get()); + t_end = std::chrono::high_resolution_clock::now(); + time_d_ms = std::chrono::duration(t_end-t_start).count(); + t_start = std::chrono::high_resolution_clock::now(); + + if (res != OK) { + logger->warn("Failed to get temperature: {}", res); + continue; + } + + // integrate gyro data + angle.x += gyro_to_dps(gyro_data->x) * time_d_ms / 1000; + angle.y += gyro_to_dps(gyro_data->y) * time_d_ms / 1000; + angle.z += gyro_to_dps(gyro_data->z) * time_d_ms / 1000; + + logger->debug("time_d: {}. ANGLE: X {}, Y {}, Z {}", time_d_ms, angle.x, angle.y, angle.z); + logger->debug("time_d: {}. GYRO: X {}, Y {}, Z {}", time_d_ms, gyro_to_dps(gyro_data->x), gyro_to_dps(gyro_data->y), gyro_to_dps(gyro_data->z)); } - logger->error("here"); close(sendpipe_fd); return 0; -} \ No newline at end of file +} + +// Some good old C! +static err_t spi_write_alias(uint8_t reg, uint8_t* data_out, uint8_t len) +{ + char buffer[len + 1]; + memset(buffer, 0, len + 1); + buffer[0] = reg; + memcpy(buffer + 1, data_out, len); + + bcm2835_spi_writenb(buffer, len + 1); + return OK; +} + +// Some good old C! +static err_t spi_read_alias(uint8_t reg, uint8_t* data_in, uint8_t len) +{ + char buffer[len + 1]; + memset(buffer, 0, len + 1); + buffer[0] = reg | 0x80; + + bcm2835_spi_transfern(buffer, len + 1); + + memcpy(data_in, buffer + 1, len); + return OK; +} + +static err_t spi_init() +{ + if (!bcm2835_init()) + { + return ERR; + } + + if (!bcm2835_spi_begin()) + { + return ERR; + } + + bcm2835_spi_setBitOrder(BCM2835_SPI_BIT_ORDER_MSBFIRST); // The default + bcm2835_spi_setDataMode(BCM2835_SPI_MODE0); // The default + bcm2835_spi_setClockDivider(BCM2835_SPI_CLOCK_DIVIDER_32768); // The default + bcm2835_spi_chipSelect(BCM2835_SPI_CS0); // The default + bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW); // the default + + return OK; +} + +static float accel_to_g(int data, unsigned int g_range) +{ + float tmp = 32768 / g_range; + tmp = data / tmp; + return tmp; +} +static float gyro_to_dps(int data, unsigned int dps_range) +{ + float tmp = 32768 / dps_range; + tmp = data / tmp; + return tmp; +} diff --git a/processes/include/c_util.h b/processes/include/c_util.h index be2e08a..3e2f258 100644 --- a/processes/include/c_util.h +++ b/processes/include/c_util.h @@ -3,9 +3,13 @@ #define HAL_PIN_NC 0 +#ifndef ERR_T +#define ERR_T + typedef enum { ERR = -1, OK = 0 } err_t; +#endif #endif \ No newline at end of file diff --git a/processes/include/util.h b/processes/include/util.h index e9a18b4..ee2923a 100644 --- a/processes/include/util.h +++ b/processes/include/util.h @@ -11,9 +11,11 @@ #define THREAD_SLEEP(time) std::this_thread::sleep_for(time) using namespace std::chrono_literals; +#ifndef ERR_T +#define ERR_T typedef enum { ERR = -1, OK = 0 } err_t; - +#endif #endif \ No newline at end of file -- 2.30.2