AP_ExternalAHRS_VectorNav: Clean run_command to accept the command to…

… send

Instead of snprintf the command to message_to_send then executing run_command, allow run_command to accept a string and format spec

libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp

@@ -289,8 +289,14 @@ bool AP_ExternalAHRS_VectorNav::check_uart()
 // 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::run_command()
+void AP_ExternalAHRS_VectorNav::run_command(const char * fmt, ...)
 {
+    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);
@@ -410,29 +416,23 @@ void AP_ExternalAHRS_VectorNav::initialize() {
     uart->begin(baudrate, 1024, 512);
 
     // Pause asynchronous communications to simplify packet finding
-    hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNASY,0");
-    run_command();
+    run_command("VNASY,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
-    hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,0,1");
-    run_command();
-    hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,0,2");
-    run_command();
+    run_command("VNWRG,06,0,1");
+    run_command("VNWRG,06,0,2");
 
     // Read Model Number Register, ID 1
-    hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNRRG,01");
-    run_command();
+    run_command("VNRRG,01");
 
     // Setup for messages respective model types (on both UARTs)
     if (strncmp(model_name, "VN-1", 4) == 0) {
         // VN-1X0
         type = TYPE::VN_AHRS;
 
         // This assumes unit is still configured at its default rate of 800hz
-        hal.util->snprintf(message_to_send, sizeof(message_to_send),
-                           "VNWRG,75,3,%u,14,073E,0004", unsigned(800 / get_rate()));
-        run_command();
+        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
@@ -443,17 +443,12 @@ void AP_ExternalAHRS_VectorNav::initialize() {
         if (strncmp(model_name, "VN-3", 4) == 0) {
             has_dual_gnss = true;
         }
-        hal.util->snprintf(message_to_send, sizeof(message_to_send),
-                           "VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate / get_rate()));
-        run_command();
-        hal.util->snprintf(message_to_send, sizeof(message_to_send),
-                           "VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate / 5));
-        run_command();
+        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
-    hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNASY,1");
-    run_command();
+    run_command("VNASY,1");
     setup_complete = true;
 }
 

libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h

@@ -66,7 +66,7 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {
     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 run_command();
+    void run_command(const char *fmt, ...);
 
     uint8_t *pktbuf;
     uint16_t pktoffset;