#include <unistd.h>
#include <array>
#include <sys/ioctl.h>
+
+#include <string>
+
#include <common/mavlink.h>
+#include <spdlog/spdlog.h>
-#include "util.h"
#include "system_def.h"
+static const char GNSSRTK_RSP_INIT ='$';
+static const char GNSSRTK_RSP_DELIMITER = ',';
+static const std::string GNSSRTK_RSP_GNGGA = "$GNGGA";
+static const std::string GNSSRTK_RSP_GNRMC = "$GNRMC";
+
+static const int GNSSRTK_DRV_TX_BUFFER_SIZE = 100;
+static const int GNSSRTK_DRV_RX_BUFFER_SIZE = 300;
+
static int serial_fd;
static std::shared_ptr<spdlog::logger> logger;
-err_t init_uart(std::shared_ptr<spdlog::logger> logg)
+typedef struct {
+ std::string time;
+ std::string latitude;
+ std::string latitude_hem;
+ std::string longitude;
+ std::string longitude_hem;
+ std::string status; // 0 = no fix, 1 = GPS fix, 2 = DGPS fix, 3 = PPS fix, 4 = RTK fixed, 5 = RTK float
+ std::string number_of_sattelites;
+ std::string h_dilution_of_pos;
+ std::string altitude;
+} gnssrtk_gngga_t;
+
+typedef struct {
+ std::string time;
+ std::string status;
+ std::string latitude;
+ std::string latitude_hem;
+ std::string longitude;
+ std::string longitude_hem;
+ std::string ground_speed;
+ std::string ground_heading;
+ std::string date;
+ std::string magnetic_declination;
+ std::string magnetic_declination_direction;
+ std::string mode;
+} gnssrtk_gnrmc_t;
+
+static AtomicWrap<gnssrtk_gngga_t> latest_gngga;
+static AtomicWrap<gnssrtk_gnrmc_t> latest_gnrmc;
+
+err_t init_uart(std::shared_ptr<spdlog::logger> logg,
+ const std::string& device,
+ unsigned int baud_rate)
{
logger = logg;
struct termios options;
- serial_fd = open(SERIAL_DEVICE, O_RDONLY | O_NOCTTY);
+ serial_fd = open(device.c_str(), O_RDONLY | O_NOCTTY);
if (serial_fd == -1)
{
logger->error("Failed to open serial port.");
options.c_cflag |= CREAD; // Enable receiver
options.c_lflag &= ~(ICANON | ISIG); // Turn off canonical mode and signals
- cfsetspeed(&options, BAUD_RATE);
+ cfsetspeed(&options, baud_rate);
if (tcsetattr(serial_fd, TCSANOW, &options))
{
return OK;
}
-void application_task(pipe_fd_t sendpipe_fd)
+static gnssrtk_gngga_t parse_gngga(std::string& NMEA_msg)
+{
+ gnssrtk_gngga_t gngga_msg;
+ std::size_t pos = 0;
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gngga_msg.time = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gngga_msg.latitude = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gngga_msg.latitude_hem = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gngga_msg.longitude = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gngga_msg.longitude_hem = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gngga_msg.status = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gngga_msg.number_of_sattelites = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gngga_msg.h_dilution_of_pos = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gngga_msg.altitude = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ return gngga_msg;
+}
+
+static gnssrtk_gnrmc_t parse_gnrmc(std::string& NMEA_msg)
+{
+ gnssrtk_gnrmc_t gnrmc_msg;
+ std::size_t pos = 0;
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.time = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.status = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.latitude = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.latitude_hem= NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.longitude = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.longitude_hem = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.ground_speed = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.ground_heading = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.date = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.magnetic_declination = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.magnetic_declination_direction = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER);
+ gnrmc_msg.mode = NMEA_msg.substr(0, pos);
+ NMEA_msg.erase(0, pos + 1);
+
+ return gnrmc_msg;
+}
+
+[[noreturn]] void uart_task()
{
char recv_buffer[GNSSRTK_DRV_RX_BUFFER_SIZE];
ioctl(serial_fd, TCFLSH, 2); // flush
}
}
- auto NMEA_msg = std::string(recv_buffer);
- logger->debug("GNSS message recieved: {}", NMEA_msg);
+ auto msg = std::string(recv_buffer);
+
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));
+ pos = msg.find(GNSSRTK_RSP_DELIMITER);
+ auto header = msg.substr(0, pos);
+ msg.erase(0, pos + 1);
+
+
+
+ if (header == GNSSRTK_RSP_GNGGA)
+ {
+ latest_gngga.set(parse_gngga(msg));
+ logger->debug("GNGGA message recieved and parsed.");
+ }
+ else if (header == GNSSRTK_RSP_GNRMC)
+ {
+ latest_gnrmc.set(parse_gnrmc(msg));
+ logger->debug("GNRMC message recieved and parsed.");
+ }
+ else
+ {
+ logger->warn("Unknown NMEA-header: {}", header);
+ }
+ }
+}
+
+[[noreturn]] void mavlink_task(pipe_fd_t send_pipe)
+{
+ while (true)
+ {
+ THREAD_SLEEP(GNSS_MAVLINK_PERIOD);
+
+ auto gngga = latest_gngga.get();
+ auto gnrmc = latest_gnrmc.get();
+
+ if (gngga.latitude != gnrmc.latitude || gngga.longitude != gnrmc.longitude)
+ {
+ logger->warn("GNGGA and GNRMC position information mismatch.");
+ }
+
+ logger->debug("Constructing MAVLINK messages.");
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;
+ mavlink_gps_raw_int_t raw_gps;
+
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ int len;
+ double u_time_ms = system_uptime_ms();
+
+ position.time_boot_ms = static_cast<uint32_t>(u_time_ms);
+ position.lat = static_cast<int32_t>((std::stoi(gngga.latitude.substr(0, 2)) + (std::stod(gngga.latitude.substr(2)) / 60)) * 1e7);
+ position.lon = static_cast<int32_t>((std::stoi(gngga.longitude.substr(0, 3)) + (std::stod(gngga.longitude.substr(3)) / 60)) * 1e7);
+ position.alt = static_cast<int32_t>(std::stod(gngga.altitude) * 1000);
+ position.relative_alt = INT32_MAX; // TODO?
position.vx = INT16_MAX;
position.vy = INT16_MAX;
- position.vx = INT16_MAX;
- position.hdg = UINT16_MAX;
+ position.vz = INT16_MAX;
+ position.hdg = UINT16_MAX; // TODO
+
+ raw_gps.time_usec = static_cast<uint32_t>(u_time_ms * 1000);
+ raw_gps.fix_type = static_cast<uint8_t>(std::stoi(gngga.status)); // TODO this is wrong
+ raw_gps.lat = position.lat;
+ raw_gps.lon = position.lon;
+ raw_gps.alt = position.alt;
+ raw_gps.eph = static_cast<uint16_t>(std::stod(gngga.h_dilution_of_pos) * 100);
+ raw_gps.epv = UINT16_MAX;
+ raw_gps.vel = static_cast<uint16_t>(std::stod(gnrmc.ground_speed) * 51.44); // knots to 100 m/s
+ raw_gps.cog = UINT16_MAX;
+ raw_gps.satellites_visible = static_cast<uint8_t>(std::stoi(gngga.number_of_sattelites));
mavlink_msg_global_position_int_encode(
MAV_SYS_ID,
&position
);
- uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
- int len;
+ len = mavlink_msg_to_send_buffer(buffer, &mavlink_msg);
+ write(send_pipe, buffer, len);
+
+ mavlink_msg_gps_raw_int_encode(
+ MAV_SYS_ID,
+ MAV_COMP_ID,
+ &mavlink_msg,
+ &raw_gps
+ );
len = mavlink_msg_to_send_buffer(buffer, &mavlink_msg);
- write(sendpipe_fd, buffer, len);
-
+ write(send_pipe, buffer, len);
}
}
projects / flygplan.git / commitdiff