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)
- 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
add_subdirectory(udp_server)
-add_subdirectory(click_reader)
add_subdirectory(inter_proc)
add_subdirectory(controller)
add_subdirectory(GNSS_reader)
#include <fcntl.h>
#include <unistd.h>
-#include <array>
#include <sys/ioctl.h>
+#include <termios.h>
#include <string>
+#include <array>
#include <common/mavlink.h>
#include <spdlog/spdlog.h>
#include "system_def.h"
+#include "util.h"
static const char GNSSRTK_RSP_INIT ='$';
static const char GNSSRTK_RSP_DELIMITER = ',';
}
}
-[[noreturn]] void mavlink_task(pipe_fd_t send_pipe)
+[[noreturn]] void mavlink_task(int send_pipe)
{
while (true)
{
#include <termios.h>
#include "util.h"
-#include "pipe_def.h"
err_t init_uart(std::shared_ptr<spdlog::logger>,
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();
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);
+++ /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 ( 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
+++ /dev/null
-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)
+++ /dev/null
-#include "controller.h"
-
-#include <math.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<spdlog::logger> 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
+++ /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 ( double *temperature );
-
-#ifdef __cplusplus
-}
-#endif
-#endif // C6DOFIMU17_H
+++ /dev/null
-#ifndef CONTROLLER_H
-#define CONTROLLER_H
-
-// Standard
-#include <memory>
-#include <chrono>
-
-// Libs
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-#include <spdlog/spdlog.h>
-
-// 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<spdlog::logger>, 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<spdlog::logger> logger;
- const double alpha;
- vec2d current_state;
- vec3d last_gyro_meas;
- vec3d last_accel_meas;
-
-};
-
-class EKF
-{
-public:
- EKF(std::shared_ptr<spdlog::logger>);
-
- void update(const vec3d&, const vec3d&,
- double);
-
- const vec2d get_state();
-
-private:
- std::shared_ptr<spdlog::logger> logger;
- vec2d current_state;
-
-};
-#endif
\ No newline at end of file
+++ /dev/null
-// Standard
-#include <memory>
-#include <thread>
-#include <stdio.h>
-#include <chrono>
-#include <math.h>
-
-// Pipes
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <unistd.h>
-
-// Libs
-#include <spdlog/spdlog.h>
-#include <spdlog/fmt/bin_to_hex.h>
-#include <spdlog/sinks/stdout_color_sinks.h>
-
-#include <bcm2835.h>
-
-// Local
-#include "pipe_def.h"
-#include "util.h"
-#include "6dofimu17.h"
-#include "controller.h"
-#include "system_def.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);
-
-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<double>(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<int16_t>(temperature * 100);
- scaled_imu.xacc = static_cast<int16_t>(accel_vec[0] * 1000); // Maybe not correct?
- scaled_imu.yacc = static_cast<int16_t>(accel_vec[1] * 1000); // Maybe not correct?
- scaled_imu.zacc = static_cast<int16_t>(accel_vec[2] * 1000); // Maybe not correct?
- scaled_imu.xgyro = static_cast<int16_t>(gyro_vec[0] * 1000); // Maybe not correct?
- scaled_imu.ygyro = static_cast<int16_t>(gyro_vec[1] * 1000); // Maybe not correct?
- scaled_imu.zgyro = static_cast<int16_t>(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
--- /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>
+#include <stdint.h>
+
+#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
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)
--- /dev/null
+#include "controller.h"
+
+#include <math.h>
+#include <memory>
+
+#include <Eigen/Core>
+#include <spdlog/spdlog.h>
+
+#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<spdlog::logger> 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
--- /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
+
+#include <stdint.h>
+
+#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
--- /dev/null
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+#include <memory>
+
+#include <Eigen/Core>
+
+#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<spdlog::logger>, 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<spdlog::logger> logger;
+ const double alpha;
+ vec2d current_state;
+ vec3d last_gyro_meas;
+ vec3d last_accel_meas;
+
+};
+
+class EKF
+{
+public:
+ EKF(std::shared_ptr<spdlog::logger>);
+
+ void update(const vec3d&, const vec3d&,
+ double);
+
+ const vec2d get_state();
+
+private:
+ std::shared_ptr<spdlog::logger> logger;
+ vec2d current_state;
+
+};
+#endif
\ No newline at end of file
// Standard
#include <memory>
-#include <thread>
+#include <stdio.h>
+#include <chrono>
+#include <math.h>
// Pipes
#include <sys/types.h>
#include <spdlog/spdlog.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"
+#include "controller.h"
+#include "system_def.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);
+
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<double>(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<int16_t>(temperature * 100);
+ scaled_imu.xacc = static_cast<int16_t>(accel_vec[0] * 1000); // Maybe not correct?
+ scaled_imu.yacc = static_cast<int16_t>(accel_vec[1] * 1000); // Maybe not correct?
+ scaled_imu.zacc = static_cast<int16_t>(accel_vec[2] * 1000); // Maybe not correct?
+ scaled_imu.xgyro = static_cast<int16_t>(gyro_vec[0] * 1000); // Maybe not correct?
+ scaled_imu.ygyro = static_cast<int16_t>(gyro_vec[1] * 1000); // Maybe not correct?
+ scaled_imu.zgyro = static_cast<int16_t>(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
#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
#include <sys/stat.h>
#include <unistd.h>
-
#include "pipe_def.h"
static std::shared_ptr<spdlog::logger> logger;
#ifndef UDP_SERVER_H
#define UDP_SERVER_H
-// Standard
#include <memory>
-// Socket
-#include <netinet/in.h>
-#include <sys/socket.h>
-
-// Libs
-#include <spdlog/spdlog.h>
-
-// 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<spdlog::logger> logg, const pipe_fd_t recvpipe_fd, const pipe_fd_t sendpipe_fd);
+err_t udp_server_init(std::shared_ptr<spdlog::logger> logg, const int recvpipe_fd, const int sendpipe_fd);
err_t udp_server_run();
#endif
\ No newline at end of file
// Standard
#include <memory>
-#include <thread>
// Pipes
#include <fcntl.h>
// Libs
#include <spdlog/spdlog.h>
#include <spdlog/sinks/stdout_color_sinks.h>
-#include <common/mavlink.h>
// Local
#include "pipe_def.h"
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);
#include "udp_server.h"
-// Standard
#include <thread>
#include <memory>
-// Socket
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
-
-// Pipes
#include <unistd.h>
-// Libs
#include <common/mavlink.h>
#include <spdlog/spdlog.h>
-// Local
#include "system_def.h"
-#include "pipe_def.h"
#include "util.h"
static std::shared_ptr<spdlog::logger> 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);
}
}
-err_t udp_server_init(std::shared_ptr<spdlog::logger> logg, const pipe_fd_t recvpipe_fd, const pipe_fd_t sendpipe_fd)
+err_t udp_server_init(std::shared_ptr<spdlog::logger> logg, const int recvpipe_fd, const int sendpipe_fd)
{
logger = logg;
sendpipe = sendpipe_fd;
# start udp server
./udp_server &
-# start controller
-./controller &
-
# start click reader
-./click_reader &
+./controller &
# start GNSS reader
./GNSS_reader
projects / flygplan.git / commitdiff