zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

add rtk mark decode,crc error

celiu-4.0.17-rc8
zbr 4 years ago
parent
81333997dd
commit
24c6fc688c
4 changed files with 91 additions and 22 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 20
      libraries/AP_GPS/AP_GPS.h
  2. 55
      libraries/AP_GPS/AP_GPS_NMEA.cpp
  3. 37
      libraries/AP_GPS/AP_GPS_NMEA.h
  4. 1
      libraries/AP_GPS/GPS_Backend.h

20
libraries/AP_GPS/AP_GPS.h
Unescape View File

@ -166,7 +166,22 @@ public: @@ -166,7 +166,22 @@ public:
uint32_t rtk_accuracy; ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999)
int32_t rtk_iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses
};
struct rtk_mark_s
{
uint16_t week;
float second;
double lat;
double lon;
double hgt;
float undulation;
float lat_sigma;
float lon_sigma;
float hgt_sigma;
float diff_age;
float sol_age;
uint8_t SVs;
uint8_t solnSVs;
};
/// Startup initialisation.
void init(const AP_SerialManager& serial_manager);
@ -486,7 +501,8 @@ private: @@ -486,7 +501,8 @@ private:
GPS_State state[GPS_MAX_INSTANCES];
AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS];
AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS];
rtk_mark_s rtk_mark[GPS_MAX_INSTANCES];
/// primary GPS instance
uint8_t primary_instance;

55
libraries/AP_GPS/AP_GPS_NMEA.cpp
Unescape View File

@ -36,6 +36,7 @@ @@ -36,6 +36,7 @@
#include "AP_GPS_NMEA.h"
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
// optionally log all NMEA data for debug purposes
@ -82,6 +83,7 @@ bool AP_GPS_NMEA::_decode(char c) @@ -82,6 +83,7 @@ bool AP_GPS_NMEA::_decode(char c)
switch (c) {
case ',': // term terminators
case ';': // term terminators
_parity ^= c;
FALLTHROUGH;
case '\r':
@ -97,6 +99,7 @@ bool AP_GPS_NMEA::_decode(char c) @@ -97,6 +99,7 @@ bool AP_GPS_NMEA::_decode(char c)
return valid_sentence;
case '$': // sentence begin
case '#': // sentence begin
_term_number = _term_offset = 0;
_parity = 0;
_sentence_type = _GPS_SENTENCE_OTHER;
@ -233,6 +236,13 @@ bool AP_GPS_NMEA::_term_complete() @@ -233,6 +236,13 @@ bool AP_GPS_NMEA::_term_complete()
return false;
}
const uint8_t checksum = (nibble_high << 4u) | nibble_low;
if(_gps_data_good && (_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT )){
if(get_new_mark){
gcs().send_text(MAV_SEVERITY_INFO, "week: %d, %f,crc:0x%02x--0x%02x",gps_mark.week,gps_mark.second,checksum,_parity);
gcs().send_text(MAV_SEVERITY_INFO, "llh: %f , %f , %f\n",gps_mark.lat,gps_mark.lon,gps_mark.hgt);
get_new_mark = false;
}
}
if (checksum == _parity) {
if (_gps_data_good) {
uint32_t now = AP_HAL::millis();
@ -296,6 +306,14 @@ bool AP_GPS_NMEA::_term_complete() @@ -296,6 +306,14 @@ bool AP_GPS_NMEA::_term_complete()
state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
state.have_gps_yaw = true;
break;
case _GPS_SENTENCE_MARK:
case _GPS_SENTENCE_EVENT:
if(get_new_mark){
gcs().send_text(MAV_SEVERITY_INFO, "week: %d, %f",gps_mark.week,gps_mark.second);
gcs().send_text(MAV_SEVERITY_INFO, "llh: %f , %f , %f\n",gps_mark.lat,gps_mark.lon,gps_mark.hgt);
get_new_mark = false;
}
break;
}
} else {
switch (_sentence_type) {
@ -328,8 +346,10 @@ bool AP_GPS_NMEA::_term_complete() @@ -328,8 +346,10 @@ bool AP_GPS_NMEA::_term_complete()
const char *term_type = &_term[2];
if (strcmp(term_type, "RMC") == 0) {
_sentence_type = _GPS_SENTENCE_RMC;
// gcs().send_text(MAV_SEVERITY_INFO, "get: %s\n",term_type);
} else if (strcmp(term_type, "GGA") == 0) {
_sentence_type = _GPS_SENTENCE_GGA;
// gcs().send_text(MAV_SEVERITY_INFO, "get: %s\n",term_type);
} else if (strcmp(term_type, "HDT") == 0) {
_sentence_type = _GPS_SENTENCE_HDT;
// HDT doesn't have a data qualifier
@ -339,6 +359,20 @@ bool AP_GPS_NMEA::_term_complete() @@ -339,6 +359,20 @@ bool AP_GPS_NMEA::_term_complete()
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
_gps_data_good = true;
// gcs().send_text(MAV_SEVERITY_INFO, "get: %s\n",term_type);
} else if (strcmp(term_type, "ENTALLA") == 0) {
_sentence_type = _GPS_SENTENCE_EVENT;
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
_gps_data_good = true;
gcs().send_text(MAV_SEVERITY_INFO, "get: %s",term_type);
} else if (strcmp(term_type, "RKPOSA") == 0) {
_sentence_type = _GPS_SENTENCE_MARK;
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
gcs().send_text(MAV_SEVERITY_INFO, "get: %s",term_type);
_gps_data_good = true;
} else {
_sentence_type = _GPS_SENTENCE_OTHER;
}
@ -418,61 +452,79 @@ bool AP_GPS_NMEA::_term_complete() @@ -418,61 +452,79 @@ bool AP_GPS_NMEA::_term_complete()
}
#if 1
static uint32_t cnt;
cnt++;
// if ((_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT ))
// gcs().send_text(MAV_SEVERITY_INFO, "%d,t:%d, n:%d,%d: %s",cnt,_sentence_type,_term_number,_term[0],_term);
// rtk event mark decode
if ((_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT )&& _term[0]) {
if ((_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT )) {
switch (_sentence_type + _term_number) {
// operational status
//
case _GPS_SENTENCE_MARK + 5: // week
case _GPS_SENTENCE_EVENT + 5: // week
gps_mark.week = atol(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%s:%d",_term,gps_mark.week);
break;
case _GPS_SENTENCE_MARK + 6: // second
case _GPS_SENTENCE_EVENT + 6: // second
gps_mark.second = atof(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.second);
break;
case _GPS_SENTENCE_MARK + 12: // lat
case _GPS_SENTENCE_EVENT + 22: // lat
gps_mark.lat = atof(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.lat);
break;
case _GPS_SENTENCE_MARK + 13: // lon
case _GPS_SENTENCE_EVENT + 23:
gps_mark.lon = atof(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.lon);
break;
case _GPS_SENTENCE_MARK + 14: // hgt
case _GPS_SENTENCE_EVENT + 24:
gps_mark.hgt = atof(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.lon);
break;
case _GPS_SENTENCE_MARK + 15: // undulation
case _GPS_SENTENCE_EVENT + 25:
gps_mark.undulation = atof(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
break;
case _GPS_SENTENCE_MARK + 17: // lat_sigma
case _GPS_SENTENCE_EVENT + 27:
gps_mark.lat_sigma = atof(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
break;
case _GPS_SENTENCE_MARK + 18: // lon_sigma
case _GPS_SENTENCE_EVENT + 28:
gps_mark.lon_sigma = atof(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
break;
case _GPS_SENTENCE_MARK + 19: // hgt_sigma
case _GPS_SENTENCE_EVENT + 29:
gps_mark.hgt_sigma = atof(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
break;
case _GPS_SENTENCE_MARK + 21: // diff_age
case _GPS_SENTENCE_EVENT + 31:
gps_mark.diff_age = atof(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
break;
case _GPS_SENTENCE_MARK + 22: // sol_age
case _GPS_SENTENCE_EVENT + 32:
gps_mark.sol_age = atof(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
break;
case _GPS_SENTENCE_MARK + 23: // SVs
case _GPS_SENTENCE_EVENT + 33:
gps_mark.SVs = atol(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
case _GPS_SENTENCE_MARK + 24: // solnSVs
case _GPS_SENTENCE_EVENT + 34:
gps_mark.solnSVs = atol(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "--- %d,%s \n",_term_number,_term);
get_new_mark = true;
break;
}
@ -480,7 +532,6 @@ bool AP_GPS_NMEA::_term_complete() @@ -480,7 +532,6 @@ bool AP_GPS_NMEA::_term_complete()
#endif
return false;
}
/*
detect a NMEA GPS. Adds one byte, and returns true if the stream
matches a NMEA string

37
libraries/AP_GPS/AP_GPS_NMEA.h
Unescape View File

@ -72,8 +72,8 @@ private: @@ -72,8 +72,8 @@ private:
_GPS_SENTENCE_GGA = 64,
_GPS_SENTENCE_VTG = 96,
_GPS_SENTENCE_HDT = 128,
_GPS_SENTENCE_MARK = 148,
_GPS_SENTENCE_EVENT = 188,
_GPS_SENTENCE_MARK = 160,
_GPS_SENTENCE_EVENT = 200,
_GPS_SENTENCE_OTHER = 0
};
@ -145,22 +145,23 @@ private: @@ -145,22 +145,23 @@ private:
uint32_t _last_VTG_ms = 0;
uint32_t _last_HDT_ms = 0;
struct mark_s
{
uint16_t week;
float second;
double lat;
double lon;
double hgt;
float undulation;
float lat_sigma;
float lon_sigma;
float hgt_sigma;
float diff_age;
float sol_age;
uint8_t SVs;
uint8_t solnSVs;
}gps_mark;
struct mark_s
{
uint16_t week;
float second;
double lat;
double lon;
double hgt;
float undulation;
float lat_sigma;
float lon_sigma;
float hgt_sigma;
float diff_age;
float sol_age;
uint8_t SVs;
uint8_t solnSVs;
}gps_mark;
bool get_new_mark;
/// @name Init strings
/// In ::init, an attempt is made to configure the GPS
/// unit to send just the messages that we are interested

1
libraries/AP_GPS/GPS_Backend.h
Unescape View File

@ -71,6 +71,7 @@ protected: @@ -71,6 +71,7 @@ protected:
AP_HAL::UARTDriver *port; ///< UART we are attached to
AP_GPS &gps; ///< access to frontend (for parameters)
AP_GPS::GPS_State &state; ///< public state for this instance
// AP_GPS::rtk_mark_s &rtk_mark; /// @Brown for rtk mark
// common utility functions
int32_t swap_int32(int32_t v) const;

Write Preview
Loading…
Cancel
Save