Add validation to initialization configuration commands

libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp

@@ -286,28 +286,35 @@ bool AP_ExternalAHRS_VectorNav::check_uart()
     return true;
 }
 
-// Send command to read given register number and wait for response
-// Only run from thread! This blocks until a response is received
+// Send command and wait for response
+// Only run from thread! This blocks and retries until a non-error response is received
 #define READ_REQUEST_RETRY_MS 500
-void AP_ExternalAHRS_VectorNav::wait_register_responce(const uint8_t register_num)
+void AP_ExternalAHRS_VectorNav::run_command(const char * fmt, ...)
 {
-    nmea.register_number = register_num;
+    va_list ap;
+
+    va_start(ap, fmt);
+    hal.util->vsnprintf(message_to_send, sizeof(message_to_send), fmt, ap);
+    va_end(ap);
 
     uint32_t request_sent = 0;
     while (true) {
         hal.scheduler->delay(1);
 
         const uint32_t now = AP_HAL::millis();
         if (now - request_sent > READ_REQUEST_RETRY_MS) {
-            // Send request to read
-            nmea_printf(uart, "$%s%u", "VNRRG,", nmea.register_number);
+            nmea_printf(uart, "$%s", message_to_send);
             request_sent = now;
         }
 
         int16_t nbytes = uart->available();
         while (nbytes-- > 0) {
             char c = uart->read();
             if (decode(c)) {
+                if (nmea.error_response && nmea.sentence_done) {
+                    // Received a valid VNERR. Try to resend after the timeout length
+                    break;
+                }
                 return;
             }
         }
@@ -356,6 +363,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;
     }
 
@@ -371,57 +379,60 @@ bool AP_ExternalAHRS_VectorNav::decode(char c)
 }
 
 // decode the most recently consumed term
-// returns true if new sentence has just passed checksum test and is validated
+// returns true if new term is valid
 bool AP_ExternalAHRS_VectorNav::decode_latest_term()
 {
+    // Check the first two terms (In most cases header + reg number) that they match the sent
+    // message. If not, the response is invalid.
     switch (nmea.term_number) {
         case 0:
-            if (strcmp(nmea.term, "VNRRG") != 0) {
+            if (strncmp(nmea.term, "VNERR", nmea.term_offset) == 0) {
+                nmea.error_response = true;  // Message will be printed on next term 
+            } else if (strncmp(nmea.term, message_to_send, nmea.term_offset) != 0) {
                 return false;
             }
-            break;
-
-        case 1:
-            if (nmea.register_number != strtoul(nmea.term, nullptr, 10)) {
+            return true;
+        case 1: 
+            if (nmea.error_response) {
+                GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "VectorNav received VNERR code: %s", nmea.term);
+            } else if (strlen(message_to_send) > 6 && 
+                       strncmp(nmea.term, &message_to_send[6], nmea.term_offset != 0)) {  // Start after "VNXXX,"
                 return false;
             }
-            break;
-
-        case 2:
-            strncpy(model_name, nmea.term, sizeof(model_name));
-            break;
-
+            return true;
+        case 2: 
+            if (strncmp(nmea.term, "VN-", 3) == 0) {
+                // This term is the model number
+                strncpy(model_name, nmea.term, sizeof(model_name));
+            }
+            return true;
         default:
-            return false;
+            return true;
     }
-    return true;
 }
 
-void AP_ExternalAHRS_VectorNav::update_thread()
-{
+void AP_ExternalAHRS_VectorNav::initialize() {
     // Open port in the thread
     uart->begin(baudrate, 1024, 512);
 
-    // Reset and wait for module to reboot
-    // VN_100 takes 1.25 seconds
-    //nmea_printf(uart, "$VNRST");
-    //hal.scheduler->delay(3000);
+    // Pause asynchronous communications to simplify packet finding
+    run_command("VNASY,0");
 
-    // Stop NMEA Async Outputs (this UART only)
-    nmea_printf(uart, "$VNWRG,6,0");
+    // Stop ASCII async outputs for both UARTs. If only active UART is disabled, we get a baudrate
+    // overflow on the other UART when configuring binary outputs (reg 75 and 76) to both UARTs
+    run_command("VNWRG,06,0,1");
+    run_command("VNWRG,06,0,2");
 
-    // Detect version
     // Read Model Number Register, ID 1
-    wait_register_responce(1);
+    run_command("VNRRG,01");
 
     // Setup for messages respective model types (on both UARTs)
     if (strncmp(model_name, "VN-1", 4) == 0) {
-        // VN-100
+        // VN-1X0
         type = TYPE::VN_AHRS;
 
         // This assumes unit is still configured at its default rate of 800hz
-        nmea_printf(uart, "$VNWRG,75,3,%u,14,073E,0004", unsigned(800/get_rate()));
-
+        run_command("VNWRG,75,3,%u,14,073E,0004", unsigned(800 / get_rate()));
     } else {
         // Default to setup for sensors other than VN-100 or VN-110
         // This assumes unit is still configured at its default IMU rate of 400hz for VN-300, 800hz for others
@@ -432,11 +443,17 @@ void AP_ExternalAHRS_VectorNav::update_thread()
         if (strncmp(model_name, "VN-3", 4) == 0) {
             has_dual_gnss = true;
         }
-        nmea_printf(uart, "$VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate/get_rate()));
-        nmea_printf(uart, "$VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate/5));
+        run_command("VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate / get_rate()));
+        run_command("VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate / 5));
     }
 
+    // Resume asynchronous communications
+    run_command("VNASY,1");
     setup_complete = true;
+}
+
+void AP_ExternalAHRS_VectorNav::update_thread() {
+    initialize();
     while (true) {
         if (!check_uart()) {
             hal.scheduler->delay(1);

libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h

@@ -61,10 +61,12 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {
     void update_thread();
     bool check_uart();
 
+    void initialize();
+
     void process_ins_packet1(const uint8_t *b);
     void process_ins_packet2(const uint8_t *b);
     void process_ahrs_packet(const uint8_t *b);
-    void wait_register_responce(const uint8_t register_num);
+    void run_command(const char *fmt, ...);
 
     uint8_t *pktbuf;
     uint16_t pktoffset;
@@ -83,22 +85,22 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {
 
     bool has_dual_gnss = false;
 
-    char model_name[25];
+    char model_name[20];
 
+    char message_to_send[50];
     // NMEA parsing for setup
     bool decode(char c);
     bool decode_latest_term();
     struct NMEA_parser {
-        char term[25];            // buffer for the current term within the current sentence
+        char term[20];            // buffer for the current term within the current sentence
         uint8_t term_offset;      // offset within the _term buffer where the next character should be placed
         uint8_t term_number;      // term index within the current sentence
         uint8_t checksum;         // checksum accumulator
         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
-        uint8_t register_number;  // VectorNAV register number were reading
+        bool error_response;      // true if received a VNERR response
     } nmea;
-
 };
 
 #endif  // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED

libraries/SITL/SIM_VectorNav.cpp

@@ -213,11 +213,14 @@ void VectorNav::update(void)
         send_packet2();
     }
 
-    // Strictly we should send this in responce to the request
-    // but sending it occasionally acheaves the same thing
-    if (now - last_type_us >= 1000000) {
-        last_type_us = now;
-        nmea_printf("$VNRRG,01,VN-300-SITL");
+    char receive_buf[50];
+    const ssize_t n = read_from_autopilot(&receive_buf[0], ARRAY_SIZE(receive_buf));
+    if (n > 0) {
+        if (strncmp(receive_buf, "$VNRRG,01", 9) == 0) {
+            nmea_printf("$VNRRG,01,VN-300-SITL");
+        } else {
+            nmea_printf("$%s", receive_buf);
+        }
     }
 
 }