zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

增加nmea解析mark，通过event做时间同步，记录RTKpos

mission-4.1.18
zbr 4 years ago
parent
d98b7aeeb0
commit
e96c36d140
10 changed files with 443 additions and 34 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. 28
      libraries/AP_GPS/AP_GPS.cpp
  2. 20
      libraries/AP_GPS/AP_GPS.h
  3. 202
      libraries/AP_GPS/AP_GPS_NMEA.cpp
  4. 113
      libraries/AP_GPS/AP_GPS_NMEA.h
  5. 1
      libraries/AP_GPS/GPS_Backend.h
  6. 1
      libraries/AP_Logger/AP_Logger.h
  7. 28
      libraries/AP_Logger/LogFile.cpp
  8. 74
      libraries/AP_Logger/LogStructure.h
  9. 2
      modules/mavlink
  10. 2
      zr-v4.sh

28
libraries/AP_GPS/AP_GPS.cpp
Unescape View File

@ -60,11 +60,13 @@ @@ -60,11 +60,13 @@
extern const AP_HAL::HAL &hal;
// baudrates to try to detect GPSes with
const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U};
// const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U};
const uint32_t AP_GPS::_baudrates[] = { 115200U, 38400U, 57600U, 230400U};
// initialisation blobs to send to the GPS to try to get it into the
// right mode
const char AP_GPS::_initialisation_blob[] = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY;
// const char AP_GPS::_initialisation_blob[] = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY;
const char AP_GPS::_initialisation_blob[] = UBLOX_SET_BINARY ;
AP_GPS *AP_GPS::_singleton;
@ -532,7 +534,11 @@ void AP_GPS::detect_instance(uint8_t instance) @@ -532,7 +534,11 @@ void AP_GPS::detect_instance(uint8_t instance)
if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) {
if (_type[instance] == GPS_TYPE_HEMI) {
send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING));
} else {
}else if (_type[instance] == GPS_TYPE_SINO) {
send_blob_start(instance, AP_GPS_NMEA_SINO_INIT_STRING, strlen(AP_GPS_NMEA_SINO_INIT_STRING));
}else if (_type[instance] == GPS_TYPE_UNICO) {
send_blob_start(instance, AP_GPS_NMEA_UNICORE_INIT_STRING, strlen(AP_GPS_NMEA_UNICORE_INIT_STRING));
}else {
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
}
@ -562,33 +568,33 @@ void AP_GPS::detect_instance(uint8_t instance) @@ -562,33 +568,33 @@ void AP_GPS::detect_instance(uint8_t instance)
#if !HAL_MINIMIZE_FEATURES
// we drop the MTK drivers when building a small build as they are so rarely used
// and are surprisingly large
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) &&
else if ((_type[instance] == GPS_TYPE_MTK19) &&
AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) {
new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]);
} else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) &&
} else if ((_type[instance] == GPS_TYPE_MTK) &&
AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) {
new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]);
}
#endif
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
else if ((_type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) {
new_gps = new AP_GPS_SBP2(*this, state[instance], _port[instance]);
}
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
else if ((_type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]);
}
#if !HAL_MINIMIZE_FEATURES
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&
else if ((_type[instance] == GPS_TYPE_SIRF) &&
AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]);
}
#endif
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) &&
else if ((_type[instance] == GPS_TYPE_ERB) &&
AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) {
new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]);
} else if ((_type[instance] == GPS_TYPE_NMEA ||
_type[instance] == GPS_TYPE_HEMI) &&
} else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA ||
_type[instance] == GPS_TYPE_HEMI || _type[instance] == GPS_TYPE_SINO || _type[instance] == GPS_TYPE_UNICO) &&
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
}

20
libraries/AP_GPS/AP_GPS.h
Unescape View File

@ -92,6 +92,8 @@ public: @@ -92,6 +92,8 @@ public:
GPS_TYPE_MAV = 14,
GPS_TYPE_NOVA = 15,
GPS_TYPE_HEMI = 16, // hemisphere NMEA
GPS_TYPE_SINO = 17, // sino NMEA
GPS_TYPE_UNICO = 18, // unicorecomm NMEA
};
/// GPS status codes
@ -166,7 +168,22 @@ public: @@ -166,7 +168,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,6 +503,7 @@ private: @@ -486,6 +503,7 @@ 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;

202
libraries/AP_GPS/AP_GPS_NMEA.cpp
Unescape View File

@ -36,6 +36,8 @@ @@ -36,6 +36,8 @@
#include "AP_GPS_NMEA.h"
#include <AP_Logger/AP_Logger.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
// optionally log all NMEA data for debug purposes
@ -82,7 +84,10 @@ bool AP_GPS_NMEA::_decode(char c) @@ -82,7 +84,10 @@ bool AP_GPS_NMEA::_decode(char c)
switch (c) {
case ',': // term terminators
case ';': // term terminators
_parity ^= c;
markCRC = aulCrcTable[(markCRC ^ c) & 0xff] ^ (markCRC >> 8);
FALLTHROUGH;
case '\r':
case '\n':
@ -97,8 +102,10 @@ bool AP_GPS_NMEA::_decode(char c) @@ -97,8 +102,10 @@ bool AP_GPS_NMEA::_decode(char c)
return valid_sentence;
case '$': // sentence begin
case '#': // sentence begin
_term_number = _term_offset = 0;
_parity = 0;
markCRC = 0;
_sentence_type = _GPS_SENTENCE_OTHER;
_is_checksum_term = false;
_gps_data_good = false;
@ -109,9 +116,12 @@ bool AP_GPS_NMEA::_decode(char c) @@ -109,9 +116,12 @@ bool AP_GPS_NMEA::_decode(char c)
// ordinary characters
if (_term_offset < sizeof(_term) - 1)
_term[_term_offset++] = c;
if (!_is_checksum_term)
if (!_is_checksum_term){
_parity ^= c;
markCRC = aulCrcTable[(markCRC ^ c) & 0xff] ^ (markCRC >> 8);
}
return valid_sentence;
}
@ -143,6 +153,35 @@ int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p) @@ -143,6 +153,35 @@ int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p)
return ret;
}
uint32_t AP_GPS_NMEA::_parse_decimal_1000(const char *p)
{
char *endptr = nullptr;
long ret = 1000 * strtol(p, &endptr, 10);
if (ret >= (long)INT32_MAX) {
return INT32_MAX;
}
if (ret <= (long)INT32_MIN) {
return INT32_MIN;
}
if (endptr == nullptr || *endptr != '.') {
return ret;
}
if (isdigit(endptr[1])) {
ret += 100 * DIGIT_TO_VAL(endptr[1]);
if (isdigit(endptr[2])) {
ret += 10 * DIGIT_TO_VAL(endptr[2]);
if (isdigit(endptr[3])) {
ret += DIGIT_TO_VAL(endptr[3]);
if (isdigit(endptr[4])) {
ret += (DIGIT_TO_VAL(endptr[4]) >= 5);
}
}
}
}
return ret;
}
/*
parse a NMEA latitude/longitude degree value. The result is in degrees*1e7
*/
@ -233,7 +272,47 @@ bool AP_GPS_NMEA::_term_complete() @@ -233,7 +272,47 @@ bool AP_GPS_NMEA::_term_complete()
return false;
}
const uint8_t checksum = (nibble_high << 4u) | nibble_low;
if (checksum == _parity) {
if(_gps_data_good && (_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT )){
if(get_new_mark){
const uint8_t mark_crc = markCRC >> 24;
// gcs().send_text(MAV_SEVERITY_INFO, "week: %d, %f,crc:0x%02x--0x%02x--0x%02x",gps_mark.week,gps_mark.second,checksum,_parity,mark_crc);
// gcs().send_text(MAV_SEVERITY_INFO, "llh: %f , %f , %f\n",gps_mark.lat,gps_mark.lon,gps_mark.hgt);
if(checksum == mark_crc){
switch (_sentence_type) {
case _GPS_SENTENCE_MARK:
case _GPS_SENTENCE_EVENT:
if(get_new_mark){
gcs().send_text(MAV_SEVERITY_INFO, "%d, %.4f, %f , %f , %f\n",gps_mark.week,gps_mark.second,gps_mark.lat,gps_mark.lon,gps_mark.hgt);
get_new_mark = false;
AP_Logger *logger = AP_Logger::get_singleton();
if ( logger!=nullptr )
{
Vector3d llh = Vector3d(gps_mark.lat,gps_mark.lon, gps_mark.hgt);
Vector3f sigma = Vector3f(gps_mark.lat_sigma,gps_mark.lon_sigma, gps_mark.hgt_sigma);
AP::logger().Write_RTK_Mark_Pos(
gps_mark.week,
gps_mark.second,
llh,
gps_mark.undulation,
sigma,
gps_mark.diff_age,
gps_mark.sol_age
);
}
}
}
return _have_new_message();
}else{
gcs().send_text(MAV_SEVERITY_INFO, "crc err:0x%02x--0x%02x--0x%02x",checksum,_parity,mark_crc);
}
get_new_mark = false;
}
}
if (checksum == _parity ) {
if (_gps_data_good) {
uint32_t now = AP_HAL::millis();
switch (_sentence_type) {
@ -296,6 +375,14 @@ bool AP_GPS_NMEA::_term_complete() @@ -296,6 +375,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 +415,10 @@ bool AP_GPS_NMEA::_term_complete() @@ -328,8 +415,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 +428,20 @@ bool AP_GPS_NMEA::_term_complete() @@ -339,6 +428,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;
}
@ -417,9 +520,91 @@ bool AP_GPS_NMEA::_term_complete() @@ -417,9 +520,91 @@ bool AP_GPS_NMEA::_term_complete()
}
}
#if 1
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);
break;
case _GPS_SENTENCE_MARK + 6: // second
case _GPS_SENTENCE_EVENT + 6: // second
gps_mark.second = _parse_decimal_1000(_term);
// gcs().send_text(MAV_SEVERITY_INFO, "event:%s:%ld,%ld",_term,gps_mark.second,_parse_decimal_100(_term));
break;
case _GPS_SENTENCE_MARK + 12: // lat
case _GPS_SENTENCE_EVENT + 22: // lat
gps_mark.lat = atof(_term);
break;
case _GPS_SENTENCE_MARK + 13: // lon
case _GPS_SENTENCE_EVENT + 23:
gps_mark.lon = atof(_term);
break;
case _GPS_SENTENCE_MARK + 14: // hgt
case _GPS_SENTENCE_EVENT + 24:
gps_mark.hgt = atof(_term);
break;
case _GPS_SENTENCE_MARK + 15: // undulation
case _GPS_SENTENCE_EVENT + 25:
gps_mark.undulation = atof(_term);
break;
case _GPS_SENTENCE_MARK + 17: // lat_sigma
case _GPS_SENTENCE_EVENT + 27:
gps_mark.lat_sigma = atof(_term);
break;
case _GPS_SENTENCE_MARK + 18: // lon_sigma
case _GPS_SENTENCE_EVENT + 28:
gps_mark.lon_sigma = atof(_term);
break;
case _GPS_SENTENCE_MARK + 19: // hgt_sigma
case _GPS_SENTENCE_EVENT + 29:
gps_mark.hgt_sigma = atof(_term);
break;
case _GPS_SENTENCE_MARK + 21: // diff_age
case _GPS_SENTENCE_EVENT + 31:
gps_mark.diff_age = atof(_term);
break;
case _GPS_SENTENCE_MARK + 22: // sol_age
case _GPS_SENTENCE_EVENT + 32:
gps_mark.sol_age = atof(_term);
break;
case _GPS_SENTENCE_MARK + 23: // SVs
case _GPS_SENTENCE_EVENT + 33:
gps_mark.SVs = atol(_term);
case _GPS_SENTENCE_MARK + 24: // solnSVs
case _GPS_SENTENCE_EVENT + 34:
gps_mark.solnSVs = atol(_term);
get_new_mark = true;
break;
}
}
#endif
return false;
}
bool
AP_GPS_NMEA::_send_message( void *msg, uint16_t size)
{
if (port->txspace() < size) {
return false;
}
port->write((const uint8_t *)msg, size);
return true;
}
const char AP_GPS_NMEA::_initialisation_blob[] = AP_GPS_NMEA_HEMISPHERE_INIT_STRING;
void AP_GPS_NMEA::send_init_blob(uint8_t instance, AP_GPS &gps)
{
gps.send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
/*
detect a NMEA GPS. Adds one byte, and returns true if the stream
matches a NMEA string
@ -458,3 +643,16 @@ AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data) @@ -458,3 +643,16 @@ AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data)
}
return false;
}
// Calculate and return the CRC for usA binary buffer
// unsigned long CalculateCRC32(u_char *szBuf, int iSize)
// {
// int iIndex;
// u_long ulCRC = 0;
// for (iIndex=0; iIndex<iSize; iIndex++)
// {
// ulCRC = aulCrcTable[(ulCRC ^ szBuf[iIndex]) & 0xff] ^ (ulCRC >> 8);
// }
// return ulCRC;
// }

113
libraries/AP_GPS/AP_GPS_NMEA.h
Unescape View File

@ -64,6 +64,7 @@ public: @@ -64,6 +64,7 @@ public:
static bool _detect(struct NMEA_detect_state &state, uint8_t data);
const char *name() const override { return "NMEA"; }
static void send_init_blob(uint8_t instance, AP_GPS &gps);
private:
/// Coding for the GPS sentences that the parser handles
@ -72,6 +73,8 @@ private: @@ -72,6 +73,8 @@ private:
_GPS_SENTENCE_GGA = 64,
_GPS_SENTENCE_VTG = 96,
_GPS_SENTENCE_HDT = 128,
_GPS_SENTENCE_MARK = 160,
_GPS_SENTENCE_EVENT = 200,
_GPS_SENTENCE_OTHER = 0
};
@ -90,6 +93,7 @@ private: @@ -90,6 +93,7 @@ private:
/// multiplied by 100.
///
static int32_t _parse_decimal_100(const char *p);
static uint32_t _parse_decimal_1000(const char *p);
/// Parses the current term as a NMEA-style degrees + minutes
/// value with up to four decimal digits.
@ -143,6 +147,23 @@ private: @@ -143,6 +147,23 @@ private:
uint32_t _last_VTG_ms = 0;
uint32_t _last_HDT_ms = 0;
struct mark_s
{
uint16_t week;
uint32_t 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
@ -154,12 +175,92 @@ private: @@ -154,12 +175,92 @@ private:
//@}
static const char _initialisation_blob[];
bool _send_message( void *msg, uint16_t size);
unsigned long markCRC;
const unsigned long aulCrcTable[256] =
{
0x00000000UL, 0x77073096UL, 0xee0e612cUL, 0x990951baUL, 0x076dc419UL,0x706af48fUL,
0xe963a535UL, 0x9e6495a3UL, 0x0edb8832UL, 0x79dcb8a4UL, 0xe0d5e91eUL,0x97d2d988UL,
0x09b64c2bUL, 0x7eb17cbdUL, 0xe7b82d07UL, 0x90bf1d91UL, 0x1db71064UL,0x6ab020f2UL,
0xf3b97148UL, 0x84be41deUL, 0x1adad47dUL, 0x6ddde4ebUL, 0xf4d4b551UL,0x83d385c7UL,
0x136c9856UL, 0x646ba8c0UL, 0xfd62f97aUL, 0x8a65c9ecUL, 0x14015c4fUL,0x63066cd9UL,
0xfa0f3d63UL, 0x8d080df5UL, 0x3b6e20c8UL, 0x4c69105eUL, 0xd56041e4UL,0xa2677172UL,
0x3c03e4d1UL, 0x4b04d447UL, 0xd20d85fdUL, 0xa50ab56bUL, 0x35b5a8faUL,0x42b2986cUL,
0xdbbbc9d6UL, 0xacbcf940UL, 0x32d86ce3UL, 0x45df5c75UL, 0xdcd60dcfUL,0xabd13d59UL,
0x26d930acUL, 0x51de003aUL, 0xc8d75180UL, 0xbfd06116UL, 0x21b4f4b5UL,0x56b3c423UL,
0xcfba9599UL, 0xb8bda50fUL, 0x2802b89eUL, 0x5f058808UL, 0xc60cd9b2UL,0xb10be924UL,
0x2f6f7c87UL, 0x58684c11UL, 0xc1611dabUL, 0xb6662d3dUL, 0x76dc4190UL,0x01db7106UL,
0x98d220bcUL, 0xefd5102aUL, 0x71b18589UL, 0x06b6b51fUL, 0x9fbfe4a5UL,0xe8b8d433UL,
0x7807c9a2UL, 0x0f00f934UL, 0x9609a88eUL, 0xe10e9818UL, 0x7f6a0dbbUL,0x086d3d2dUL,
0x91646c97UL, 0xe6635c01UL, 0x6b6b51f4UL, 0x1c6c6162UL, 0x856530d8UL,0xf262004eUL,
0x6c0695edUL, 0x1b01a57bUL, 0x8208f4c1UL, 0xf50fc457UL, 0x65b0d9c6UL,0x12b7e950UL,
0x8bbeb8eaUL, 0xfcb9887cUL, 0x62dd1ddfUL, 0x15da2d49UL, 0x8cd37cf3UL,0xfbd44c65UL,
0x4db26158UL, 0x3ab551ceUL, 0xa3bc0074UL, 0xd4bb30e2UL, 0x4adfa541UL,0x3dd895d7UL,
0xa4d1c46dUL, 0xd3d6f4fbUL, 0x4369e96aUL, 0x346ed9fcUL, 0xad678846UL,0xda60b8d0UL,
0x44042d73UL, 0x33031de5UL, 0xaa0a4c5fUL, 0xdd0d7cc9UL, 0x5005713cUL,0x270241aaUL,
0xbe0b1010UL, 0xc90c2086UL, 0x5768b525UL, 0x206f85b3UL, 0xb966d409UL,0xce61e49fUL,
0x5edef90eUL, 0x29d9c998UL, 0xb0d09822UL, 0xc7d7a8b4UL, 0x59b33d17UL,0x2eb40d81UL,
0xb7bd5c3bUL, 0xc0ba6cadUL, 0xedb88320UL, 0x9abfb3b6UL, 0x03b6e20cUL,0x74b1d29aUL,
0xead54739UL, 0x9dd277afUL, 0x04db2615UL, 0x73dc1683UL, 0xe3630b12UL,0x94643b84UL,
0x0d6d6a3eUL, 0x7a6a5aa8UL, 0xe40ecf0bUL, 0x9309ff9dUL, 0x0a00ae27UL,0x7d079eb1UL,
0xf00f9344UL, 0x8708a3d2UL, 0x1e01f268UL, 0x6906c2feUL, 0xf762575dUL,0x806567cbUL,
0x196c3671UL, 0x6e6b06e7UL, 0xfed41b76UL, 0x89d32be0UL, 0x10da7a5aUL,0x67dd4accUL,
0xf9b9df6fUL, 0x8ebeeff9UL, 0x17b7be43UL, 0x60b08ed5UL, 0xd6d6a3e8UL,0xa1d1937eUL,
0x38d8c2c4UL, 0x4fdff252UL, 0xd1bb67f1UL, 0xa6bc5767UL, 0x3fb506ddUL,0x48b2364bUL,
0xd80d2bdaUL, 0xaf0a1b4cUL, 0x36034af6UL, 0x41047a60UL, 0xdf60efc3UL,0xa867df55UL,
0x316e8eefUL, 0x4669be79UL, 0xcb61b38cUL, 0xbc66831aUL, 0x256fd2a0UL,0x5268e236UL,
0xcc0c7795UL, 0xbb0b4703UL, 0x220216b9UL, 0x5505262fUL, 0xc5ba3bbeUL,0xb2bd0b28UL,
0x2bb45a92UL, 0x5cb36a04UL, 0xc2d7ffa7UL, 0xb5d0cf31UL, 0x2cd99e8bUL,0x5bdeae1dUL,
0x9b64c2b0UL, 0xec63f226UL, 0x756aa39cUL, 0x026d930aUL, 0x9c0906a9UL,0xeb0e363fUL,
0x72076785UL, 0x05005713UL, 0x95bf4a82UL, 0xe2b87a14UL, 0x7bb12baeUL,0x0cb61b38UL,
0x92d28e9bUL, 0xe5d5be0dUL, 0x7cdcefb7UL, 0x0bdbdf21UL, 0x86d3d2d4UL,0xf1d4e242UL,
0x68ddb3f8UL, 0x1fda836eUL, 0x81be16cdUL, 0xf6b9265bUL, 0x6fb077e1UL,0x18b74777UL,
0x88085ae6UL, 0xff0f6a70UL, 0x66063bcaUL, 0x11010b5cUL, 0x8f659effUL,0xf862ae69UL,
0x616bffd3UL, 0x166ccf45UL, 0xa00ae278UL, 0xd70dd2eeUL, 0x4e048354UL,0x3903b3c2UL,
0xa7672661UL, 0xd06016f7UL, 0x4969474dUL, 0x3e6e77dbUL, 0xaed16a4aUL,0xd9d65adcUL,
0x40df0b66UL, 0x37d83bf0UL, 0xa9bcae53UL, 0xdebb9ec5UL, 0x47b2cf7fUL,0x30b5ffe9UL,
0xbdbdf21cUL, 0xcabac28aUL, 0x53b39330UL, 0x24b4a3a6UL, 0xbad03605UL,0xcdd70693UL,
0x54de5729UL, 0x23d967bfUL, 0xb3667a2eUL, 0xc4614ab8UL, 0x5d681b02UL,0x2a6f2b94UL,
0xb40bbe37UL, 0xc30c8ea1UL, 0x5a05df1bUL, 0x2d02ef8dUL
};
};
#define AP_GPS_NMEA_HEMISPHERE_INIT_STRING \
"$JATT,NMEAHE,0\r\n" /* Prefix of GP on the HDT message */ \
"$JASC,GPGGA,5\r\n" /* GGA at 5Hz */ \
"$JASC,GPRMC,5\r\n" /* RMC at 5Hz */ \
"$JASC,GPVTG,5\r\n" /* VTG at 5Hz */ \
"$JASC,GPHDT,5\r\n" /* HDT at 5Hz */ \
"$JMODE,SBASR,YES\r\n" /* Enable SBAS */
"interfacemode com1 compass compass on\r\n" /* Prefix of GP on the HDT message */ \
"COM COM2 230400\r\n" /* GGA at 5Hz */ \
"COM COM3 460800\r\n" /* RMC at 5Hz */ \
"COM COM1 115200\r\n" /* VTG at 5Hz */ \
"interfacemode com1 auto auto on\r\n" /* HDT at 5Hz */ \
"saveconfig\r\n" /* Enable SBAS */
#define AP_GPS_NMEA_SINO_INIT_STRING \
"interfacemode com1 compass compass on\r\n" /* Prefix of GP on the HDT message */ \
"log com1 gpgga ontime 0.1\r\n" /* GGA at 5Hz */ \
"log com1 markposa onnew\r\n" /* RMC at 5Hz */ \
"log com1 marktimea onnew\r\n" /* VTG at 5Hz */ \
"interfacemode com1 auto auto on\r\n" /* HDT at 5Hz */ \
"saveconfig\r\n" /* Enable SBAS */
#define AP_GPS_NMEA_UNICORE_INIT_STRING \
"UNLOG COM1\r\n" /* Prefix of GP on the HDT message */ \
"log com1 gpgga ontime 0.1\r\n" /* Prefix of GP on the HDT message */ \
"log com1 gprmc ontime 0.1\r\n" /* GGA at 5Hz */ \
"log com1 gpvtg ontime 0.1\r\n" /* HDT at 5Hz */ \
"LOG COM1 EVENTALLA ONCHANGED\r\n" /* VTG at 5Hz */
#if 0
"LOG COM1 EVENTMARKA ONCHANGED\r\n" /* RMC at 5Hz */
"log com3 GPSEPHEMB ontime 300\r\n" \
"log com3 GLOEPHEMERISB ontime 300\r\n" \
"log com3 BD2EPHEMB ontime 300\r\n" \
"log com3 rangecmpb ontime 0.1\r\n" \
"LOG COM3 EVENTALLB ONCHANGED\r\n" \
"LOG COM3 EVENTMARKB ONCHANGED\r\n" \
"LOG COM3 EVENTALLA ONCHANGED\r\n" \
"CONFIG PPS ENABLE GPS POSITIVE 500000 1000 0 0\r\n" \
"log com3 gpgga ontime 0.05\r\n" \
// "saveconfig\r\n" /* Enable SBAS */
#endif

1
libraries/AP_GPS/GPS_Backend.h
Unescape View File

@ -69,6 +69,7 @@ protected: @@ -69,6 +69,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;

1
libraries/AP_Logger/AP_Logger.h
Unescape View File

@ -285,6 +285,7 @@ public: @@ -285,6 +285,7 @@ public:
void Write_OABendyRuler(bool active, float target_yaw, float margin, const Location &final_dest, const Location &oa_dest);
void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest);
void Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2,uint16_t cam3,uint16_t cam4,uint16_t cam5,uint8_t flag);
void Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d& llh,float undulation,const Vector3f& sigma,float diff_age,float sol_age);
void Write(const char *name, const char *labels, const char *fmt, ...);
void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);

28
libraries/AP_Logger/LogFile.cpp
Unescape View File

@ -1028,3 +1028,31 @@ void AP_Logger::Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2 @@ -1028,3 +1028,31 @@ void AP_Logger::Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2
};
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d& llh,float undulation,const Vector3f& sigma,float diff_age,float sol_age)
{
const AP_AHRS &ahrs = AP::ahrs();
struct log_rtk_mark_pos pkt
{
LOG_PACKET_HEADER_INIT(LOG_RTK_MARK_POS),
time_us : AP_HAL::micros64(),
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
yaw : (uint16_t)ahrs.yaw_sensor,
week :week,
second :second,
lat :llh.x,
lon :llh.y ,
hgt :llh.z ,
undulation :undulation ,
lat_sigma :sigma.x ,
lon_sigma :sigma.y ,
hgt_sigma :sigma.z ,
diff_age :diff_age ,
sol_age :sol_age ,
SVs :0 ,
solnSVs : 0
};
WriteBlock(&pkt, sizeof(pkt));
}

74
libraries/AP_Logger/LogStructure.h
Unescape View File

@ -1217,6 +1217,30 @@ struct PACKED log_Camera_FeedBack @@ -1217,6 +1217,30 @@ struct PACKED log_Camera_FeedBack
uint8_t flag;
};
struct PACKED log_rtk_mark_pos
{
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t roll;
int16_t pitch;
uint16_t yaw;
uint16_t week;
uint32_t 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;
};
// FMT messages define all message formats other than FMT
// UNIT messages define units which can be referenced by FMTU messages
// FMTU messages associate types (e.g. centimeters/second/second) to FMT message fields
@ -1414,7 +1438,9 @@ struct PACKED log_Camera_FeedBack @@ -1414,7 +1438,9 @@ struct PACKED log_Camera_FeedBack
{ LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \
"OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" },\
{ LOG_CAM_FEEDBACK_MSG, sizeof(log_Camera_FeedBack), \
"CAMF","QHHHHHHB","TimeUS,Index,CAM1,CAM2,CAM3,CAM4,CAM5,FLAG", "s-------", "F-------" }
"CAMF","QHHHHHHB","TimeUS,Index,CAM1,CAM2,CAM3,CAM4,CAM5,FLAG", "s-------", "F-------" },\
{ LOG_RTK_MARK_POS, sizeof(log_rtk_mark_pos), \
"MARK","QhhHHIdddffffffBB","TimeUS,R,P,Y,Week,Secms,Lat,Lon,Hgt,Undu,Lat_s,Lon_s,Hgt_s,D_age,S_age,SVs,SoSVs", "sddh-------------", "F----------------" }
// messages for more advanced boards
#define LOG_EXTRA_STRUCTURES \
{ LOG_IMU2_MSG, sizeof(log_IMU), \
@ -1688,14 +1714,14 @@ enum LogMessages : uint8_t { @@ -1688,14 +1714,14 @@ enum LogMessages : uint8_t {
LOG_GPS_UBX2_MSG,
LOG_GPS2_UBX1_MSG,
LOG_GPS2_UBX2_MSG,
LOG_ESC1_MSG,
LOG_ESC2_MSG,
LOG_ESC3_MSG,
LOG_ESC4_MSG,
LOG_ESC5_MSG,
LOG_ESC6_MSG,
LOG_ESC7_MSG,
LOG_ESC8_MSG,
// LOG_ESC1_MSG,
// LOG_ESC2_MSG,
// LOG_ESC3_MSG,
// LOG_ESC4_MSG,
// LOG_ESC5_MSG,
// LOG_ESC6_MSG,
// LOG_ESC7_MSG,
// LOG_ESC8_MSG,
LOG_BAR2_MSG,
LOG_ARSP_MSG,
LOG_ATTITUDE_MSG,
@ -1790,11 +1816,41 @@ enum LogMessages : uint8_t { @@ -1790,11 +1816,41 @@ enum LogMessages : uint8_t {
LOG_OA_DIJKSTRA_MSG,
LOG_RFND2_MSG,
LOG_CAM_FEEDBACK_MSG,
LOG_RTK_MARK_POS,
_LOG_LAST_MSG_
};
static_assert(_LOG_LAST_MSG_ <= 255, "Too many message formats");
enum LogMessages_back : uint8_t {
LOG_ESC1_MSG = 154,
LOG_ESC2_MSG,
LOG_ESC3_MSG,
LOG_ESC4_MSG,
LOG_ESC5_MSG,
LOG_ESC6_MSG,
LOG_ESC7_MSG,
LOG_ESC8_MSG,
// LOG_CURRENT3_MSG,
// LOG_CURRENT4_MSG,
// LOG_CURRENT5_MSG,
// LOG_CURRENT6_MSG,
// LOG_CURRENT7_MSG,
// LOG_CURRENT8_MSG,
// LOG_CURRENT9_MSG,
// LOG_CURRENT_CELLS_MSG,
// LOG_CURRENT_CELLS2_MSG,
// LOG_CURRENT_CELLS3_MSG,
// LOG_CURRENT_CELLS4_MSG,
// LOG_CURRENT_CELLS5_MSG,
// LOG_CURRENT_CELLS6_MSG,
// LOG_CURRENT_CELLS7_MSG,
// LOG_CURRENT_CELLS8_MSG,
// LOG_CURRENT_CELLS9_MSG
};
enum LogOriginType {
ekf_origin = 0,
ahrs_home = 1

2
modules/mavlink

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit 6d62818f975c25ffc507904658d8510d3de53c80
Subproject commit 72b407db3991ed74631fa59a5e826615f569819f

2
zr-v4.sh
Unescape View File

@ -1,3 +1,3 @@ @@ -1,3 +1,3 @@
./waf configure --board zr-v4
./waf copter
cp ./build/zr-v4/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/qusd-zrv4-v4.0.8-rc8.px4
cp ./build/zr-v4/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/quard-zrv4-gps.px4

Write Preview
Loading…
Cancel
Save