SITL: FlightAxis: add options bitmask parameter

This commit is contained in:
Bob Long 2024-10-02 18:29:21 +10:00 committed by Peter Barker
parent 5e47fa1353
commit 9fdd0a4e84
5 changed files with 57 additions and 14 deletions

View File

@ -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,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

View File

@ -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 @@ 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

View File

@ -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

View File

@ -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
};

View File

@ -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;