From: Nils Forssén Date: Sun, 8 Jun 2025 22:35:59 +0000 (+0200) Subject: GNSS working X-Git-Url: https://gitweb.forssennils.se/?a=commitdiff_plain;h=7e930f3bb8cff278b6d2b7f82482bf8662f426df;p=flygplan.git GNSS working --- diff --git a/README.md b/README.md index 8603eaa..0cfbd3d 100755 --- a/README.md +++ b/README.md @@ -27,7 +27,8 @@ just run wvdial, correct settings should be in `/etc/wvdial.conf`. - 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? + diff --git a/processes/CMakeLists.txt b/processes/CMakeLists.txt index 2da4dd5..cfbba58 100644 --- a/processes/CMakeLists.txt +++ b/processes/CMakeLists.txt @@ -1,4 +1,7 @@ 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 diff --git a/processes/GNSS_reader/CMakeLists.txt b/processes/GNSS_reader/CMakeLists.txt new file mode 100755 index 0000000..a44c20d --- /dev/null +++ b/processes/GNSS_reader/CMakeLists.txt @@ -0,0 +1,14 @@ +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) + + diff --git a/processes/GNSS_reader/gnssrtk.cpp b/processes/GNSS_reader/gnssrtk.cpp new file mode 100644 index 0000000..92046bb --- /dev/null +++ b/processes/GNSS_reader/gnssrtk.cpp @@ -0,0 +1,173 @@ +#include "gnssrtk.h" + +#include +#include +#include +#include +#include + +#include "util.h" +#include "system_def.h" + +static int serial_fd; +static std::shared_ptr logger; + +err_t init_uart(std::shared_ptr 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((std::stoi(lat.substr(0, 2)) + (std::stod(lat.substr(2)) / 60)) * 1e7), static_cast((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((std::stoi(lat.substr(0, 2)) + (std::stod(lat.substr(2)) / 60)) * 1e7); + position.lon = static_cast((std::stoi(lon.substr(0, 3)) + (std::stod(lon.substr(3)) / 60)) * 1e7); + position.alt = static_cast(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 diff --git a/processes/GNSS_reader/include/gnssrtk.h b/processes/GNSS_reader/include/gnssrtk.h new file mode 100644 index 0000000..429eb02 --- /dev/null +++ b/processes/GNSS_reader/include/gnssrtk.h @@ -0,0 +1,46 @@ +#ifndef GNSSRTK_H +#define GNSSRTK_H + +#include +#include +#include + +#include + +#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); +void application_task(pipe_fd_t); + +void close_uart(); + +#endif \ No newline at end of file diff --git a/processes/GNSS_reader/main.cpp b/processes/GNSS_reader/main.cpp new file mode 100644 index 0000000..b905531 --- /dev/null +++ b/processes/GNSS_reader/main.cpp @@ -0,0 +1,47 @@ +// Standard +#include +#include + +// Pipes +#include +#include +#include +#include + +// Libs +#include +#include + +// Local +#include "pipe_def.h" +#include "util.h" +#include "gnssrtk.h" + +#define BAUD_RATE B38400 + +static std::shared_ptr 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; +} + diff --git a/processes/click_reader/CMakeLists.txt b/processes/click_reader/CMakeLists.txt index 0bf3a99..582866d 100644 --- a/processes/click_reader/CMakeLists.txt +++ b/processes/click_reader/CMakeLists.txt @@ -9,6 +9,7 @@ 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}) diff --git a/processes/click_reader/controller.cpp b/processes/click_reader/controller.cpp index 1ccbadc..eacfcb0 100644 --- a/processes/click_reader/controller.cpp +++ b/processes/click_reader/controller.cpp @@ -29,6 +29,7 @@ const vec2d Complementary_filter::get_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) { @@ -40,6 +41,7 @@ void Complementary_filter::update(const vec3d &gyro_meas, const vec3d &accel_mea } 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); @@ -47,36 +49,6 @@ void Complementary_filter::update(const vec3d &gyro_meas, const vec3d &accel_mea 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]; diff --git a/processes/click_reader/main.cpp b/processes/click_reader/main.cpp index 87b32d0..1bfa4c7 100644 --- a/processes/click_reader/main.cpp +++ b/processes/click_reader/main.cpp @@ -31,7 +31,6 @@ 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[]) { @@ -68,9 +67,9 @@ 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}; @@ -81,10 +80,11 @@ int main(int argc, char* argv[]) 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(); @@ -93,8 +93,10 @@ int main(int argc, char* argv[]) 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; } @@ -103,8 +105,8 @@ int main(int argc, char* argv[]) 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; @@ -112,6 +114,18 @@ int main(int argc, char* argv[]) attitude.pitchspeed = 0.0; attitude.yawspeed = 0.0; + scaled_imu.time_boot_ms = system_uptime_ms(); + scaled_imu.temperature = static_cast(temperature * 100); + scaled_imu.xacc = static_cast(accel_vec[0] * 1000); // Maybe not correct? + scaled_imu.yacc = static_cast(accel_vec[1] * 1000); // Maybe not correct? + scaled_imu.zacc = static_cast(accel_vec[2] * 1000); // Maybe not correct? + scaled_imu.xgyro = static_cast(gyro_vec[0] * 1000); // Maybe not correct? + scaled_imu.ygyro = static_cast(gyro_vec[1] * 1000); // Maybe not correct? + scaled_imu.zgyro = static_cast(gyro_vec[2] * 1000); // Maybe not correct? + scaled_imu.xmag = INT16_MAX; + scaled_imu.ymag = INT16_MAX; + scaled_imu.zmag = INT16_MAX; + mavlink_msg_attitude_encode( MAV_SYS_ID, MAV_COMP_ID, @@ -120,8 +134,18 @@ int main(int argc, char* argv[]) ); 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); } @@ -173,4 +197,4 @@ static err_t spi_init() bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW); // the default return OK; -} +} \ No newline at end of file diff --git a/processes/common.cpp b/processes/common.cpp new file mode 100644 index 0000000..c5d54ec --- /dev/null +++ b/processes/common.cpp @@ -0,0 +1,17 @@ +#include "include/util.h" +#include + +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 diff --git a/processes/include/util.h b/processes/include/util.h index 1102a34..28fbc55 100644 --- a/processes/include/util.h +++ b/processes/include/util.h @@ -7,6 +7,7 @@ #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; @@ -18,4 +19,7 @@ typedef enum { OK = 0 } err_t; #endif + +double system_uptime_ms(); + #endif \ No newline at end of file diff --git a/run.sh b/run.sh index 956bada..deadd0d 100755 --- a/run.sh +++ b/run.sh @@ -30,7 +30,10 @@ sleep 1 ./controller & # start click reader -./click_reader +./click_reader & + +# start GNSS reader +./GNSS_reader wait