From 4536af3eeb49d89062ee7d8fcc5a0dcaa9063884 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 2 Oct 2024 18:29:21 +1000 Subject: [PATCH 1/3] 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 b2aff331f8d31..52b24f03c3b6a 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 c8fcd97c19d52..e19e11d94d810 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 @@ 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 diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index aa281c45f753a..91956ad90c2ae 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 970fdd6a52f41..1ff14520b72df 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 d3ad9ebbd962a..e23b2f6144791 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 @@ class SIM { #endif #if AP_SIM_SLUNGPAYLOAD_ENABLED SlungPayloadSim slung_payload_sim; +#endif +#if AP_SIM_FLIGHTAXIS_ENABLED + FlightAxis *flightaxis_ptr; #endif }; ModelParm models; From 53b631743e2241906380396b9a46d95a9595db66 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 2 Oct 2024 18:45:52 +1000 Subject: [PATCH 2/3] SITL: FlightAxis: add position reset option --- libraries/SITL/SIM_FlightAxis.cpp | 12 +++++++++++- libraries/SITL/SIM_FlightAxis.h | 1 + 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 52b24f03c3b6a..bd33e5822faf2 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -40,10 +40,11 @@ const AP_Param::GroupInfo FlightAxis::var_info[] = { // @Param: OPTS // @DisplayName: FlightAxis options // @Description: Bitmask of FlightAxis options + // @Bitmask: 0: Reset position on startup // @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_GROUPINFO("OPTS", 1, FlightAxis, _options, uint32_t(Option::ResetPosition)), AP_GROUPEND }; @@ -303,6 +304,15 @@ void FlightAxis::exchange_data(const struct sitl_input &input) )"); soap_request_end(1000); + if(option_is_set(Option::ResetPosition)) { + soap_request_start("ResetAircraft", R"( + + +12 + +)"); + soap_request_end(1000); + } soap_request_start("InjectUAVControllerInterface", R"( diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h index e19e11d94d810..ea987c663b859 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/libraries/SITL/SIM_FlightAxis.h @@ -177,6 +177,7 @@ class FlightAxis : public Aircraft { AP_Int32 _options; enum class Option : uint32_t{ + ResetPosition = (1U<<0), Rev4Servos = (1U<<1), HeliDemix = (1U<<2), }; From 0556889d8d35d81f1e9ef31c8ed2a295480bb844 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 2 Oct 2024 19:24:26 +1000 Subject: [PATCH 3/3] SITL: FlightAxis: add option to silence FPS --- libraries/SITL/SIM_FlightAxis.cpp | 7 +++++-- libraries/SITL/SIM_FlightAxis.h | 1 + 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index bd33e5822faf2..0e1a947449e6b 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -43,6 +43,7 @@ const AP_Param::GroupInfo FlightAxis::var_info[] = { // @Bitmask: 0: Reset position on startup // @Bitmask: 1: Swap first 4 and last 4 servos (for quadplane testing) // @Bitmask: 2: Demix heli servos and send roll/pitch/collective/yaw + // @Bitmask: 3: Don't print frame rate stats // @User: Advanced AP_GROUPINFO("OPTS", 1, FlightAxis, _options, uint32_t(Option::ResetPosition)), AP_GROUPEND @@ -603,8 +604,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(!option_is_set(Option::SilenceFPS)) { + 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 index ea987c663b859..276a2aa69c20d 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/libraries/SITL/SIM_FlightAxis.h @@ -180,6 +180,7 @@ class FlightAxis : public Aircraft { ResetPosition = (1U<<0), Rev4Servos = (1U<<1), HeliDemix = (1U<<2), + SilenceFPS = (1U<<3), }; // return true if an option is set