build/
bin/
-.vscode
-WiringPi-3.6/
\ No newline at end of file
+.vscode
\ No newline at end of file
just run wvdial
+# TODO
+
+Fix the spi functions in pressure4.
\ No newline at end of file
#!/usr/bin/bash
cd $(dirname "$0")/build
-cmake -G Ninja ..
-
ninja
\ No newline at end of file
--- /dev/null
+cd $(dirname "$0")/build
+cmake -G Ninja .
+ninja
\ No newline at end of file
--- /dev/null
+/****************************************************************************
+** 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 <unistd.h>
+
+/**
+ * @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
add_executable(click_reader
main.cpp
+ 6dofimu17.c
)
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)
--- /dev/null
+/****************************************************************************
+** 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 <stdint.h>
+
+#include <c_util.h>
+
+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
#include <memory>
#include <thread>
#include <stdio.h>
+#include <chrono>
// Pipes
#include <sys/types.h>
// Libs
#include <spdlog/spdlog.h>
+#include <spdlog/fmt/bin_to_hex.h>
#include <spdlog/sinks/stdout_color_sinks.h>
#include <common/mavlink.h>
#include <bcm2835.h>
// Local
#include "pipe_def.h"
#include "util.h"
+#include "6dofimu17.h"
static std::shared_ptr<spdlog::logger> 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[])
{
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<c6dofimu17_gyro_cfg_t>();
+ auto accel_cfg = std::make_unique<c6dofimu17_accel_cfg_t>();
+
+ 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;
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<c6dofimu17_axis_t>();
+ auto gyro_data = std::make_unique<c6dofimu17_axis_t>();
+ 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<double, std::milli>(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;
+}
#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
#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
projects / flygplan.git / commitdiff