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
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      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

18
libraries/AP_GPS/AP_GPS.h
Unescape View File

@ -166,7 +166,22 @@ public:
uint32_t rtk_accuracy; ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999) 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 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. /// Startup initialisation.
void init(const AP_SerialManager& serial_manager); void init(const AP_SerialManager& serial_manager);
@ -486,6 +501,7 @@ private:
GPS_State state[GPS_MAX_INSTANCES]; GPS_State state[GPS_MAX_INSTANCES];
AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS]; AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS];
AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS]; AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS];
rtk_mark_s rtk_mark[GPS_MAX_INSTANCES];
/// primary GPS instance /// primary GPS instance
uint8_t primary_instance; uint8_t primary_instance;

55
libraries/AP_GPS/AP_GPS_NMEA.cpp
Unescape View File

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

37
libraries/AP_GPS/AP_GPS_NMEA.h
Unescape View File

@ -72,8 +72,8 @@ private:
_GPS_SENTENCE_GGA = 64, _GPS_SENTENCE_GGA = 64,
_GPS_SENTENCE_VTG = 96, _GPS_SENTENCE_VTG = 96,
_GPS_SENTENCE_HDT = 128, _GPS_SENTENCE_HDT = 128,
_GPS_SENTENCE_MARK = 148, _GPS_SENTENCE_MARK = 160,
_GPS_SENTENCE_EVENT = 188, _GPS_SENTENCE_EVENT = 200,
_GPS_SENTENCE_OTHER = 0 _GPS_SENTENCE_OTHER = 0
}; };
@ -145,22 +145,23 @@ private:
uint32_t _last_VTG_ms = 0; uint32_t _last_VTG_ms = 0;
uint32_t _last_HDT_ms = 0; uint32_t _last_HDT_ms = 0;
struct mark_s struct mark_s
{ {
uint16_t week; uint16_t week;
float second; float second;
double lat; double lat;
double lon; double lon;
double hgt; double hgt;
float undulation; float undulation;
float lat_sigma; float lat_sigma;
float lon_sigma; float lon_sigma;
float hgt_sigma; float hgt_sigma;
float diff_age; float diff_age;
float sol_age; float sol_age;
uint8_t SVs; uint8_t SVs;
uint8_t solnSVs; uint8_t solnSVs;
}gps_mark; }gps_mark;
bool get_new_mark;
/// @name Init strings /// @name Init strings
/// In ::init, an attempt is made to configure the GPS /// In ::init, an attempt is made to configure the GPS
/// unit to send just the messages that we are interested /// 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:
AP_HAL::UARTDriver *port; ///< UART we are attached to AP_HAL::UARTDriver *port; ///< UART we are attached to
AP_GPS &gps; ///< access to frontend (for parameters) AP_GPS &gps; ///< access to frontend (for parameters)
AP_GPS::GPS_State &state; ///< public state for this instance AP_GPS::GPS_State &state; ///< public state for this instance
// AP_GPS::rtk_mark_s &rtk_mark; /// @Brown for rtk mark
// common utility functions // common utility functions
int32_t swap_int32(int32_t v) const; int32_t swap_int32(int32_t v) const;

Write Preview
Loading…
Cancel
Save