From: Nils Forssén Date: Wed, 18 Jun 2025 22:06:13 +0000 (+0200) Subject: restructuring and simplifying includes X-Git-Url: https://gitweb.forssennils.se/?a=commitdiff_plain;h=9c1af77755c9f19a399b1cbcabcd38ced2cf4df2;p=flygplan.git restructuring and simplifying includes --- diff --git a/CMakeLists.txt b/CMakeLists.txt index e10f23c..e8730d1 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ add_definitions(-DLOG_INFO) find_library(BCM2835_LIB bcm2835) add_subdirectory(lib/mavlink) add_subdirectory(lib/spdlog) + set(Eigen3_DIR /usr/share/eigen/cmake) find_package(Eigen3 CONFIG REQUIRED) diff --git a/README.md b/README.md index ff3f365..532b720 100755 --- a/README.md +++ b/README.md @@ -30,4 +30,6 @@ just run wvdial, correct settings should be in `/etc/wvdial.conf`. - GNSS, see TODO comments - Fix cpp code (See GNSS for reference, remove header inclusion by using forward declaration, remove stuff from headers etc). - Start working on xbox controller, PWM, ESC, etc. - - Where is the controll-loop, autonomous?, how should xbox-controller inputs be handled? \ No newline at end of file + - Where is the controll-loop, autonomous?, how should xbox-controller inputs be handled? + - See compiler-warnings? + - controller process vs controller in click-reader? \ No newline at end of file diff --git a/processes/CMakeLists.txt b/processes/CMakeLists.txt index cfbba58..2505a60 100644 --- a/processes/CMakeLists.txt +++ b/processes/CMakeLists.txt @@ -1,5 +1,4 @@ add_subdirectory(udp_server) -add_subdirectory(click_reader) add_subdirectory(inter_proc) add_subdirectory(controller) add_subdirectory(GNSS_reader) diff --git a/processes/GNSS_reader/gnssrtk.cpp b/processes/GNSS_reader/gnssrtk.cpp index 59bea9e..b15238c 100644 --- a/processes/GNSS_reader/gnssrtk.cpp +++ b/processes/GNSS_reader/gnssrtk.cpp @@ -2,15 +2,17 @@ #include #include -#include #include +#include #include +#include #include #include #include "system_def.h" +#include "util.h" static const char GNSSRTK_RSP_INIT ='$'; static const char GNSSRTK_RSP_DELIMITER = ','; @@ -252,7 +254,7 @@ static gnssrtk_gnrmc_t parse_gnrmc(std::string& NMEA_msg) } } -[[noreturn]] void mavlink_task(pipe_fd_t send_pipe) +[[noreturn]] void mavlink_task(int send_pipe) { while (true) { diff --git a/processes/GNSS_reader/include/gnssrtk.h b/processes/GNSS_reader/include/gnssrtk.h index a427bb1..57d80bb 100644 --- a/processes/GNSS_reader/include/gnssrtk.h +++ b/processes/GNSS_reader/include/gnssrtk.h @@ -6,14 +6,13 @@ #include #include "util.h" -#include "pipe_def.h" err_t init_uart(std::shared_ptr, const std::string& = "/dev/serial0", unsigned int = B38400); [[noreturn]] void uart_task(); -[[noreturn]] void mavlink_task(pipe_fd_t); +[[noreturn]] void mavlink_task(int); void close_uart(); diff --git a/processes/GNSS_reader/main.cpp b/processes/GNSS_reader/main.cpp index e72209e..57d78c8 100644 --- a/processes/GNSS_reader/main.cpp +++ b/processes/GNSS_reader/main.cpp @@ -26,7 +26,7 @@ int main(int argc, char* argv[]) logger->set_level(spdlog::level::debug); // TODO change to glob logger->debug("Opening sendpipe for writing."); - pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY); + int sendpipe_fd = open(SEND_PIPE, O_WRONLY); logger->info("Initializing UART."); int ret = init_uart(logger); diff --git a/processes/click_reader/6dofimu17.c b/processes/click_reader/6dofimu17.c deleted file mode 100644 index 37ffaf0..0000000 --- a/processes/click_reader/6dofimu17.c +++ /dev/null @@ -1,330 +0,0 @@ -/**************************************************************************** -** 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 ( double *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 = ( double ) 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 deleted file mode 100644 index 582866d..0000000 --- a/processes/click_reader/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -add_executable(click_reader - main.cpp - 6dofimu17.c - controller.cpp -) - -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 Eigen3::Eigen) -target_link_libraries(click_reader PRIVATE common_lib) - -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/controller.cpp b/processes/click_reader/controller.cpp deleted file mode 100644 index eacfcb0..0000000 --- a/processes/click_reader/controller.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include "controller.h" - -#include - -static vec2d pqr_to_euler(const vec3d&, const vec2d&); -static vec2d accel_to_pitch_roll(const vec3d&); - -vec3d struct_to_vec_scaled(const c6dofimu17_axis_t& data, const double scale) -{ - double data_x = data.x * scale; - double data_y = data.y * scale; - double data_z = data.z * scale; - - return vec3d{data_x, data_y, data_z}; -} - - -Complementary_filter::Complementary_filter(std::shared_ptr logg, const double alpha) : - logger{logg}, - alpha{alpha}, - current_state{vec2d::Zero()} -{} - -const vec2d Complementary_filter::get_state() -{ - return current_state; -} - -void Complementary_filter::update(const vec3d &gyro_meas, const vec3d &accel_meas, - double dt_sec, unsigned int gyro_range, unsigned int accel_range) -{ - /* - // Crude check for new data - if (last_accel_meas == accel_meas) - { - logger->warn("Last accel measurement indentical to new."); - } - if (last_gyro_meas == gyro_meas) - { - logger->warn("Last gyro measurement identical to new."); - } - last_accel_meas = accel_meas; - last_gyro_meas = gyro_meas; - */ - - auto accel_est = accel_to_pitch_roll(accel_meas); - auto gyro_est = pqr_to_euler(gyro_meas, current_state); - - current_state = (alpha * accel_est) + (1 - alpha) * (current_state + (dt_sec * gyro_est)); -} - -static vec2d pqr_to_euler(const vec3d& pqr, const vec2d& pitch_roll) -{ - double phi = pitch_roll[0]; - double theta = pitch_roll[1]; - - auto euler_angles = Eigen::MatrixXd{ - {1, sin(phi) * tan(theta), cos(phi) * tan(theta)}, - {0, cos(phi), -sin(phi)} - }; - - return euler_angles * pqr; -} - -static vec2d accel_to_pitch_roll(const vec3d& accel) -{ - return vec2d{ - atan2(accel[1], accel[2]), - atan2(-accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2]))) - }; - -} \ No newline at end of file diff --git a/processes/click_reader/include/6dofimu17.h b/processes/click_reader/include/6dofimu17.h deleted file mode 100644 index 6ed01da..0000000 --- a/processes/click_reader/include/6dofimu17.h +++ /dev/null @@ -1,321 +0,0 @@ -/**************************************************************************** -** 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 ( double *temperature ); - -#ifdef __cplusplus -} -#endif -#endif // C6DOFIMU17_H diff --git a/processes/click_reader/include/controller.h b/processes/click_reader/include/controller.h deleted file mode 100644 index 389c689..0000000 --- a/processes/click_reader/include/controller.h +++ /dev/null @@ -1,57 +0,0 @@ -#ifndef CONTROLLER_H -#define CONTROLLER_H - -// Standard -#include -#include - -// Libs -#include -#include -#include - -// Local -#include "6dofimu17.h" - -#define GRAVITY 9.82f - -typedef Eigen::Vector3d vec3d; -typedef Eigen::Vector2d vec2d; - -vec3d struct_to_vec_scaled(const c6dofimu17_axis_t&, const double); - -class Complementary_filter -{ -public: - Complementary_filter(std::shared_ptr, const double = 0.02); - - void update(const vec3d&, const vec3d&, - double, const unsigned int = 1000, const unsigned int = 16); - - const vec2d get_state(); - -private: - std::shared_ptr logger; - const double alpha; - vec2d current_state; - vec3d last_gyro_meas; - vec3d last_accel_meas; - -}; - -class EKF -{ -public: - EKF(std::shared_ptr); - - void update(const vec3d&, const vec3d&, - double); - - const vec2d get_state(); - -private: - std::shared_ptr logger; - vec2d current_state; - -}; -#endif \ No newline at end of file diff --git a/processes/click_reader/main.cpp b/processes/click_reader/main.cpp deleted file mode 100644 index 4a0ab90..0000000 --- a/processes/click_reader/main.cpp +++ /dev/null @@ -1,200 +0,0 @@ -// Standard -#include -#include -#include -#include -#include - -// Pipes -#include -#include -#include -#include - -// Libs -#include -#include -#include - -#include - -// Local -#include "pipe_def.h" -#include "util.h" -#include "6dofimu17.h" -#include "controller.h" -#include "system_def.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); - -int main(int argc, char* argv[]) -{ - - logger = spdlog::stdout_color_mt("click_reader"); - logger->set_level(GLOB_LOGLEVEL); - - logger->debug("Opening sendpipe for writing."); - pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY); - - logger->info("Initializing BCM2835 and SPI."); - if (spi_init() != OK) - { - 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); - - c6dofimu17_gyro_cfg_t gyro_cfg; - c6dofimu17_accel_cfg_t accel_cfg; - - 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, &accel_cfg); - - c6dofimu17_axis_t accel_data; - c6dofimu17_axis_t gyro_data; - double temperature = 0.0; - - Complementary_filter comp_filter{logger, 0.02}; - - int res = 0; - auto t_start = std::chrono::high_resolution_clock::now(); - auto t_end = std::chrono::high_resolution_clock::now(); - double time_d = 0.0; - - mavlink_message_t msg; - mavlink_attitude_t attitude; - mavlink_scaled_imu_t scaled_imu; - // Control loop - while (true) - { - THREAD_SLEEP(CLICK_READ_SPI_PERIOD); - - res = c6dofimu17_get_gyro_data(&gyro_data); - t_end = std::chrono::high_resolution_clock::now(); - time_d = std::chrono::duration(t_end-t_start).count(); - t_start = std::chrono::high_resolution_clock::now(); - - res += c6dofimu17_get_accel_data(&accel_data); - - res += c6dofimu17_get_temperature(&temperature); - - if (res != OK) { - logger->warn("Failed to get imu data: {}", res); - continue; - } - - vec3d gyro_vec = struct_to_vec_scaled(gyro_data, (M_PI * 1000.0) / (32768.0 * 180.0)); - vec3d accel_vec = struct_to_vec_scaled(accel_data, (GRAVITY * 16.0) / 32768.0); - - comp_filter.update(gyro_vec, accel_vec, time_d); - - // Pack data into MAVLINK packages - attitude.time_boot_ms = system_uptime_ms(); - attitude.roll = comp_filter.get_state()[1]; - attitude.pitch = comp_filter.get_state()[0]; - attitude.yaw = 0.0; - attitude.rollspeed = 0.0; - attitude.pitchspeed = 0.0; - attitude.yawspeed = 0.0; - - scaled_imu.time_boot_ms = system_uptime_ms(); - scaled_imu.temperature = static_cast(temperature * 100); - scaled_imu.xacc = static_cast(accel_vec[0] * 1000); // Maybe not correct? - scaled_imu.yacc = static_cast(accel_vec[1] * 1000); // Maybe not correct? - scaled_imu.zacc = static_cast(accel_vec[2] * 1000); // Maybe not correct? - scaled_imu.xgyro = static_cast(gyro_vec[0] * 1000); // Maybe not correct? - scaled_imu.ygyro = static_cast(gyro_vec[1] * 1000); // Maybe not correct? - scaled_imu.zgyro = static_cast(gyro_vec[2] * 1000); // Maybe not correct? - scaled_imu.xmag = INT16_MAX; - scaled_imu.ymag = INT16_MAX; - scaled_imu.zmag = INT16_MAX; - - mavlink_msg_attitude_encode( - MAV_SYS_ID, - MAV_COMP_ID, - &msg, - &attitude - ); - - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - int len; - - len = mavlink_msg_to_send_buffer(buffer, &msg); - write(sendpipe_fd, buffer, len); - - mavlink_msg_scaled_imu_encode( - MAV_SYS_ID, - MAV_COMP_ID, - &msg, - &scaled_imu - ); - len = mavlink_msg_to_send_buffer(buffer, &msg); - write(sendpipe_fd, buffer, len); - } - - close(sendpipe_fd); - return 0; -} - -// 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_32); // The default - bcm2835_spi_chipSelect(BCM2835_SPI_CS0); // The default - bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW); // the default - - return OK; -} \ No newline at end of file diff --git a/processes/controller/6dofimu17.c b/processes/controller/6dofimu17.c new file mode 100644 index 0000000..204e639 --- /dev/null +++ b/processes/controller/6dofimu17.c @@ -0,0 +1,333 @@ +/**************************************************************************** +** 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 +#include + +#include "c_util.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 ( double *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 = ( double ) 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/controller/CMakeLists.txt b/processes/controller/CMakeLists.txt old mode 100755 new mode 100644 index 1bd5bd9..d59d546 --- a/processes/controller/CMakeLists.txt +++ b/processes/controller/CMakeLists.txt @@ -1,10 +1,16 @@ add_executable(controller main.cpp + 6dofimu17.c + controller.cpp ) target_include_directories(controller PRIVATE include) target_link_libraries(controller PRIVATE MAVLink) target_link_libraries(controller PRIVATE spdlog::spdlog) +target_link_libraries(controller PRIVATE Eigen3::Eigen) +target_link_libraries(controller PRIVATE common_lib) + +target_link_libraries(controller PRIVATE ${BCM2835_LIB}) target_include_directories(controller PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include) diff --git a/processes/controller/controller.cpp b/processes/controller/controller.cpp new file mode 100644 index 0000000..79ff0f2 --- /dev/null +++ b/processes/controller/controller.cpp @@ -0,0 +1,79 @@ +#include "controller.h" + +#include +#include + +#include +#include + +#include "6dofimu17.h" +#include "util.h" + +static vec2d pqr_to_euler(const vec3d&, const vec2d&); +static vec2d accel_to_pitch_roll(const vec3d&); + +vec3d struct_to_vec_scaled(const c6dofimu17_axis_t& data, const double scale) +{ + double data_x = data.x * scale; + double data_y = data.y * scale; + double data_z = data.z * scale; + + return vec3d{data_x, data_y, data_z}; +} + + +Complementary_filter::Complementary_filter(std::shared_ptr logg, const double alpha) : + logger{logg}, + alpha{alpha}, + current_state{vec2d::Zero()} +{} + +const vec2d Complementary_filter::get_state() +{ + return current_state; +} + +void Complementary_filter::update(const vec3d &gyro_meas, const vec3d &accel_meas, + double dt_sec, unsigned int gyro_range, unsigned int accel_range) +{ + /* + // Crude check for new data + if (last_accel_meas == accel_meas) + { + logger->warn("Last accel measurement indentical to new."); + } + if (last_gyro_meas == gyro_meas) + { + logger->warn("Last gyro measurement identical to new."); + } + last_accel_meas = accel_meas; + last_gyro_meas = gyro_meas; + */ + + auto accel_est = accel_to_pitch_roll(accel_meas); + auto gyro_est = pqr_to_euler(gyro_meas, current_state); + + current_state = (alpha * accel_est) + (1 - alpha) * (current_state + (dt_sec * gyro_est)); +} + +static vec2d pqr_to_euler(const vec3d& pqr, const vec2d& pitch_roll) +{ + double phi = pitch_roll[0]; + double theta = pitch_roll[1]; + + auto euler_angles = Eigen::MatrixXd{ + {1, sin(phi) * tan(theta), cos(phi) * tan(theta)}, + {0, cos(phi), -sin(phi)} + }; + + return euler_angles * pqr; +} + +static vec2d accel_to_pitch_roll(const vec3d& accel) +{ + return vec2d{ + atan2(accel[1], accel[2]), + atan2(-accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2]))) + }; + +} \ No newline at end of file diff --git a/processes/controller/include/6dofimu17.h b/processes/controller/include/6dofimu17.h new file mode 100644 index 0000000..95e1c29 --- /dev/null +++ b/processes/controller/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 + +#include + +#include "c_util.h" + +// 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 + +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 ( double *temperature ); + +#ifdef __cplusplus +} +#endif +#endif // C6DOFIMU17_H diff --git a/processes/controller/include/controller.h b/processes/controller/include/controller.h new file mode 100644 index 0000000..8cfc66b --- /dev/null +++ b/processes/controller/include/controller.h @@ -0,0 +1,52 @@ +#ifndef CONTROLLER_H +#define CONTROLLER_H + +#include + +#include + +#include "util.h" +#include "6dofimu17.h" + +const double GRAVITY = 9.82f; + +typedef Eigen::Vector3d vec3d; +typedef Eigen::Vector2d vec2d; + +vec3d struct_to_vec_scaled(const c6dofimu17_axis_t&, const double); + +class Complementary_filter +{ +public: + Complementary_filter(std::shared_ptr, const double = 0.02); + + void update(const vec3d&, const vec3d&, + double, const unsigned int = 1000, const unsigned int = 16); + + const vec2d get_state(); + +private: + std::shared_ptr logger; + const double alpha; + vec2d current_state; + vec3d last_gyro_meas; + vec3d last_accel_meas; + +}; + +class EKF +{ +public: + EKF(std::shared_ptr); + + void update(const vec3d&, const vec3d&, + double); + + const vec2d get_state(); + +private: + std::shared_ptr logger; + vec2d current_state; + +}; +#endif \ No newline at end of file diff --git a/processes/controller/main.cpp b/processes/controller/main.cpp index 4d68c3e..fe89c61 100644 --- a/processes/controller/main.cpp +++ b/processes/controller/main.cpp @@ -1,6 +1,8 @@ // Standard #include -#include +#include +#include +#include // Pipes #include @@ -12,42 +14,185 @@ #include #include #include +#include // Local #include "pipe_def.h" #include "util.h" +#include "6dofimu17.h" +#include "controller.h" +#include "system_def.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); + int main(int argc, char* argv[]) { - logger = spdlog::stdout_color_mt("controller"); + + logger = spdlog::stdout_color_mt("click_reader"); logger->set_level(GLOB_LOGLEVEL); -/* + logger->debug("Opening sendpipe for writing."); - pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY); -*/ - logger->debug("Opening recvpipe for reading."); - pipe_fd_t recvpipe_fd = open(RECV_PIPE, O_RDONLY); + int sendpipe_fd = open(SEND_PIPE, O_WRONLY); - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - while(true) + logger->info("Initializing BCM2835 and SPI."); + if (spi_init() != OK) { - int ret = read(recvpipe_fd, buffer, MAVLINK_MAX_PACKET_LEN); + logger->error("spi_init() failed. Are you running as root?"); + return 1; + } - if (ret == 0) { - logger->error("No writing process on recvpipe. Exiting."); - break; - } - else if (ret < 0) - logger->error("Error while reading message. {}", strerror(errno)); - else { - // handle mavlink message + logger->debug("Initializing 6DOF IMU 17 Click."); + c6dofimu17_init_comm(*spi_read_alias, *spi_write_alias); + + c6dofimu17_gyro_cfg_t gyro_cfg; + c6dofimu17_accel_cfg_t accel_cfg; + + 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, &accel_cfg); + + c6dofimu17_axis_t accel_data; + c6dofimu17_axis_t gyro_data; + double temperature = 0.0; + + Complementary_filter comp_filter{logger, 0.02}; + + int res = 0; + auto t_start = std::chrono::high_resolution_clock::now(); + auto t_end = std::chrono::high_resolution_clock::now(); + double time_d = 0.0; + + mavlink_message_t msg; + mavlink_attitude_t attitude; + mavlink_scaled_imu_t scaled_imu; + // Control loop + while (true) + { + THREAD_SLEEP(CLICK_READ_SPI_PERIOD); + + res = c6dofimu17_get_gyro_data(&gyro_data); + t_end = std::chrono::high_resolution_clock::now(); + time_d = std::chrono::duration(t_end-t_start).count(); + t_start = std::chrono::high_resolution_clock::now(); + + res += c6dofimu17_get_accel_data(&accel_data); + + res += c6dofimu17_get_temperature(&temperature); + + if (res != OK) { + logger->warn("Failed to get imu data: {}", res); + continue; } + + vec3d gyro_vec = struct_to_vec_scaled(gyro_data, (M_PI * 1000.0) / (32768.0 * 180.0)); + vec3d accel_vec = struct_to_vec_scaled(accel_data, (GRAVITY * 16.0) / 32768.0); + + comp_filter.update(gyro_vec, accel_vec, time_d); + + // Pack data into MAVLINK packages + attitude.time_boot_ms = system_uptime_ms(); + attitude.roll = comp_filter.get_state()[1]; + attitude.pitch = comp_filter.get_state()[0]; + attitude.yaw = 0.0; + attitude.rollspeed = 0.0; + attitude.pitchspeed = 0.0; + attitude.yawspeed = 0.0; + + scaled_imu.time_boot_ms = system_uptime_ms(); + scaled_imu.temperature = static_cast(temperature * 100); + scaled_imu.xacc = static_cast(accel_vec[0] * 1000); // Maybe not correct? + scaled_imu.yacc = static_cast(accel_vec[1] * 1000); // Maybe not correct? + scaled_imu.zacc = static_cast(accel_vec[2] * 1000); // Maybe not correct? + scaled_imu.xgyro = static_cast(gyro_vec[0] * 1000); // Maybe not correct? + scaled_imu.ygyro = static_cast(gyro_vec[1] * 1000); // Maybe not correct? + scaled_imu.zgyro = static_cast(gyro_vec[2] * 1000); // Maybe not correct? + scaled_imu.xmag = INT16_MAX; + scaled_imu.ymag = INT16_MAX; + scaled_imu.zmag = INT16_MAX; + + mavlink_msg_attitude_encode( + MAV_SYS_ID, + MAV_COMP_ID, + &msg, + &attitude + ); + + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + int len; + + len = mavlink_msg_to_send_buffer(buffer, &msg); + write(sendpipe_fd, buffer, len); + + mavlink_msg_scaled_imu_encode( + MAV_SYS_ID, + MAV_COMP_ID, + &msg, + &scaled_imu + ); + len = mavlink_msg_to_send_buffer(buffer, &msg); + write(sendpipe_fd, buffer, len); } - logger->error("here"); - //close(sendpipe_fd); - close(recvpipe_fd); + close(sendpipe_fd); return 0; +} + +// 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_32); // The default + bcm2835_spi_chipSelect(BCM2835_SPI_CS0); // The default + bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW); // the default + + return OK; } \ No newline at end of file diff --git a/processes/include/pipe_def.h b/processes/include/pipe_def.h index f34774e..4c518fc 100644 --- a/processes/include/pipe_def.h +++ b/processes/include/pipe_def.h @@ -4,6 +4,4 @@ #define RECV_PIPE "/var/lock/recv_pipe" #define SEND_PIPE "/var/lock/send_pipe" -typedef int pipe_fd_t; - #endif \ No newline at end of file diff --git a/processes/inter_proc/main.cpp b/processes/inter_proc/main.cpp index 0b0ec33..dc361e9 100644 --- a/processes/inter_proc/main.cpp +++ b/processes/inter_proc/main.cpp @@ -7,7 +7,6 @@ #include #include - #include "pipe_def.h" static std::shared_ptr logger; diff --git a/processes/udp_server/include/udp_server.h b/processes/udp_server/include/udp_server.h index 6902b61..6e098ac 100644 --- a/processes/udp_server/include/udp_server.h +++ b/processes/udp_server/include/udp_server.h @@ -1,31 +1,11 @@ #ifndef UDP_SERVER_H #define UDP_SERVER_H -// Standard #include -// Socket -#include -#include - -// Libs -#include - -// Local -#include "pipe_def.h" #include "util.h" -#define UDP_PORT 14552 -#define UDP_RECV_BUF_SIZE 2048 // enough for MTU 1500 bytes - -typedef struct { - int socket_fd; - struct sockaddr_in addr; - socklen_t addr_len; - bool addr_set; -} gcs_t; - -err_t udp_server_init(std::shared_ptr logg, const pipe_fd_t recvpipe_fd, const pipe_fd_t sendpipe_fd); +err_t udp_server_init(std::shared_ptr logg, const int recvpipe_fd, const int sendpipe_fd); err_t udp_server_run(); #endif \ No newline at end of file diff --git a/processes/udp_server/main.cpp b/processes/udp_server/main.cpp index c02a5b3..af94c11 100755 --- a/processes/udp_server/main.cpp +++ b/processes/udp_server/main.cpp @@ -1,6 +1,5 @@ // Standard #include -#include // Pipes #include @@ -9,7 +8,6 @@ // Libs #include #include -#include // Local #include "pipe_def.h" @@ -25,8 +23,8 @@ int main(int argc, char* argv[]) logger->set_level(GLOB_LOGLEVEL); logger->debug("Opening pipes, waiting for writing process to start."); - pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_RDONLY); - pipe_fd_t recvpipe_fd = open(RECV_PIPE, O_WRONLY); + int sendpipe_fd = open(SEND_PIPE, O_RDONLY); + int recvpipe_fd = open(RECV_PIPE, O_WRONLY); udp_server_init(logger, recvpipe_fd, sendpipe_fd); diff --git a/processes/udp_server/udp_server.cpp b/processes/udp_server/udp_server.cpp index 9f19a41..f534dea 100644 --- a/processes/udp_server/udp_server.cpp +++ b/processes/udp_server/udp_server.cpp @@ -1,33 +1,36 @@ #include "udp_server.h" -// Standard #include #include -// Socket #include #include #include - -// Pipes #include -// Libs #include #include -// Local #include "system_def.h" -#include "pipe_def.h" #include "util.h" static std::shared_ptr logger; +static const int UDP_PORT = 14552; +static const int UDP_RECV_BUF_SIZE = 2048; // enough for MTU 1500 bytes + +typedef struct { + int socket_fd; + struct sockaddr_in addr; + socklen_t addr_len; + bool addr_set; +} gcs_t; + static bool recv_thread_running = true; static gcs_t gcs = {}; -static pipe_fd_t recvpipe; -static pipe_fd_t sendpipe; +static int recvpipe; +static int sendpipe; // Declarations static void handle_request_autopilot_capabilities(const mavlink_command_long_t& cmd_long); @@ -172,7 +175,7 @@ static void handle_mavlink(mavlink_message_t& message) } } -err_t udp_server_init(std::shared_ptr logg, const pipe_fd_t recvpipe_fd, const pipe_fd_t sendpipe_fd) +err_t udp_server_init(std::shared_ptr logg, const int recvpipe_fd, const int sendpipe_fd) { logger = logg; sendpipe = sendpipe_fd; diff --git a/run.sh b/run.sh index deadd0d..07a430f 100755 --- a/run.sh +++ b/run.sh @@ -26,11 +26,8 @@ sleep 1 # start udp server ./udp_server & -# start controller -./controller & - # start click reader -./click_reader & +./controller & # start GNSS reader ./GNSS_reader