- 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.
+ - GNSS. add GPS raw data with number of satelittes etc etc.
+ - add support for more NMEA commands, especially $GNRMC
+ - Start working on xbox controller, PWM, ESC, etc.
+ - Where is the controll-loop, autonomous?, how should xbox-controller inputs be handled?
+
add_subdirectory(udp_server)
add_subdirectory(click_reader)
add_subdirectory(inter_proc)
-add_subdirectory(controller)
\ No newline at end of file
+add_subdirectory(controller)
+add_subdirectory(GNSS_reader)
+
+add_library(common_lib OBJECT common.cpp)
\ No newline at end of file
--- /dev/null
+add_executable(GNSS_reader
+ main.cpp
+ gnssrtk.cpp
+)
+
+target_include_directories(GNSS_reader PRIVATE include)
+
+target_link_libraries(GNSS_reader PRIVATE MAVLink)
+target_link_libraries(GNSS_reader PRIVATE spdlog::spdlog)
+target_link_libraries(GNSS_reader PRIVATE common_lib)
+
+target_include_directories(GNSS_reader PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include)
+
+
--- /dev/null
+#include "gnssrtk.h"
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <array>
+#include <sys/ioctl.h>
+#include <common/mavlink.h>
+
+#include "util.h"
+#include "system_def.h"
+
+static int serial_fd;
+static std::shared_ptr<spdlog::logger> logger;
+
+err_t init_uart(std::shared_ptr<spdlog::logger> logg)
+{
+ logger = logg;
+
+ struct termios options;
+ serial_fd = open(SERIAL_DEVICE, O_RDONLY | O_NOCTTY);
+ if (serial_fd == -1)
+ {
+ logger->error("Failed to open serial port.");
+ return ERR;
+ }
+
+ tcgetattr(serial_fd, &options);
+
+ // Set 8 data bits
+ options.c_cflag &= ~CSIZE;
+ options.c_cflag |= CS8;
+
+ options.c_cflag &= ~PARENB; // Set no parity
+ options.c_cflag &= ~CSTOPB; // 1 stop bit
+ options.c_lflag &= ~ECHO; // no echo
+
+ options.c_cflag |= HUPCL; // Turn on hangup on close
+ options.c_cflag |= CLOCAL; // Set local mode on
+ options.c_cflag |= CREAD; // Enable receiver
+ options.c_lflag &= ~(ICANON | ISIG); // Turn off canonical mode and signals
+
+ cfsetspeed(&options, BAUD_RATE);
+
+ if (tcsetattr(serial_fd, TCSANOW, &options))
+ {
+ logger->error("Error setting options on port.");
+ return ERR;
+ }
+
+ return OK;
+}
+
+void application_task(pipe_fd_t sendpipe_fd)
+{
+ char recv_buffer[GNSSRTK_DRV_RX_BUFFER_SIZE];
+ ioctl(serial_fd, TCFLSH, 2); // flush
+
+ while (1)
+ {
+ while (1)
+ {
+ read(serial_fd, recv_buffer, 1);
+ if (recv_buffer[0] == GNSSRTK_RSP_INIT)
+ {
+ break;
+ }
+ }
+
+ for (int i = 1; i < GNSSRTK_DRV_RX_BUFFER_SIZE; i++)
+ {
+ read(serial_fd, recv_buffer + i, 1);
+ if (recv_buffer[i] == '\n')
+ {
+ recv_buffer[i] = '\0';
+ break;
+ }
+ if (i == GNSSRTK_DRV_RX_BUFFER_SIZE)
+ {
+ logger->error("GNSS RX buffer too small or other error.");
+ recv_buffer[i - 1] = '\0';
+ break;
+ }
+ }
+
+ auto NMEA_msg = std::string(recv_buffer);
+ logger->debug("GNSS message recieved: {}", NMEA_msg);
+
+ std::size_t pos;
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ auto header = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ auto time = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ auto lat = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ auto lat_hem = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ auto lon = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ auto lon_hem = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ auto status = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ auto num_of_sattelites = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ auto HDOP = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ auto alt = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ // logger->info("header: {}", header);
+ // logger->info("time: {}", time);
+ // logger->info("lat: {}", lat);
+ // logger->info("lat_hem: {}", lat_hem);
+ // logger->info("lon: {}", lon);
+ // logger->info("lon_hem: {}", lon_hem);
+ // logger->info("status: {}", status);
+ // logger->info("num_of_sattelites: {}", num_of_sattelites);
+ // logger->info("hdop: {}", HDOP);
+ // logger->info("alt: {}", alt);
+
+ logger->debug("Lat: {}, Lon: {}", static_cast<int32_t>((std::stoi(lat.substr(0, 2)) + (std::stod(lat.substr(2)) / 60)) * 1e7), static_cast<int32_t>((std::stoi(lon.substr(0, 3)) + (std::stod(lon.substr(3)) / 60)) * 1e7));
+
+ mavlink_message_t mavlink_msg;
+ mavlink_global_position_int_t position;
+ position.time_boot_ms = system_uptime_ms();
+ position.lat = static_cast<int32_t>((std::stoi(lat.substr(0, 2)) + (std::stod(lat.substr(2)) / 60)) * 1e7);
+ position.lon = static_cast<int32_t>((std::stoi(lon.substr(0, 3)) + (std::stod(lon.substr(3)) / 60)) * 1e7);
+ position.alt = static_cast<int32_t>(std::stod(alt) * 1000);
+ position.relative_alt = INT32_MAX;
+ position.vx = INT16_MAX;
+ position.vy = INT16_MAX;
+ position.vx = INT16_MAX;
+ position.hdg = UINT16_MAX;
+
+ mavlink_msg_global_position_int_encode(
+ MAV_SYS_ID,
+ MAV_COMP_ID,
+ &mavlink_msg,
+ &position
+ );
+
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ int len;
+
+ len = mavlink_msg_to_send_buffer(buffer, &mavlink_msg);
+ write(sendpipe_fd, buffer, len);
+
+ }
+}
+
+void close_uart()
+{
+ close(serial_fd);
+}
\ No newline at end of file
--- /dev/null
+#ifndef GNSSRTK_H
+#define GNSSRTK_H
+
+#include <termios.h>
+#include <memory>
+#include <string>
+
+#include <spdlog/spdlog.h>
+
+#include "util.h"
+#include "pipe_def.h"
+
+#define GNSSRTK_RSP_INIT '$'
+#define GNSSRTK_RSP_DELIMITER ","
+#define GNSSRTK_RSP_GNGGA "$GNGGA"
+
+#define GNSSRTK_GNGGA_ELEMENT_SIZE 15
+#define GNSSRTK_GNGGA_TIME 1
+#define GNSSRTK_GNGGA_LATITUDE 2
+#define GNSSRTK_GNGGA_LATITUDE_SIDE 3
+#define GNSSRTK_GNGGA_LONGITUDE 4
+#define GNSSRTK_GNGGA_LONGITUDE_SIDE 5
+#define GNSSRTK_GNGGA_QUALITY_INDICATOR 6
+#define GNSSRTK_GNGGA_NUMBER_OF_SATELLITES 7
+#define GNSSRTK_GNGGA_H_DILUTION_OF_POS 8
+#define GNSSRTK_GNGGA_ALTITUDE 9
+#define GNSSRTK_GNGGA_ALTITUDE_UNIT 10
+#define GNSSRTK_GNGGA_GEOIDAL_SEPARATION 11
+#define GNSSRTK_GNGGA_GEOIDAL_SEPARATION_UNIT 12
+#define GNSSRTK_GNGGA_TIME_SINCE_LAST_DGPS 13
+#define GNSSRTK_GNGGA_DGPS_REFERENCE_STATION_ID 14
+
+#define GNSSRTK_DUMMY 0xFF
+
+#define GNSSRTK_DRV_TX_BUFFER_SIZE 100
+#define GNSSRTK_DRV_RX_BUFFER_SIZE 300
+
+#define BAUD_RATE B38400
+#define SERIAL_DEVICE "/dev/serial0"
+
+err_t init_uart(std::shared_ptr<spdlog::logger>);
+void application_task(pipe_fd_t);
+
+void close_uart();
+
+#endif
\ No newline at end of file
--- /dev/null
+// Standard
+#include <memory>
+#include <thread>
+
+// Pipes
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+// Libs
+#include <spdlog/spdlog.h>
+#include <spdlog/sinks/stdout_color_sinks.h>
+
+// Local
+#include "pipe_def.h"
+#include "util.h"
+#include "gnssrtk.h"
+
+#define BAUD_RATE B38400
+
+static std::shared_ptr<spdlog::logger> logger;
+
+int main(int argc, char* argv[])
+{
+ logger = spdlog::stdout_color_mt("GNSS_reader");
+ logger->set_level(GNSS_READER_LOGLEVEL);
+
+ logger->debug("Opening sendpipe for writing.");
+ pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY);
+
+ logger->info("Initializing UART.");
+ int ret = init_uart(logger);
+ if (ret != OK)
+ {
+ return 1;
+ }
+
+ // Mainloop
+ application_task(sendpipe_fd);
+
+ close_uart();
+ close(sendpipe_fd);
+ //close(recvpipe_fd);
+ return 0;
+}
+
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})
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)
{
}
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];
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[])
{
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};
mavlink_message_t msg;
mavlink_attitude_t attitude;
+ mavlink_scaled_imu_t scaled_imu;
// Control loop
while (true)
{
- THREAD_SLEEP(1ms);
+ THREAD_SLEEP(10ms);
res = c6dofimu17_get_gyro_data(&gyro_data);
t_end = 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 accel data: {}", res);
+ logger->warn("Failed to get imu data: {}", res);
continue;
}
comp_filter.update(gyro_vec, accel_vec, time_d);
- // Pack attitude into MAVLINK package
- attitude.time_boot_ms = 0;
+ // 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.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,
);
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
- const int len = mavlink_msg_to_send_buffer(buffer, &msg);
-
+ 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);
}
bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW); // the default
return OK;
-}
+}
\ No newline at end of file
--- /dev/null
+#include "include/util.h"
+#include <fstream>
+
+double system_uptime_ms()
+{
+ double t_ms;
+ double t_s;
+ if (std::ifstream("/proc/uptime", std::ios::in) >> t_s)
+ {
+ t_ms = t_s * 1000.0;
+ }
+ else
+ {
+ return -1;
+ }
+ return t_ms;
+}
\ No newline at end of file
#define UDP_LOGLEVEL spdlog::level::info
#define CLICK_LOGLEVEL spdlog::level::debug
#define CONTROLLER_LOGLEVEL spdlog::level::debug
+#define GNSS_READER_LOGLEVEL spdlog::level::debug
#define THREAD_SLEEP(time) std::this_thread::sleep_for(time)
using namespace std::chrono_literals;
OK = 0
} err_t;
#endif
+
+double system_uptime_ms();
+
#endif
\ No newline at end of file
./controller &
# start click reader
-./click_reader
+./click_reader &
+
+# start GNSS reader
+./GNSS_reader
wait
projects / flygplan.git / commitdiff