aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_ExternalAHRS_VectorNav: Handle VNERR responses

Browse Source 
Print received VNERR response to console and continually retry sending the configuration message, instead of panicing
This commit is contained in:
BLash 2024-07-08 18:38:49 -05:00 committed by Andrew Tridgell
parent 024d50ed09
commit 1288810c70
2 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
View File

@ -305,6 +305,10 @@ void AP_ExternalAHRS_VectorNav::send_command_blocking()
while (nbytes-- > 0) {
char c = uart->read();
if (decode(c)) {
if (nmea.error_response) {
// Received a valid VNERR. Try to resend after the timeout length
break;
}
return;
}
}
@ -353,6 +357,7 @@ bool AP_ExternalAHRS_VectorNav::decode(char c)
nmea.checksum = 0;
nmea.term_is_checksum = false;
nmea.sentence_done = false;
nmea.error_response = false;
return false;
}
@ -378,10 +383,14 @@ bool AP_ExternalAHRS_VectorNav::decode_latest_term()
if (strncmp(nmea.term, message_to_send, nmea.term_offset) != 0) { // ignore sync byte in comparison
return false;
} else if (strncmp(nmea.term, "VNERR", nmea.term_offset) == 0) {
AP_HAL::panic("VectorNav received VN error response");
nmea.error_response = true; // Message will be printed on next term
}
break;
case 1:
if (nmea.error_response) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "VectorNav received VNERR code: %s", nmea.term);
break;
}
if (strncmp(nmea.term, message_to_send + 6,
nmea.term_offset) != 0) { // Start after "VNXXX,"
return false;

1
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
View File

@ -99,6 +99,7 @@ private:
bool term_is_checksum; // current term is the checksum
bool sentence_valid; // is current sentence valid so far
bool sentence_done; // true if this sentence has already been decoded
bool error_response; // true if received a VNERR response
} nmea;
};