SIM_FlightAxis: create autotest option

- Send ResetAircraft command on startup - Take RC inputs from the regular SITL UDP port - Silence the console spam
ArgenTech-Solutions · Mar 24, 2024 · b3f94ab · b3f94ab
1 parent 3d068ab
commit b3f94ab
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 6 deletions.
diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp
@@ -112,6 +112,7 @@ FlightAxis::FlightAxis(const char *frame_str) :
     rate_hz = 250 / target_speedup;
     heli_demix = strstr(frame_str, "helidemix") != nullptr;
     rev4_servos = strstr(frame_str, "rev4") != nullptr;
+    autotest = strstr(frame_str, "autotest") != nullptr;
     const char *colon = strchr(frame_str, ':');
     if (colon) {
         controller_ip = colon+1;
@@ -284,6 +285,15 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
 </soap:Body>
 </soap:Envelope>)");
         soap_request_end(1000);
+        if(autotest) {
+            soap_request_start("ResetAircraft", R"(<?xml version='1.0' encoding='UTF-8'?>
+<soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
+<soap:Body>
+<ResetAircraft><a>1</a><b>2</b></ResetAircraft>
+</soap:Body>
+</soap:Envelope>)");
+            soap_request_end(1000);
+        }
         soap_request_start("InjectUAVControllerInterface", R"(<?xml version='1.0' encoding='UTF-8'?>
 <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
 <soap:Body>
@@ -516,9 +526,11 @@ void FlightAxis::update(const struct sitl_input &input)
     /*
       the interlink interface supports 12 input channels
      */
-    rcin_chan_count = 12;
-    for (uint8_t i=0; i<rcin_chan_count; i++) {
-        rcin[i] = state.rcin[i];
+    if(!autotest) {
+        rcin_chan_count = 12;
+        for (uint8_t i=0; i<rcin_chan_count; i++) {
+            rcin[i] = state.rcin[i];
+        }
     }
 
     update_position();
@@ -538,7 +550,7 @@ void FlightAxis::update(const struct sitl_input &input)
             // we've had a network glitch, compensate by advancing initial time
             float adjustment_s = (dt_us-glitch_threshold_us)*1.0e-6;
             initial_time_s += adjustment_s;
-            printf("glitch %.2fs\n", adjustment_s);
+            if(!autotest) printf("glitch %.2fs\n", adjustment_s);
             dt_us = glitch_threshold_us;
             glitch_count++;
         }
@@ -572,8 +584,10 @@ void FlightAxis::report_FPS(void)
             uint64_t frames = socket_frame_counter - last_socket_frame_counter;
             last_socket_frame_counter = socket_frame_counter;
             double dt = state.m_currentPhysicsTime_SEC - last_frame_count_s;
-            printf("%.2f/%.2f FPS avg=%.2f glitches=%u\n",
-                   frames / dt, 1000 / dt, 1.0/average_frame_time_s, unsigned(glitch_count));
+            if(!autotest) {
+                printf("%.2f/%.2f FPS avg=%.2f glitches=%u\n",
+                       frames / dt, 1000 / dt, 1.0/average_frame_time_s, unsigned(glitch_count));
+            }
         } else {
             printf("Initial position %f %f %f\n", position.x, position.y, position.z);
         }

diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h
@@ -181,6 +181,7 @@ class FlightAxis : public Aircraft {
     double last_time_s;
     bool heli_demix;
     bool rev4_servos;
+    bool autotest;
     bool controller_started;
     uint32_t glitch_count;
     uint64_t frame_counter;