From 80093f41b0de788d5077e89ba3604e437a4a7450 Mon Sep 17 00:00:00 2001 From: Francisco Ferreira Date: Mon, 4 Mar 2019 18:13:01 +0000 Subject: [PATCH] AP_NMEA_Output: new library for writing NMEA to serial ports --- libraries/AP_NMEA_Output/AP_NMEA_Output.cpp | 181 ++++++++++++++++++++ libraries/AP_NMEA_Output/AP_NMEA_Output.h | 53 ++++++ 2 files changed, 234 insertions(+) create mode 100644 libraries/AP_NMEA_Output/AP_NMEA_Output.cpp create mode 100644 libraries/AP_NMEA_Output/AP_NMEA_Output.h diff --git a/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp new file mode 100644 index 0000000000..8e694ef5ff --- /dev/null +++ b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp @@ -0,0 +1,181 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + + Author: Francisco Ferreira (some code is copied from sitl_gps.cpp) + + */ + +#include "AP_NMEA_Output.h" + +#if !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE + +#include +#include +#include + +#include +#include + +AP_NMEA_Output* AP_NMEA_Output::_singleton; + +AP_NMEA_Output::AP_NMEA_Output() +{ + AP_SerialManager& sm = AP::serialmanager(); + + for (uint8_t i = 0; i < ARRAY_SIZE(_uart); i++) { + _uart[i] = sm.find_serial(AP_SerialManager::SerialProtocol_NMEAOutput, i); + + if (_uart[i] == nullptr) { + break; + } + _num_outputs++; + } +} + +AP_NMEA_Output* AP_NMEA_Output::probe() +{ + if (!_singleton && AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_NMEAOutput, 0) != nullptr) { + _singleton = new AP_NMEA_Output(); + } + + return _singleton; +} + +uint8_t AP_NMEA_Output::_nmea_checksum(const char *str) +{ + uint8_t checksum = 0; + const uint8_t* bytes = (const uint8_t*) str; + + for (uint16_t i = 1; str[i]; i++) { + checksum ^= bytes[i]; + } + + return checksum; +} + +void AP_NMEA_Output::update() +{ + uint16_t now = AP_HAL::millis16(); + uint16_t time_diff = now - _last_sent; + + // only send at 10Hz + if (time_diff < 100) { + return; + } + + // get time and date + uint64_t time_usec; + bool time_valid = AP::rtc().get_utc_usec(time_usec); + + if (!time_valid) { + return; + } + + // not completely accurate, our time includes leap seconds and time_t should be without + time_t time_sec = time_usec / 1000000; + struct tm* tm = gmtime(&time_sec); + + // format time string + char tstring[11]; + snprintf(tstring, sizeof(tstring), "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6); + + // format date string + char dstring[7]; + snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100); + + AP_AHRS_NavEKF& ahrs = AP::ahrs_navekf(); + + // get location (note: get_position from AHRS always returns true after having GPS position once) + Location loc; + bool pos_valid = ahrs.get_location(loc); + + // format latitude + char lat_string[13]; + float deg = fabsf(loc.lat * 1.0e-7f); + snprintf(lat_string, + sizeof(lat_string), + "%02u%08.5f,%c", + (unsigned) deg, + double((deg - int(deg)) * 60), + loc.lat < 0 ? 'S' : 'N'); + + // format longitude + char lng_string[14]; + deg = fabsf(loc.lng * 1.0e-7f); + snprintf(lng_string, + sizeof(lng_string), + "%03u%08.5f,%c", + (unsigned) deg, + double((deg - int(deg)) * 60), + loc.lng < 0 ? 'W' : 'E'); + + // format GGA message + char* gga = nullptr; + int16_t gga_res = asprintf(&gga, + "$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,", + tstring, + lat_string, + lng_string, + pos_valid ? 1 : 0, + pos_valid ? 6 : 3, + 2.0, + loc.alt * 0.01f); + char gga_end[6]; + snprintf(gga_end, sizeof(gga_end), "*%02X\r\n", (unsigned) _nmea_checksum(gga)); + + // get speed + Vector2f speed = ahrs.groundspeed_vector(); + float speed_knots = norm(speed.x, speed.y) * M_PER_SEC_TO_KNOTS; + float heading = wrap_360(degrees(atan2f(speed.x, speed.y))); + + // format RMC message + char* rmc = nullptr; + int16_t rmc_res = asprintf(&rmc, + "$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,", + tstring, + pos_valid ? 'A' : 'V', + lat_string, + lng_string, + speed_knots, + heading, + dstring); + char rmc_end[6]; + snprintf(rmc_end, sizeof(rmc_end), "*%02X\r\n", (unsigned) _nmea_checksum(rmc)); + + // send to all NMEA output ports + for (uint8_t i = 0; i < _num_outputs; i++) { + if (gga_res != -1) { + _uart[i]->write(gga); + _uart[i]->write(gga_end); + } + + if (rmc_res != -1) { + _uart[i]->write(rmc); + _uart[i]->write(rmc_end); + } + } + + if (gga_res != -1) { + free(gga); + } + + if (rmc_res != -1) { + free(rmc); + } + + _last_sent = now; +} + +#endif // !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_NMEA_Output/AP_NMEA_Output.h b/libraries/AP_NMEA_Output/AP_NMEA_Output.h new file mode 100644 index 0000000000..a0e656b378 --- /dev/null +++ b/libraries/AP_NMEA_Output/AP_NMEA_Output.h @@ -0,0 +1,53 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + + Author: Francisco Ferreira + + */ + +#pragma once + +#include +#include + +#if !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE + +#include + +class AP_NMEA_Output { + +public: + static AP_NMEA_Output* probe(); + + /* Do not allow copies */ + AP_NMEA_Output(const AP_NMEA_Output &other) = delete; + AP_NMEA_Output &operator=(const AP_NMEA_Output&) = delete; + + void update(); + +private: + AP_NMEA_Output(); + + uint8_t _nmea_checksum(const char *str); + + static AP_NMEA_Output* _singleton; + + uint8_t _num_outputs; + AP_HAL::UARTDriver* _uart[SERIALMANAGER_NUM_PORTS]; + + uint16_t _last_sent; +}; + +#endif // !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE