set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
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)
add_subdirectory(processes)
\ No newline at end of file
Enable fan pin 20 through raspi-config
```
-sudo apt install -y cmake build-essential checkinstall zlib1g-dev libssl-dev python3 python3-pip ninja-build git ufw python3-picamera2 --no-install-recommends libspdlog-dev
+sudo apt install -y cmake build-essential checkinstall zlib1g-dev libssl-dev python3 python3-pip ninja-build git ufw python3-picamera2 libspdlog-dev libeigen3-dev --no-install-recommends
```
-based a bit on example [this is my link](https://github.com/mavlink/mavlink/blob/master/examples/c/udp_example.c)
-also want to create mavlink base server using mavproxy
+based a bit on [example](https://github.com/mavlink/mavlink/blob/master/examples/c/udp_example.c).
+
+Using mission planner UDPCI port 14552. Camera is streamed to `http://192.168.0.197:8000/stream.mjpg`
ufw for firewall
# Used libraries
-```
-IMU:
-https://github.com/meierman1/IIM42652-Sensor-API
-
-PRESSURE:
-https://github.com/YuechengLi/BMP280_driver (deprecated and not very good, should consider changing to a newer librar, e.g: https://github.com/boschsensortec/BME280_SensorAPI) (check BME280 vs BMP280)
-```
+ - [BCM2835](https://www.airspayce.com/mikem/bcm2835/index.html), hardware communication.
+ - [Mavlink](https://mavlink.io/en/).
+ - [spdlog](https://github.com/gabime/spdlog), the best logging library.
+ - [Eigen3](https://eigen.tuxfamily.org/dox/index.html), linear algebra.
# Modem
-just run wvdial
+just run wvdial, correct settings should be in `/etc/wvdial.conf`.
# TODO
-
-Fix the spi functions in pressure4.
\ No newline at end of file
+ - EKF for IMU (maybe incorporate GNSS, probably not).
+ - Yaw, maybe add time-derivates to attitude?
+ - Fix all unhandled mavlink messages.
+ - GNSS.
+ - Start working on xbox controller.
+ - Temperature.
+ - Attitude time_boot_ms.
#!/usr/bin/bash
cd $(dirname "$0")/build
-ninja
\ No newline at end of file
+ninja -j2
\ No newline at end of file
cd $(dirname "$0")/build
-cmake -G Ninja .
-ninja
\ No newline at end of file
+cmake -G Ninja .. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -j2
+
+ninja -j2
\ No newline at end of file
return err_flag;
}
-err_t c6dofimu17_get_temperature ( float *temperature )
+err_t c6dofimu17_get_temperature ( double *temperature )
{
uint8_t rx_buf[ 2 ];
int16_t tmp;
tmp <<= 8;
tmp |= rx_buf[ 1 ];
- *temperature = ( float ) tmp;
+ *temperature = ( double ) tmp;
*temperature /= 132.48;
*temperature += 25;
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 ${BCM2835_LIB})
--- /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 vec3d accel_to_g(const c6dofimu17_axis_t &data, const unsigned int g_range)
+{
+ double tmp = 32768 / g_range;
+ double accel_x = data.x / tmp;
+ double accel_y = data.y / tmp;
+ double accel_z = data.z / tmp;
+
+ accel_x *= GRAVITY; // Convert to m/s^2
+ accel_y *= GRAVITY; // Convert to m/s^2
+ accel_z *= GRAVITY; // Convert to m/s^2
+
+ return vec3d{accel_x, accel_y, accel_z};
+}
+
+static vec3d gyro_to_rad(const c6dofimu17_axis_t &data, const unsigned int dps_range)
+{
+ double tmp = 32768 / dps_range;
+ double gyro_x = data.x / tmp;
+ double gyro_y = data.y / tmp;
+ double gyro_z = data.z / tmp;
+
+ gyro_x *= M_PI / 180.0; // Convert to radians
+ gyro_y *= M_PI / 180.0; // Convert to radians
+ gyro_z *= M_PI / 180.0; // Convert to radians
+
+ return vec3d{gyro_x, gyro_y, gyro_z};
+}
+*/
+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
err_t c6dofimu17_get_gyro_data ( c6dofimu17_axis_t *gyro_data );
-err_t c6dofimu17_get_temperature ( float *temperature );
+err_t c6dofimu17_get_temperature ( double *temperature );
#ifdef __cplusplus
}
--- /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
#include <thread>
#include <stdio.h>
#include <chrono>
+#include <math.h>
// Pipes
#include <sys/types.h>
#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_write_alias(uint8_t reg, uint8_t* data_out, uint8_t len);
static err_t spi_read_alias(uint8_t reg, uint8_t* data_in, uint8_t len);
-static float accel_to_g(int, unsigned int = 16);
-static float gyro_to_dps(int, unsigned int = 1000);
int main(int argc, char* argv[])
{
logger->debug("Opening sendpipe for writing.");
pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY);
- logger->info("Initializing BCM2835 and SPI!");
+ logger->info("Initializing BCM2835 and SPI.");
if (spi_init() != OK)
{
logger->error("spi_init() failed. Are you running as root?");
logger->debug("Initializing 6DOF IMU 17 Click.");
c6dofimu17_init_comm(*spi_read_alias, *spi_write_alias);
- auto gyro_cfg = std::make_unique<c6dofimu17_gyro_cfg_t>();
- auto accel_cfg = std::make_unique<c6dofimu17_accel_cfg_t>();
-
- accel_cfg->accel_fs_sel = C6DOFIMU17_SET_ACCEL_FS_SEL_16g; // Drones typically between 0-15g?
- accel_cfg->accel_odr = C6DOFIMU17_SET_ACCEL_ODR_1kHz;
- accel_cfg->accel_dec2_m2_ord = C6DOFIMU17_SET_ACCEL_DEC2_M2_ORD_3st;
- accel_cfg->accel_ui_filt_ord = C6DOFIMU17_SET_ACCEL_UI_FILT_ORD_3st;
- accel_cfg->accel_ui_filt_bw = C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_20; // ~50Hz
-
- gyro_cfg->gyro_fs_sel = C6DOFIMU17_SET_GYRO_FS_SEL_1000_dps; // Drones typically between 0-500 deg/s?
- gyro_cfg->gyro_odr = C6DOFIMU17_SET_GYRO_ODR_1kHz;
- gyro_cfg->gyro_ui_filt_ord = C6DOFIMU17_SET_GYRO_UI_FILT_ORD_3st;
- gyro_cfg->gyro_dec2_m2_ord = C6DOFIMU17_SET_GYRO_DEC2_M2_ORD_3st;
- gyro_cfg->gyro_ui_filt_bw = C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_20; // ~50Hz
-
- c6dofimu17_cfg(gyro_cfg.get(), accel_cfg.get());
-
-
- /*
- mavlink_message_t message;
- const uint8_t system_id = 42;
- const uint8_t base_mode = 0;
- const uint8_t custom_mode = 0;
- mavlink_msg_global_position_int_pack_chan(
- system_id,
- MAV_COMP_ID_PERIPHERAL,
- MAVLINK_COMM_0,
- &message,
- 1,
- 2,
- 3,
- 4,
- 5,
- 6,
- 7,
- 8,
- 9);
-
- uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
- const int len = mavlink_msg_to_send_buffer(buffer, &message);
-
- */
-
- auto accel_data = std::make_unique<c6dofimu17_axis_t>();
- auto gyro_data = std::make_unique<c6dofimu17_axis_t>();
- struct {
- float x = 0.0;
- float y = 0.0;
- float z = 0.0;
- } angle;
+ 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;
+
+ 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();
- float time_d_ms = 0.0;
+ double time_d = 0.0;
+ mavlink_message_t msg;
+ mavlink_attitude_t attitude;
// Control loop
while (true)
{
- THREAD_SLEEP(100ms);
- int res = 0;
- res = c6dofimu17_get_gyro_data(gyro_data.get());
+ THREAD_SLEEP(1ms);
+
+ res = c6dofimu17_get_gyro_data(&gyro_data);
t_end = std::chrono::high_resolution_clock::now();
- time_d_ms = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+ 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);
+
if (res != OK) {
- logger->warn("Failed to get temperature: {}", res);
+ logger->warn("Failed to get accel data: {}", res);
continue;
}
- // integrate gyro data
- angle.x += gyro_to_dps(gyro_data->x) * time_d_ms / 1000;
- angle.y += gyro_to_dps(gyro_data->y) * time_d_ms / 1000;
- angle.z += gyro_to_dps(gyro_data->z) * time_d_ms / 1000;
-
- logger->debug("time_d: {}. ANGLE: X {}, Y {}, Z {}", time_d_ms, angle.x, angle.y, angle.z);
- logger->debug("time_d: {}. GYRO: X {}, Y {}, Z {}", time_d_ms, gyro_to_dps(gyro_data->x), gyro_to_dps(gyro_data->y), gyro_to_dps(gyro_data->z));
+ 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 attitude into MAVLINK package
+ attitude.time_boot_ms = 0;
+ 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;
+
+ mavlink_msg_attitude_encode(
+ MAV_SYS_ID,
+ MAV_COMP_ID,
+ &msg,
+ &attitude
+ );
+
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ const int len = mavlink_msg_to_send_buffer(buffer, &msg);
+
+ write(sendpipe_fd, buffer, len);
}
close(sendpipe_fd);
bcm2835_spi_setBitOrder(BCM2835_SPI_BIT_ORDER_MSBFIRST); // The default
bcm2835_spi_setDataMode(BCM2835_SPI_MODE0); // The default
- bcm2835_spi_setClockDivider(BCM2835_SPI_CLOCK_DIVIDER_32768); // The default
+ bcm2835_spi_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;
}
-
-static float accel_to_g(int data, unsigned int g_range)
-{
- float tmp = 32768 / g_range;
- tmp = data / tmp;
- return tmp;
-}
-static float gyro_to_dps(int data, unsigned int dps_range)
-{
- float tmp = 32768 / dps_range;
- tmp = data / tmp;
- return tmp;
-}
#include <common/mavlink.h>
const uint8_t MAV_SYS_ID = 1;
+const uint8_t MAV_COMP_ID = MAV_COMP_ID_AUTOPILOT1;
+const uint8_t MAV_CHAN_ID = MAVLINK_COMM_0;
#endif
\ No newline at end of file
#include <chrono>
#include <thread>
-#define UDP_LOGLEVEL spdlog::level::debug
+#define UDP_LOGLEVEL spdlog::level::info
#define CLICK_LOGLEVEL spdlog::level::debug
#define CONTROLLER_LOGLEVEL spdlog::level::debug
mavlink_msg_autopilot_version_encode(
MAV_SYS_ID,
- MAV_COMP_ID_AUTOPILOT1,
+ MAV_COMP_ID,
&response_msg,
&version_struct
);
static void heartbeat_thread()
{
mavlink_message_t message;
- mavlink_msg_heartbeat_pack_chan(
- 1, // System ID
- MAV_COMP_ID_AUTOPILOT1,
- MAVLINK_COMM_0,
+ mavlink_msg_heartbeat_pack(
+ MAV_SYS_ID, // System ID
+ MAV_COMP_ID,
&message,
MAV_TYPE_GENERIC,
MAV_AUTOPILOT_GENERIC,
projects / flygplan.git / commitdiff