AP_NMEA_Output: add params and optimized

libraries/AP_NMEA_Output/AP_NMEA_Output.cpp

@@ -16,6 +16,7 @@
    Author: Francisco Ferreira (some code is copied from sitl_gps.cpp)
 
  */
+
 #define ALLOW_DOUBLE_MATH_FUNCTIONS
 
 #include "AP_NMEA_Output.h"
@@ -27,14 +28,45 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_GPS/AP_GPS.h>
 #include <AP_SerialManager/AP_SerialManager.h>
-
+#include <AP_Common/NMEA.h>
 #include <stdio.h>
 #include <time.h>
 
-AP_NMEA_Output::AP_NMEA_Output()
+#ifndef AP_NMEA_OUTPUT_MESSAGE_ENABLED_DEFAULT
-{
+#define AP_NMEA_OUTPUT_MESSAGE_ENABLED_DEFAULT      3   // GPGGA and GPRMC
-    AP_SerialManager& sm = AP::serialmanager();
+#endif
 
+#define AP_NMEA_OUTPUT_INTERVAL_MS_MIN              10
+#define AP_NMEA_OUTPUT_INTERVAL_MS_MAX              5000
+
+extern const AP_HAL::HAL& hal;
+
+const AP_Param::GroupInfo AP_NMEA_Output::var_info[] = {
+
+    // @Param: RATE_MS
+    // @DisplayName: NMEA Output rate
+    // @Description: NMEA Output rate. This controls the interval at which all the enabled NMEA messages are sent. Most NMEA systems expect 100ms (10Hz) or slower.
+    // @Range: 20 2000
+    // @Increment: 1
+    // @Units: ms
+    // @User: Standard
+    AP_GROUPINFO("RATE_MS", 1, AP_NMEA_Output, _interval_ms, 100),
+
+    // @Param: MSG_EN
+    // @DisplayName: Messages Enable bitmask
+    // @Description: This is a bitmask of enabled NMEA messages. All messages will be sent consecutively at the same rate interval
+    // @Bitmask: 0:GPGGA,1:GPRMC,2:PASHR
+    // @User: Standard
+    AP_GROUPINFO("MSG_EN", 2, AP_NMEA_Output, _message_enable_bitmask, AP_NMEA_OUTPUT_MESSAGE_ENABLED_DEFAULT),
+
+    AP_GROUPEND
+};
+
+void AP_NMEA_Output::init()
+{
+    const AP_SerialManager& sm = AP::serialmanager();
+
+    _num_outputs = 0;
     for (uint8_t i = 0; i < ARRAY_SIZE(_uart); i++) {
         _uart[i] = sm.find_serial(AP_SerialManager::SerialProtocol_NMEAOutput, i);
 
@@ -45,34 +77,15 @@ AP_NMEA_Output::AP_NMEA_Output()
     }
 }
 
-AP_NMEA_Output* AP_NMEA_Output::probe()
-{
-    AP_NMEA_Output *ret = new AP_NMEA_Output();
-    if (ret == nullptr || ret->_num_outputs == 0) {
-        delete ret;
-        return nullptr;
-    }
-    return ret;
-}
-
-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()
 {
+    if (_num_outputs == 0 || _message_enable_bitmask == 0) {
+        return;
+    }
+
     const uint32_t now_ms = AP_HAL::millis();
 
-    // only send at 10Hz
+    if ((now_ms - _last_run_ms) < static_cast<uint32_t>(MAX(_interval_ms.get(), 20))) {
-    if ((now_ms - _last_run_ms) < 100) {
         return;
     }
     _last_run_ms = now_ms;
@@ -83,6 +96,8 @@ void AP_NMEA_Output::update()
         return;
     }
 
+    uint32_t space_required = 0;
+
     // not completely accurate, our time includes leap seconds and time_t should be without
     const time_t time_sec = time_usec / 1000000;
     struct tm* tm = gmtime(&time_sec);
@@ -91,15 +106,15 @@ void AP_NMEA_Output::update()
     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);
-
-    auto &ahrs = AP::ahrs();
-
     // get location (note: get_location from AHRS always returns true after having GPS position once)
     Location loc;
-    bool pos_valid = ahrs.get_location(loc);
+#ifndef HAL_BUILD_AP_PERIPH
+    auto &ahrs = AP::ahrs();
+    const bool pos_valid = ahrs.get_location(loc);
+#else
+    const bool pos_valid = AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D;
+    loc = AP::gps().location();
+#endif
 
     // format latitude
     char lat_string[13];
@@ -112,7 +127,6 @@ void AP_NMEA_Output::update()
             min_dec,
             loc.lat < 0 ? 'S' : 'N');
 
-
     // format longitude
     char lng_string[14];
     deg = fabs(loc.lng * 1.0e-7f);
@@ -124,9 +138,12 @@ void AP_NMEA_Output::update()
             min_dec,
             loc.lng < 0 ? 'W' : 'E');
 
+
+    char gga[100];
+    uint16_t gga_length = 0;
+    if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::GPGGA)) != 0) {
         // format GGA message
-    char* gga = nullptr;
+        gga_length = nmea_printf_buffer(gga, sizeof(gga),
-    int16_t gga_res = asprintf(&gga,
                                     "$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,",
                                     tstring,
                                     lat_string,
@@ -135,20 +152,31 @@ void AP_NMEA_Output::update()
                                     pos_valid ? 6 : 3,
                                     2.0,
                                     loc.alt * 0.01f);
-    if (gga_res == -1) {
+
-        return;
+        space_required += gga_length;
     }
-    char gga_end[6];
+
-    snprintf(gga_end, sizeof(gga_end), "*%02X\r\n", (unsigned) _nmea_checksum(gga));
+    char rmc[100];
+    uint16_t rmc_length = 0;
+    if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::GPRMC)) != 0) {
+        // format date string
+        char dstring[7];
+        snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100);
 
         // get speed
-    Vector2f speed = ahrs.groundspeed_vector();
+#ifndef HAL_BUILD_AP_PERIPH
-    float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
+        const Vector2f speed = ahrs.groundspeed_vector();
-    float heading = wrap_360(degrees(atan2f(speed.x, speed.y)));
+        const float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
+        const float heading = wrap_360(degrees(atan2f(speed.x, speed.y)));
+#else
+        const float speed_knots = AP::gps().ground_speed() * M_PER_SEC_TO_KNOTS;
+        const float heading = AP::gps().ground_course();
+#endif
+
+
 
         // format RMC message
-    char* rmc = nullptr;
+        rmc_length = nmea_printf_buffer(rmc, sizeof(rmc),
-    int16_t rmc_res = asprintf(&rmc,
                                     "$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
                                     tstring,
                                     pos_valid ? 'A' : 'V',
@@ -157,14 +185,14 @@ void AP_NMEA_Output::update()
                                     speed_knots,
                                     heading,
                                     dstring);
-    if (rmc_res == -1) {
+
-        free(gga);
+        space_required += rmc_length;
-        return;
     }
-    char rmc_end[6];
-    snprintf(rmc_end, sizeof(rmc_end), "*%02X\r\n", (unsigned) _nmea_checksum(rmc));
-
 
+    uint16_t pashr_length = 0;
+    char pashr[100];
+#ifndef HAL_BUILD_AP_PERIPH
+    if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::PASHR)) != 0) {
         // get roll, pitch, yaw
         const float roll_deg = wrap_180(degrees(ahrs.get_roll()));
         const float pitch_deg = wrap_180(degrees(ahrs.get_pitch()));
@@ -190,8 +218,7 @@ void AP_NMEA_Output::update()
                                         (!ahrs.have_inertial_nav() || AP::ins().accel_calibrated_ok_all());
 
         // format PASHR message
-    char* pashr = nullptr;
+        pashr_length = nmea_printf_buffer(pashr, sizeof(pashr),
-    int16_t pashr_res = asprintf(&pashr,
                                 "$PASHR,%s,%.2f,T,%c%.2f,%c%.2f,%c%.2f,%.3f,%.3f,%.3f,%u,%u",
                                 tstring,
                                 yaw_deg, // This is a TRUE NORTH value
@@ -203,16 +230,10 @@ void AP_NMEA_Output::update()
                                 heading_deg_accuracy,
                                 (unsigned)gps_status_flag,
                                 (unsigned)ins_status_flag);
-    if (pashr_res == -1) {
+
-        free(gga);
+        space_required += pashr_length;
-        free(rmc);
-        return;
     }
-    char pashr_end[6];
+#endif
-    snprintf(pashr_end, sizeof(pashr_end), "*%02X\r\n", (unsigned) _nmea_checksum(pashr));
-
-
-    const uint32_t space_required = strlen(gga) + strlen(gga_end) + strlen(rmc) + strlen(rmc_end) + strlen(pashr) + strlen(pashr_end);
 
     // send to all NMEA output ports
     for (uint8_t i = 0; i < _num_outputs; i++) {
@@ -220,19 +241,19 @@ void AP_NMEA_Output::update()
             continue;
         }
 
+        if (gga_length > 0) {
             _uart[i]->write(gga);
-        _uart[i]->write(gga_end);
-
-        _uart[i]->write(rmc);
-        _uart[i]->write(rmc_end);
-
-        _uart[i]->write(pashr);
-        _uart[i]->write(pashr_end);
         }
 
-    free(gga);
+        if (rmc_length > 0) {
-    free(rmc);
+            _uart[i]->write(rmc);
-    free(pashr);
+        }
+
+        if (pashr_length > 0) {
+            _uart[i]->write(pashr);
+        }
+    }
 }
 
+
 #endif  // HAL_NMEA_OUTPUT_ENABLED

libraries/AP_NMEA_Output/AP_NMEA_Output.h

@@ -19,11 +19,7 @@
 
 #pragma once
 
-#include <AP_HAL/AP_HAL.h>
+#include "AP_NMEA_Output_config.h"
-
-#ifndef HAL_NMEA_OUTPUT_ENABLED
-#define HAL_NMEA_OUTPUT_ENABLED !HAL_MINIMIZE_FEATURES
-#endif
 
 #if HAL_NMEA_OUTPUT_ENABLED
 
@@ -31,25 +27,41 @@
 #define NMEA_MAX_OUTPUTS 3
 #endif
 
+#include <AP_Param/AP_Param.h>
+
 class AP_NMEA_Output {
 
 public:
-    static AP_NMEA_Output* probe();
+
+    AP_NMEA_Output() {
+        // setup parameter defaults
+        AP_Param::setup_object_defaults(this, var_info);
+    }
 
     /* Do not allow copies */
     CLASS_NO_COPY(AP_NMEA_Output);
 
     void update();
 
-private:
+    void init();
-    AP_NMEA_Output();
 
-    uint8_t _nmea_checksum(const char *str);
+    enum class Enabled_Messages {
+        GPGGA   = (1<<0),
+        GPRMC   = (1<<1),
+        PASHR   = (1<<2),
+    };
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
 
     uint8_t _num_outputs;
     AP_HAL::UARTDriver* _uart[NMEA_MAX_OUTPUTS];
 
     uint32_t _last_run_ms;
+
+    AP_Int16 _interval_ms;
+    AP_Int16 _message_enable_bitmask;
 };
 
-#endif  // !HAL_MINIMIZE_FEATURES
+#endif  // !HAL_NMEA_OUTPUT_ENABLED

libraries/AP_NMEA_Output/AP_NMEA_Output_config.h

@@ -0,0 +1,7 @@
+#include <AP_HAL/AP_HAL_Boards.h>
+#include <GCS_MAVLink/GCS_config.h>
+
+// Needs SerialManager + (AHRS or GPS)
+#ifndef HAL_NMEA_OUTPUT_ENABLED
+    #define HAL_NMEA_OUTPUT_ENABLED !HAL_MINIMIZE_FEATURES && HAL_GCS_ENABLED
+#endif