SITL: FlightAxis: add options bitmask parameter

libraries/SITL/SIM_FlightAxis.cpp

@@ -18,7 +18,7 @@
 
 #include "SIM_FlightAxis.h"
 
-#if HAL_SIM_FLIGHTAXIS_ENABLED
+#if AP_SIM_FLIGHTAXIS_ENABLED
 
 #include <arpa/inet.h>
 #include <errno.h>
@@ -36,6 +36,17 @@ extern const AP_HAL::HAL& hal;
 
 using namespace SITL;
 
+const AP_Param::GroupInfo FlightAxis::var_info[] = {
+    // @Param: OPTS
+    // @DisplayName: FlightAxis options
+    // @Description: Bitmask of FlightAxis options
+    // @Bitmask: 1: Swap first 4 and last 4 servos (for quadplane testing)
+    // @Bitmask: 2: Demix heli servos and send roll/pitch/collective/yaw
+    // @User: Advanced
+    AP_GROUPINFO("OPTS", 1, FlightAxis, _options, 0),
+    AP_GROUPEND
+};
+
 /*
   we use a thread for socket creation to reduce the impact of socket
   creation latency. These condition variables are used to synchronise
@@ -108,10 +119,18 @@ static double timestamp_sec()
 FlightAxis::FlightAxis(const char *frame_str) :
     Aircraft(frame_str)
 {
+    AP::sitl()->models.flightaxis_ptr = this;
+    AP_Param::setup_object_defaults(this, var_info);
+
     use_time_sync = false;
     rate_hz = 250 / target_speedup;
-    heli_demix = strstr(frame_str, "helidemix") != nullptr;
-    rev4_servos = strstr(frame_str, "rev4") != nullptr;
+    if(strstr(frame_str, "helidemix") != nullptr) {
+        _options.set(_options | uint32_t(Option::HeliDemix));
+    }
+    if(strstr(frame_str, "rev4") != nullptr) {
+        _options.set(_options | uint32_t(Option::Rev4Servos));
+    }
+
     const char *colon = strchr(frame_str, ':');
     if (colon) {
         controller_ip = colon+1;
@@ -301,15 +320,15 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
         scaled_servos[i] = (input.servos[i] - 1000) / 1000.0f;
     }
 
-    if (rev4_servos) {
+    if (option_is_set(Option::Rev4Servos)) {
         // swap first 4 and last 4 servos, for quadplane testing
         float saved[4];
         memcpy(saved, &scaled_servos[0], sizeof(saved));
         memcpy(&scaled_servos[0], &scaled_servos[4], sizeof(saved));
         memcpy(&scaled_servos[4], saved, sizeof(saved));
     }
 
-    if (heli_demix) {
+    if (option_is_set(Option::HeliDemix)) {
         // FlightAxis expects "roll/pitch/collective/yaw" input
         float swash1 = scaled_servos[0];
         float swash2 = scaled_servos[1];
@@ -615,4 +634,4 @@ void FlightAxis::socket_creator(void)
     }
 }
 
-#endif // HAL_SIM_FLIGHTAXIS_ENABLED
+#endif // AP_SIM_FLIGHTAXIS_ENABLED

libraries/SITL/SIM_FlightAxis.h

@@ -19,12 +19,9 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL_Boards.h>
+#include "SIM_config.h"
 
-#ifndef HAL_SIM_FLIGHTAXIS_ENABLED
-#define HAL_SIM_FLIGHTAXIS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
-#endif
-
-#if HAL_SIM_FLIGHTAXIS_ENABLED
+#if AP_SIM_FLIGHTAXIS_ENABLED
 
 #include <AP_HAL/utility/Socket_native.h>
 
@@ -39,6 +36,8 @@ class FlightAxis : public Aircraft {
 public:
     FlightAxis(const char *frame_str);
 
+    static const struct AP_Param::GroupInfo var_info[];
+
     /* update model by one time step */
     void update(const struct sitl_input &input) override;
 
@@ -175,12 +174,22 @@ class FlightAxis : public Aircraft {
 
     struct sitl_input last_input;
 
+    AP_Int32 _options;
+
+    enum class Option : uint32_t{
+        Rev4Servos    = (1U<<1),
+        HeliDemix     = (1U<<2),
+    };
+
+    // return true if an option is set
+    bool option_is_set(Option option) const {
+        return (uint32_t(option) & uint32_t(_options)) != 0;
+    }
+
     double average_frame_time_s;
     double extrapolated_s;
     double initial_time_s;
     double last_time_s;
-    bool heli_demix;
-    bool rev4_servos;
     bool controller_started;
     uint32_t glitch_count;
     uint64_t frame_counter;
@@ -204,4 +213,4 @@ class FlightAxis : public Aircraft {
 
 } // namespace SITL
 
-#endif // HAL_SIM_FLIGHTAXIS_ENABLED
+#endif // AP_SIM_FLIGHTAXIS_ENABLED

libraries/SITL/SIM_config.h

@@ -55,6 +55,10 @@
 #define AP_SIM_SLUNGPAYLOAD_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
 
+#ifndef AP_SIM_FLIGHTAXIS_ENABLED
+#define AP_SIM_FLIGHTAXIS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif
+
 #ifndef AP_SIM_TSYS03_ENABLED
 #define AP_SIM_TSYS03_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif

libraries/SITL/SITL.cpp

@@ -39,6 +39,7 @@
 
 #include "SIM_StratoBlimp.h"
 #include "SIM_Glider.h"
+#include "SIM_FlightAxis.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -1487,6 +1488,12 @@ const AP_Param::GroupInfo SIM::ModelParm::var_info[] = {
     AP_SUBGROUPINFO(slung_payload_sim, "SLUP_", 4, SIM::ModelParm, SlungPayloadSim),
 #endif
 
+#if AP_SIM_FLIGHTAXIS_ENABLED
+    // @Group: RFL_
+    // @Path: ./SIM_FlightAxis.cpp
+    AP_SUBGROUPPTR(flightaxis_ptr, "RFL_", 5, SIM::ModelParm, FlightAxis),
+#endif
+
     AP_GROUPEND
 };
 

libraries/SITL/SITL.h

@@ -51,6 +51,7 @@ struct float_array {
 
 class StratoBlimp;
 class Glider;
+class FlightAxis;
 
 struct sitl_fdm {
     // this is the structure passed between FDM models and the main SITL code
@@ -326,6 +327,9 @@ class SIM {
 #endif
 #if AP_SIM_SLUNGPAYLOAD_ENABLED
         SlungPayloadSim slung_payload_sim;
+#endif
+#if AP_SIM_FLIGHTAXIS_ENABLED
+        FlightAxis *flightaxis_ptr;
 #endif
     };
     ModelParm models;