From 9fdd0a4e84001902c4452750d027e82b5afd93e2 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 2 Oct 2024 18:29:21 +1000 Subject: [PATCH] SITL: FlightAxis: add options bitmask parameter --- libraries/SITL/SIM_FlightAxis.cpp | 31 +++++++++++++++++++++++++------ libraries/SITL/SIM_FlightAxis.h | 25 +++++++++++++++++-------- libraries/SITL/SIM_config.h | 4 ++++ libraries/SITL/SITL.cpp | 7 +++++++ libraries/SITL/SITL.h | 4 ++++ 5 files changed, 57 insertions(+), 14 deletions(-) diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index b2aff331f8..52b24f03c3 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -18,7 +18,7 @@ #include "SIM_FlightAxis.h" -#if HAL_SIM_FLIGHTAXIS_ENABLED +#if AP_SIM_FLIGHTAXIS_ENABLED #include #include @@ -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,7 +320,7 @@ 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)); @@ -309,7 +328,7 @@ void FlightAxis::exchange_data(const struct sitl_input &input) 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 diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h index c8fcd97c19..e19e11d94d 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/libraries/SITL/SIM_FlightAxis.h @@ -19,12 +19,9 @@ #pragma once #include +#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 @@ -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 @@ private: 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 @@ private: } // namespace SITL -#endif // HAL_SIM_FLIGHTAXIS_ENABLED +#endif // AP_SIM_FLIGHTAXIS_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index aa281c45f7..91956ad90c 100644 --- a/libraries/SITL/SIM_config.h +++ b/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 diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 970fdd6a52..1ff14520b7 100644 --- a/libraries/SITL/SITL.cpp +++ b/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 }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index d3ad9ebbd9..e23b2f6144 100644 --- a/libraries/SITL/SITL.h +++ b/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 @@ public: #endif #if AP_SIM_SLUNGPAYLOAD_ENABLED SlungPayloadSim slung_payload_sim; +#endif +#if AP_SIM_FLIGHTAXIS_ENABLED + FlightAxis *flightaxis_ptr; #endif }; ModelParm models;