mirror of https://github.com/ArduPilot/ardupilot
SITL: FlightAxis: add options bitmask parameter
This commit is contained in:
parent
5e47fa1353
commit
9fdd0a4e84
|
@ -18,7 +18,7 @@
|
||||||
|
|
||||||
#include "SIM_FlightAxis.h"
|
#include "SIM_FlightAxis.h"
|
||||||
|
|
||||||
#if HAL_SIM_FLIGHTAXIS_ENABLED
|
#if AP_SIM_FLIGHTAXIS_ENABLED
|
||||||
|
|
||||||
#include <arpa/inet.h>
|
#include <arpa/inet.h>
|
||||||
#include <errno.h>
|
#include <errno.h>
|
||||||
|
@ -36,6 +36,17 @@ extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
using namespace SITL;
|
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
|
we use a thread for socket creation to reduce the impact of socket
|
||||||
creation latency. These condition variables are used to synchronise
|
creation latency. These condition variables are used to synchronise
|
||||||
|
@ -108,10 +119,18 @@ static double timestamp_sec()
|
||||||
FlightAxis::FlightAxis(const char *frame_str) :
|
FlightAxis::FlightAxis(const char *frame_str) :
|
||||||
Aircraft(frame_str)
|
Aircraft(frame_str)
|
||||||
{
|
{
|
||||||
|
AP::sitl()->models.flightaxis_ptr = this;
|
||||||
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
|
|
||||||
use_time_sync = false;
|
use_time_sync = false;
|
||||||
rate_hz = 250 / target_speedup;
|
rate_hz = 250 / target_speedup;
|
||||||
heli_demix = strstr(frame_str, "helidemix") != nullptr;
|
if(strstr(frame_str, "helidemix") != nullptr) {
|
||||||
rev4_servos = strstr(frame_str, "rev4") != 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, ':');
|
const char *colon = strchr(frame_str, ':');
|
||||||
if (colon) {
|
if (colon) {
|
||||||
controller_ip = colon+1;
|
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;
|
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
|
// swap first 4 and last 4 servos, for quadplane testing
|
||||||
float saved[4];
|
float saved[4];
|
||||||
memcpy(saved, &scaled_servos[0], sizeof(saved));
|
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));
|
memcpy(&scaled_servos[4], saved, sizeof(saved));
|
||||||
}
|
}
|
||||||
|
|
||||||
if (heli_demix) {
|
if (option_is_set(Option::HeliDemix)) {
|
||||||
// FlightAxis expects "roll/pitch/collective/yaw" input
|
// FlightAxis expects "roll/pitch/collective/yaw" input
|
||||||
float swash1 = scaled_servos[0];
|
float swash1 = scaled_servos[0];
|
||||||
float swash2 = scaled_servos[1];
|
float swash2 = scaled_servos[1];
|
||||||
|
@ -615,4 +634,4 @@ void FlightAxis::socket_creator(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // HAL_SIM_FLIGHTAXIS_ENABLED
|
#endif // AP_SIM_FLIGHTAXIS_ENABLED
|
||||||
|
|
|
@ -19,12 +19,9 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <AP_HAL/AP_HAL_Boards.h>
|
#include <AP_HAL/AP_HAL_Boards.h>
|
||||||
|
#include "SIM_config.h"
|
||||||
|
|
||||||
#ifndef HAL_SIM_FLIGHTAXIS_ENABLED
|
#if AP_SIM_FLIGHTAXIS_ENABLED
|
||||||
#define HAL_SIM_FLIGHTAXIS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if HAL_SIM_FLIGHTAXIS_ENABLED
|
|
||||||
|
|
||||||
#include <AP_HAL/utility/Socket_native.h>
|
#include <AP_HAL/utility/Socket_native.h>
|
||||||
|
|
||||||
|
@ -39,6 +36,8 @@ class FlightAxis : public Aircraft {
|
||||||
public:
|
public:
|
||||||
FlightAxis(const char *frame_str);
|
FlightAxis(const char *frame_str);
|
||||||
|
|
||||||
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
/* update model by one time step */
|
/* update model by one time step */
|
||||||
void update(const struct sitl_input &input) override;
|
void update(const struct sitl_input &input) override;
|
||||||
|
|
||||||
|
@ -175,12 +174,22 @@ private:
|
||||||
|
|
||||||
struct sitl_input last_input;
|
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 average_frame_time_s;
|
||||||
double extrapolated_s;
|
double extrapolated_s;
|
||||||
double initial_time_s;
|
double initial_time_s;
|
||||||
double last_time_s;
|
double last_time_s;
|
||||||
bool heli_demix;
|
|
||||||
bool rev4_servos;
|
|
||||||
bool controller_started;
|
bool controller_started;
|
||||||
uint32_t glitch_count;
|
uint32_t glitch_count;
|
||||||
uint64_t frame_counter;
|
uint64_t frame_counter;
|
||||||
|
@ -204,4 +213,4 @@ private:
|
||||||
|
|
||||||
} // namespace SITL
|
} // namespace SITL
|
||||||
|
|
||||||
#endif // HAL_SIM_FLIGHTAXIS_ENABLED
|
#endif // AP_SIM_FLIGHTAXIS_ENABLED
|
||||||
|
|
|
@ -55,6 +55,10 @@
|
||||||
#define AP_SIM_SLUNGPAYLOAD_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
#define AP_SIM_SLUNGPAYLOAD_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef AP_SIM_FLIGHTAXIS_ENABLED
|
||||||
|
#define AP_SIM_FLIGHTAXIS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifndef AP_SIM_TSYS03_ENABLED
|
#ifndef AP_SIM_TSYS03_ENABLED
|
||||||
#define AP_SIM_TSYS03_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
#define AP_SIM_TSYS03_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -39,6 +39,7 @@
|
||||||
|
|
||||||
#include "SIM_StratoBlimp.h"
|
#include "SIM_StratoBlimp.h"
|
||||||
#include "SIM_Glider.h"
|
#include "SIM_Glider.h"
|
||||||
|
#include "SIM_FlightAxis.h"
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
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),
|
AP_SUBGROUPINFO(slung_payload_sim, "SLUP_", 4, SIM::ModelParm, SlungPayloadSim),
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if AP_SIM_FLIGHTAXIS_ENABLED
|
||||||
|
// @Group: RFL_
|
||||||
|
// @Path: ./SIM_FlightAxis.cpp
|
||||||
|
AP_SUBGROUPPTR(flightaxis_ptr, "RFL_", 5, SIM::ModelParm, FlightAxis),
|
||||||
|
#endif
|
||||||
|
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -51,6 +51,7 @@ struct float_array {
|
||||||
|
|
||||||
class StratoBlimp;
|
class StratoBlimp;
|
||||||
class Glider;
|
class Glider;
|
||||||
|
class FlightAxis;
|
||||||
|
|
||||||
struct sitl_fdm {
|
struct sitl_fdm {
|
||||||
// this is the structure passed between FDM models and the main SITL code
|
// this is the structure passed between FDM models and the main SITL code
|
||||||
|
@ -326,6 +327,9 @@ public:
|
||||||
#endif
|
#endif
|
||||||
#if AP_SIM_SLUNGPAYLOAD_ENABLED
|
#if AP_SIM_SLUNGPAYLOAD_ENABLED
|
||||||
SlungPayloadSim slung_payload_sim;
|
SlungPayloadSim slung_payload_sim;
|
||||||
|
#endif
|
||||||
|
#if AP_SIM_FLIGHTAXIS_ENABLED
|
||||||
|
FlightAxis *flightaxis_ptr;
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
ModelParm models;
|
ModelParm models;
|
||||||
|
|
Loading…
Reference in New Issue