diff --git a/APMrover2/version.h b/APMrover2/version.h index 0bf178f88a..95e0625fea 100644 --- a/APMrover2/version.h +++ b/APMrover2/version.h @@ -6,7 +6,7 @@ #include "ap_version.h" -#define THISFIRMWARE "ZR_Boat V4.0.3 RC2" +#define THISFIRMWARE "ZR_Boat V4.0.3 RC3" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,0,3,FIRMWARE_VERSION_TYPE_DEV @@ -21,5 +21,6 @@ * * 4.0.3 去除声呐串口读取，使用uavcan，增加8、9通道控制relay 1、2 * 4.0.3 rc2 uavcan避障调整 + * rc3 增加NMEA声呐 * */ diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index 56e4f8ab4c..cff9a6787e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -18,7 +18,16 @@ #include #include #include "AP_RangeFinder_NMEA.h" -// #include +#include + +#define BOAT_DEBUG 1 +#if BOAT_DEBUG +#include + # define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO, "BOAT:%d: " fmt "\n", ## args) +#else + # define Debug(fmt, args ...) +#endif + extern const AP_HAL::HAL& hal; // constructor initialises the rangefinder @@ -50,8 +59,6 @@ void AP_RangeFinder_NMEA::update(void) if (get_reading(state.distance_cm)) { // update range_valid state based on distance measured state.last_reading_ms = now; - // gcs().send_text(MAV_SEVERITY_INFO,"t:%ld,dist:%dcm",state.last_reading_ms,state.distance_cm); - update_status(); } else if ((now - state.last_reading_ms) > 3000) { set_status(RangeFinder::RangeFinder_NoData); @@ -64,24 +71,27 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm) if (uart == nullptr) { return false; } - bool ret = false; + // read any available lines from the lidar + float sum = 0.0f; + uint16_t count = 0; int16_t nbytes = uart->available(); while (nbytes-- > 0) { - uint8_t c = uart->read(); + char c = uart->read(); if (decode(c)) { - ret = true; - _sonar_deep = radar_data[1]; - _sonar_voltage = radar_data[5]; - - // gcs().send_text(MAV_SEVERITY_INFO,"deep:%.2f,volt:%.2f",_sonar_deep,_sonar_voltage); + sum += _distance_m; + count++; } } + // return false on failure + if (count == 0) { + return false; + } // return average of all measurements - reading_cm = 100.0f * _sonar_deep; - return ret; + reading_cm = 100.0f * sum / count; + return true; } // add a single character to the buffer and attempt to decode @@ -89,25 +99,44 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm) bool AP_RangeFinder_NMEA::decode(char c) { switch (c) { - // case '\r': + case ',': + // end of a term, add to checksum + _checksum ^= c; + FALLTHROUGH; + case '\r': case '\n': + case '*': + { + // null terminate and decode latest term + _term[_term_offset] = 0; + bool valid_sentence = decode_latest_term(); + + // move onto next term + _term_number++; + _term_offset = 0; + _term_is_checksum = (c == '*'); + return valid_sentence; + } + + case '@': // sentence begin + case '$': // sentence begin + _sentence_type = SONAR_UNKNOWN; + _term_number = 0; _term_offset = 0; - decode_step = 0; - memset(_term,0,sizeof(_term)); - return true; - case ' ': - if(decode_step < 8){ - radar_data[decode_step] = atof(_term); - _term_offset = 0; - decode_step += 1; - memset(_term,0,sizeof(_term)); - } - break; + _checksum = 0; + _term_is_checksum = false; + _distance_m = -1.0f; + return false; } - if (_term_offset < sizeof(_term) - 1) + // ordinary characters are added to term + if (_term_offset < sizeof(_term) - 1) { _term[_term_offset++] = c; - + } + if (!_term_is_checksum) { + _checksum ^= c; + } + return false; } @@ -123,11 +152,14 @@ bool AP_RangeFinder_NMEA::decode_latest_term() return false; } const uint8_t checksum = (nibble_high << 4u) | nibble_low; - return ((checksum == _checksum) && - !is_negative(_distance_m) && - (_sentence_type == SONAR_DBT || _sentence_type == SONAR_DPT)); + + // Debug("red:%x,_checksum:%x\n",checksum,_checksum); + // gcs().send_text(MAV_SEVERITY_INFO, "[%d] read:%x,_checksum:%x,dist:%.2f\n",(checksum == _checksum),checksum,_checksum, _distance_m); + // return true; + return ((checksum == _checksum) && (_sentence_type == SONAR_SIC || _sentence_type == SONAR_DPT)); } + // the first term determines the sentence type if (_term_number == 0) { // the first two letters of the NMEA term are the talker ID. @@ -138,17 +170,26 @@ bool AP_RangeFinder_NMEA::decode_latest_term() return false; } const char *term_type = &_term[2]; + const char *term_sic = &_term[0]; if (strcmp(term_type, "DBT") == 0) { _sentence_type = SONAR_DBT; } else if (strcmp(term_type, "DPT") == 0) { _sentence_type = SONAR_DPT; + }else if (strcmp(term_sic, "SIC") == 0) { + _sentence_type = SONAR_SIC; } else { _sentence_type = SONAR_UNKNOWN; } return false; } + if ((_sentence_type == SONAR_SIC )) { - if (_sentence_type == SONAR_DBT) { + if (_term_number == 5) { + _distance_m = atof(_term); + // Debug("dist:%.2f",_distance_m); + // cout << "type " << SONAR_SIC + 5 << ": " << _term << ", " << _term[1] <<", --- " << sonar_deep << endl; + } + }else if (_sentence_type == SONAR_DBT) { // parse DBT messages if (_term_number == 3) { _distance_m = atof(_term); @@ -160,5 +201,6 @@ bool AP_RangeFinder_NMEA::decode_latest_term() } } + // cout << "term " << int(_term_number) << ": " << _term << endl; return false; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index e96ea14605..1fff3f6a4b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -45,6 +45,7 @@ private: enum sentence_types : uint8_t { SONAR_UNKNOWN = 0, SONAR_DBT, + SONAR_SIC, SONAR_DPT };