Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NMEA_Output: add params and optimized

This commit is contained in:
Tom Pittenger 2023-01-10 10:33:54 -08:00 committed by Peter Barker
parent 95b4ded3e8
commit 3f5276c3d7
3 changed files with 186 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

293
libraries/AP_NMEA_Output/AP_NMEA_Output.cpp
View File

@ -16,6 +16,7 @@
Author: Francisco Ferreira (some code is copied from sitl_gps.cpp) Author: Francisco Ferreira (some code is copied from sitl_gps.cpp)
*/ */
#define ALLOW_DOUBLE_MATH_FUNCTIONS #define ALLOW_DOUBLE_MATH_FUNCTIONS
#include "AP_NMEA_Output.h" #include "AP_NMEA_Output.h"
@ -27,14 +28,45 @@
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#include <AP_SerialManager/AP_SerialManager.h> #include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Common/NMEA.h>
#include <stdio.h> #include <stdio.h>
#include <time.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++) { for (uint8_t i = 0; i < ARRAY_SIZE(_uart); i++) {
_uart[i] = sm.find_serial(AP_SerialManager::SerialProtocol_NMEAOutput, 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() void AP_NMEA_Output::update()
{ {
if (_num_outputs == 0 || _message_enable_bitmask == 0) {
return;
}
const uint32_t now_ms = AP_HAL::millis(); 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; return;
} }
_last_run_ms = now_ms; _last_run_ms = now_ms;
@ -83,6 +96,8 @@ void AP_NMEA_Output::update()
return; return;
} }
uint32_t space_required = 0;
// not completely accurate, our time includes leap seconds and time_t should be without // not completely accurate, our time includes leap seconds and time_t should be without
const time_t time_sec = time_usec / 1000000; const time_t time_sec = time_usec / 1000000;
struct tm* tm = gmtime(&time_sec); struct tm* tm = gmtime(&time_sec);
@ -91,128 +106,134 @@ void AP_NMEA_Output::update()
char tstring[11]; 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); 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) // get location (note: get_location from AHRS always returns true after having GPS position once)
Location loc; 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 // format latitude
char lat_string[13]; char lat_string[13];
double deg = fabs(loc.lat * 1.0e-7f); double deg = fabs(loc.lat * 1.0e-7f);
double min_dec = ((fabs(loc.lat) - (unsigned)deg * 1.0e7)) * 60 * 1.e-7f; double min_dec = ((fabs(loc.lat) - (unsigned)deg * 1.0e7)) * 60 * 1.e-7f;
snprintf(lat_string, snprintf(lat_string,
sizeof(lat_string), sizeof(lat_string),
"%02u%08.5f,%c", "%02u%08.5f,%c",
(unsigned) deg, (unsigned) deg,
min_dec, min_dec,
loc.lat < 0 ? 'S' : 'N'); loc.lat < 0 ? 'S' : 'N');
// format longitude // format longitude
char lng_string[14]; char lng_string[14];
deg = fabs(loc.lng * 1.0e-7f); deg = fabs(loc.lng * 1.0e-7f);
min_dec = ((fabs(loc.lng) - (unsigned)deg * 1.0e7)) * 60 * 1.e-7f; min_dec = ((fabs(loc.lng) - (unsigned)deg * 1.0e7)) * 60 * 1.e-7f;
snprintf(lng_string, snprintf(lng_string,
sizeof(lng_string), sizeof(lng_string),
"%03u%08.5f,%c", "%03u%08.5f,%c",
(unsigned) deg, (unsigned) deg,
min_dec, min_dec,
loc.lng < 0 ? 'W' : 'E'); loc.lng < 0 ? 'W' : 'E');
// format GGA message
char* gga = nullptr; char gga[100];
int16_t gga_res = asprintf(&gga, uint16_t gga_length = 0;
"$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,", if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::GPGGA)) != 0) {
tstring, // format GGA message
lat_string, gga_length = nmea_printf_buffer(gga, sizeof(gga),
lng_string, "$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,",
pos_valid ? 1 : 0, tstring,
pos_valid ? 6 : 3, lat_string,
2.0, lng_string,
loc.alt * 0.01f); pos_valid ? 1 : 0,
if (gga_res == -1) { pos_valid ? 6 : 3,
return; 2.0,
loc.alt * 0.01f);
space_required += gga_length;
} }
char gga_end[6];
snprintf(gga_end, sizeof(gga_end), "*%02X\r\n", (unsigned) _nmea_checksum(gga));
// get speed char rmc[100];
Vector2f speed = ahrs.groundspeed_vector(); uint16_t rmc_length = 0;
float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS; if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::GPRMC)) != 0) {
float heading = wrap_360(degrees(atan2f(speed.x, speed.y))); // format date string
char dstring[7];
snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100);
// format RMC message // get speed
char* rmc = nullptr; #ifndef HAL_BUILD_AP_PERIPH
int16_t rmc_res = asprintf(&rmc, const Vector2f speed = ahrs.groundspeed_vector();
"$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,", const float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
tstring, const float heading = wrap_360(degrees(atan2f(speed.x, speed.y)));
pos_valid ? 'A' : 'V', #else
lat_string, const float speed_knots = AP::gps().ground_speed() * M_PER_SEC_TO_KNOTS;
lng_string, const float heading = AP::gps().ground_course();
speed_knots, #endif
heading,
dstring);
if (rmc_res == -1) {
free(gga); // format RMC message
return; rmc_length = nmea_printf_buffer(rmc, sizeof(rmc),
"$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
tstring,
pos_valid ? 'A' : 'V',
lat_string,
lng_string,
speed_knots,
heading,
dstring);
space_required += rmc_length;
} }
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()));
const float yaw_deg = wrap_360(degrees(ahrs.get_yaw()));
const float heave_m = 0; // instantaneous heave in meters
const float roll_deg_accuracy = 0; // stddev of roll_deg;
const float pitch_deg_accuracy = 0; // stddev of pitch_deg;
const float heading_deg_accuracy = 0; // stddev of yaw_deg;
// get roll, pitch, yaw // GPS Update Quality Flag:
const float roll_deg = wrap_180(degrees(ahrs.get_roll())); // 0 = no position
const float pitch_deg = wrap_180(degrees(ahrs.get_pitch())); // 1 = All non-RTK fixed integer positions
const float yaw_deg = wrap_360(degrees(ahrs.get_yaw())); // 2 = RTK fixed integer positions
const float heave_m = 0; // instantaneous heave in meters const AP_GPS::GPS_Status gps_status = AP::gps().status();
const float roll_deg_accuracy = 0; // stddev of roll_deg; const uint8_t gps_status_flag = (gps_status >= AP_GPS::GPS_OK_FIX_3D_RTK_FIXED) ? 2 :
const float pitch_deg_accuracy = 0; // stddev of pitch_deg; (gps_status >= AP_GPS::GPS_OK_FIX_2D ? 1 : 0);
const float heading_deg_accuracy = 0; // stddev of yaw_deg;
// GPS Update Quality Flag: // INS Status Flag:
// 0 = no position // 0 = All SPAN Pre-Alignment INS Status
// 1 = All non-RTK fixed integer positions // 1 = All SPAN Post-Alignment INS Status
// 2 = RTK fixed integer positions const bool ins_status_flag = ahrs.initialised() &&
const AP_GPS::GPS_Status gps_status = AP::gps().status(); ahrs.healthy() &&
const uint8_t gps_status_flag = (gps_status >= AP_GPS::GPS_OK_FIX_3D_RTK_FIXED) ? 2 : (!ahrs.have_inertial_nav() || AP::ins().accel_calibrated_ok_all());
(gps_status >= AP_GPS::GPS_OK_FIX_2D ? 1 : 0);
// INS Status Flag: // format PASHR message
// 0 = All SPAN Pre-Alignment INS Status pashr_length = nmea_printf_buffer(pashr, sizeof(pashr),
// 1 = All SPAN Post-Alignment INS Status "$PASHR,%s,%.2f,T,%c%.2f,%c%.2f,%c%.2f,%.3f,%.3f,%.3f,%u,%u",
const bool ins_status_flag = ahrs.initialised() && tstring,
ahrs.healthy() && yaw_deg, // This is a TRUE NORTH value
(!ahrs.have_inertial_nav() || AP::ins().accel_calibrated_ok_all()); roll_deg<0? '-':'+', fabs(roll_deg), // always show + or - symbol
pitch_deg<0?'-':'+', fabs(pitch_deg), // always show + or - symbol
heave_m<0? '-':'+', fabs(heave_m), // always show + or - symbol
roll_deg_accuracy,
pitch_deg_accuracy,
heading_deg_accuracy,
(unsigned)gps_status_flag,
(unsigned)ins_status_flag);
// format PASHR message space_required += pashr_length;
char* pashr = nullptr;
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
roll_deg<0? '-':'+', fabs(roll_deg), // always show + or - symbol
pitch_deg<0?'-':'+', fabs(pitch_deg), // always show + or - symbol
heave_m<0? '-':'+', fabs(heave_m), // always show + or - symbol
roll_deg_accuracy,
pitch_deg_accuracy,
heading_deg_accuracy,
(unsigned)gps_status_flag,
(unsigned)ins_status_flag);
if (pashr_res == -1) {
free(gga);
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 // send to all NMEA output ports
for (uint8_t i = 0; i < _num_outputs; i++) { for (uint8_t i = 0; i < _num_outputs; i++) {
@ -220,19 +241,19 @@ void AP_NMEA_Output::update()
continue; continue;
} }
_uart[i]->write(gga); if (gga_length > 0) {
_uart[i]->write(gga_end); _uart[i]->write(gga);
}
_uart[i]->write(rmc); if (rmc_length > 0) {
_uart[i]->write(rmc_end); _uart[i]->write(rmc);
}
_uart[i]->write(pashr); if (pashr_length > 0) {
_uart[i]->write(pashr_end); _uart[i]->write(pashr);
}
} }
free(gga);
free(rmc);
free(pashr);
} }
#endif // HAL_NMEA_OUTPUT_ENABLED #endif // HAL_NMEA_OUTPUT_ENABLED

32
libraries/AP_NMEA_Output/AP_NMEA_Output.h
View File

@ -19,11 +19,7 @@
#pragma once #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 #if HAL_NMEA_OUTPUT_ENABLED
@ -31,25 +27,41 @@
#define NMEA_MAX_OUTPUTS 3 #define NMEA_MAX_OUTPUTS 3
#endif #endif
#include <AP_Param/AP_Param.h>
class AP_NMEA_Output { class AP_NMEA_Output {
public: public:
static AP_NMEA_Output* probe();
AP_NMEA_Output() {
// setup parameter defaults
AP_Param::setup_object_defaults(this, var_info);
}
/* Do not allow copies */ /* Do not allow copies */
CLASS_NO_COPY(AP_NMEA_Output); CLASS_NO_COPY(AP_NMEA_Output);
void update(); 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; uint8_t _num_outputs;
AP_HAL::UARTDriver* _uart[NMEA_MAX_OUTPUTS]; AP_HAL::UARTDriver* _uart[NMEA_MAX_OUTPUTS];
uint32_t _last_run_ms; uint32_t _last_run_ms;
AP_Int16 _interval_ms;
AP_Int16 _message_enable_bitmask;
}; };
#endif // !HAL_MINIMIZE_FEATURES #endif // !HAL_NMEA_OUTPUT_ENABLED

7
libraries/AP_NMEA_Output/AP_NMEA_Output_config.h Normal file
View File

@ -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