SITL: add Sagetech MXS simulator
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
6d1d9df875
commit
fd67c71eb0
@ -28,6 +28,7 @@
|
||||
|
||||
#include "SIM_Aircraft.h"
|
||||
#include <AP_HAL_SITL/SITL_State.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
|
||||
namespace SITL {
|
||||
|
||||
@ -82,10 +83,15 @@ void ADSB_Vehicle::update(float delta_t)
|
||||
}
|
||||
}
|
||||
|
||||
bool ADSB_Vehicle::location(Location &loc) const
|
||||
{
|
||||
return AP::ahrs().get_location_from_home_offset(loc, position);
|
||||
}
|
||||
|
||||
/*
|
||||
update the ADSB peripheral state
|
||||
*/
|
||||
void ADSB::update(const class Aircraft &aircraft)
|
||||
void ADSB::update_simulated_vehicles(const class Aircraft &aircraft)
|
||||
{
|
||||
if (_sitl == nullptr) {
|
||||
_sitl = AP::sitl();
|
||||
@ -109,11 +115,36 @@ void ADSB::update(const class Aircraft &aircraft)
|
||||
float delta_t = (now_us - last_update_us) * 1.0e-6f;
|
||||
last_update_us = now_us;
|
||||
|
||||
const Location &home = aircraft.get_home();
|
||||
|
||||
for (uint8_t i=0; i<num_vehicles; i++) {
|
||||
vehicles[i].update(delta_t);
|
||||
|
||||
Location loc = home;
|
||||
|
||||
ADSB_Vehicle &vehicle = vehicles[i];
|
||||
loc.offset(vehicle.position.x, vehicle.position.y);
|
||||
|
||||
// re-init when exceeding radius range
|
||||
if (home.get_distance(loc) > _sitl->adsb_radius_m) {
|
||||
vehicle.initialised = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ADSB::update(const class Aircraft &aircraft)
|
||||
{
|
||||
update_simulated_vehicles(aircraft);
|
||||
|
||||
// see if we should do a report.
|
||||
if ((_sitl->adsb_types & (1U << (uint8_t)SIM::ADSBType::Shortcut)) == 0) {
|
||||
// some other simulated device is in use (e.g. MXS)
|
||||
return;
|
||||
}
|
||||
|
||||
// see if we should do a report
|
||||
// bakwards compatability; the parameters specify ADSB simulation,
|
||||
// but we are not configured to use a simulated ADSB driver.
|
||||
// Pretend to be a uAvionix mavlink device:
|
||||
send_report(aircraft);
|
||||
}
|
||||
|
||||
@ -190,16 +221,14 @@ void ADSB::send_report(const class Aircraft &aircraft)
|
||||
uint32_t now_us = AP_HAL::micros();
|
||||
if (now_us - last_report_us >= reporting_period_ms*1000UL) {
|
||||
for (uint8_t i=0; i<num_vehicles; i++) {
|
||||
ADSB_Vehicle &vehicle = vehicles[i];
|
||||
Location loc = home;
|
||||
|
||||
loc.offset(vehicle.position.x, vehicle.position.y);
|
||||
|
||||
// re-init when exceeding radius range
|
||||
if (home.get_distance(loc) > _sitl->adsb_radius_m) {
|
||||
vehicle.initialised = false;
|
||||
const ADSB_Vehicle &vehicle = vehicles[i];
|
||||
if (!vehicle.initialised) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Location loc = home;
|
||||
loc.offset(vehicle.position.x, vehicle.position.y);
|
||||
|
||||
mavlink_adsb_vehicle_t adsb_vehicle {};
|
||||
last_report_us = now_us;
|
||||
|
||||
|
||||
@ -34,28 +34,37 @@ namespace SITL {
|
||||
class ADSB_Vehicle {
|
||||
friend class ADSB;
|
||||
|
||||
public:
|
||||
bool initialised = false;
|
||||
|
||||
bool location(class Location &) const; // return vehicle absolute location
|
||||
// return earth-frame vehicle velocity:
|
||||
bool velocity(Vector3F &ret) const { ret = velocity_ef; return true; }
|
||||
|
||||
uint32_t ICAO_address;
|
||||
Vector3F velocity_ef; // NED
|
||||
char callsign[9];
|
||||
|
||||
private:
|
||||
void update(float delta_t);
|
||||
|
||||
Vector3f position; // NED from origin
|
||||
Vector3f velocity_ef; // NED
|
||||
char callsign[9];
|
||||
uint32_t ICAO_address;
|
||||
bool initialised = false;
|
||||
Vector3p position; // NED from origin
|
||||
ADSB_EMITTER_TYPE type;
|
||||
uint64_t stationary_object_created_ms; // allows expiring of slow/stationary objects
|
||||
};
|
||||
|
||||
class ADSB : public SerialDevice {
|
||||
class ADSB : public SerialDevice {
|
||||
public:
|
||||
ADSB() {};
|
||||
void update(const class Aircraft &aircraft);
|
||||
|
||||
private:
|
||||
uint8_t num_vehicles;
|
||||
static const uint8_t num_vehicles_MAX = 200;
|
||||
ADSB_Vehicle vehicles[num_vehicles_MAX];
|
||||
|
||||
private:
|
||||
void update_simulated_vehicles(const class Aircraft &aircraft);
|
||||
|
||||
// reporting period in ms
|
||||
const float reporting_period_ms = 1000;
|
||||
uint32_t last_report_us;
|
||||
|
||||
12
libraries/SITL/SIM_ADSB_Device.h
Normal file
12
libraries/SITL/SIM_ADSB_Device.h
Normal file
@ -0,0 +1,12 @@
|
||||
#pragma once
|
||||
|
||||
#include "SIM_SerialDevice.h"
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class ADSB_Device : public SerialDevice
|
||||
{
|
||||
using SerialDevice::SerialDevice;
|
||||
};
|
||||
|
||||
}; // namespace SITL
|
||||
562
libraries/SITL/SIM_ADSB_Sagetech_MXS.cpp
Normal file
562
libraries/SITL/SIM_ADSB_Sagetech_MXS.cpp
Normal file
@ -0,0 +1,562 @@
|
||||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_ADSB_SAGETECH_MXS_ENABLED
|
||||
|
||||
#include "SIM_ADSB_Sagetech_MXS.h"
|
||||
|
||||
#include <AP_Common/Location.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_InternalError/AP_InternalError.h>
|
||||
#include "SITL.h"
|
||||
#include <ctype.h>
|
||||
#include "SIM_Aircraft.h"
|
||||
#include "SIM_ADSB.h"
|
||||
|
||||
#include <errno.h>
|
||||
|
||||
static const uint8_t FIRST_BYTE { 0xAA };
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
void ADSB_Sagetech_MXS::move_preamble_in_buffer(uint8_t search_start_pos)
|
||||
{
|
||||
uint8_t i;
|
||||
for (i=search_start_pos; i<buflen; i++) {
|
||||
if ((uint8_t)msg.buffer[i] == FIRST_BYTE) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i == 0) {
|
||||
return;
|
||||
}
|
||||
memmove(msg.buffer, &msg.buffer[i], buflen-i);
|
||||
buflen = buflen - i;
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::update_serial_input()
|
||||
{
|
||||
const ssize_t n = read_from_autopilot(&msg.buffer[buflen], ARRAY_SIZE(msg.buffer) - buflen - 1);
|
||||
if (n < 0) {
|
||||
// TODO: do better here
|
||||
if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
|
||||
AP_HAL::panic("Failed to read from autopilot");
|
||||
}
|
||||
} else {
|
||||
buflen += n;
|
||||
}
|
||||
|
||||
|
||||
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "read (%u) bytes", (unsigned)n);
|
||||
|
||||
switch (input_state) {
|
||||
case InputState::WANT_START_BYTE:
|
||||
move_preamble_in_buffer();
|
||||
if (buflen == 0) {
|
||||
return;
|
||||
}
|
||||
set_input_state(InputState::WANT_PREAMBLE);
|
||||
FALLTHROUGH;
|
||||
case InputState::WANT_PREAMBLE:
|
||||
if (buflen < sizeof(msg.preamble)) {
|
||||
return;
|
||||
}
|
||||
set_input_state(InputState::WANT_PAYLOAD);
|
||||
FALLTHROUGH;
|
||||
case InputState::WANT_PAYLOAD: {
|
||||
// 1 preamble, 2 flags, 1 msg + payload
|
||||
const uint8_t want_len = 4 + msg.preamble.payload_length;
|
||||
if (buflen < want_len) {
|
||||
return;
|
||||
}
|
||||
set_input_state(InputState::WANT_CHECKSUM);
|
||||
FALLTHROUGH;
|
||||
}
|
||||
case InputState::WANT_CHECKSUM: {
|
||||
// 1 start byte, 1 msg type, 1 msgid, 1 payload len
|
||||
const uint8_t want_len = sizeof(msg.preamble) + msg.preamble.payload_length + 1;
|
||||
if (buflen < want_len) {
|
||||
return;
|
||||
}
|
||||
const uint8_t received_checksum = msg.buffer[want_len-1];
|
||||
|
||||
uint8_t calculated_checksum = 0;
|
||||
for (uint8_t i=0; i<sizeof(msg.preamble)+msg.preamble.payload_length; i++) {
|
||||
calculated_checksum += msg.buffer[i];
|
||||
}
|
||||
|
||||
if (calculated_checksum == received_checksum) {
|
||||
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "sim-MXS: Got one (%u)!", (unsigned)msg.preamble.msgtype);
|
||||
handle_message();
|
||||
move_preamble_in_buffer(want_len);
|
||||
} else {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "sim-MXS: Bad checksum");
|
||||
move_preamble_in_buffer(1);
|
||||
}
|
||||
// consume these bytes
|
||||
set_input_state(InputState::WANT_PREAMBLE);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool ADSB_Sagetech_MXS::_handle_message()
|
||||
{
|
||||
switch (msg.preamble.msgtype) {
|
||||
case MsgType::INSTALLATION:
|
||||
return handle_message(msg.packed_installation.msg);
|
||||
case MsgType::FLIGHTID:
|
||||
return handle_message(msg.packed_flightidmessage.msg);
|
||||
case MsgType::OPMSG:
|
||||
return handle_message(msg.packed_operating.msg);
|
||||
case MsgType::GPS:
|
||||
return handle_message(msg.packed_gps.msg);
|
||||
case MsgType::DATAREQ:
|
||||
return handle_message(msg.packed_data_req.msg);
|
||||
case MsgType::TARGETREQUEST:
|
||||
return handle_message(msg.packed_target_req.msg);
|
||||
|
||||
case MsgType::ACK:
|
||||
case MsgType::STATEVECTORREPORT:
|
||||
case MsgType::MODESTATUSREPORT:
|
||||
// we should never receive one of these?
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
break;
|
||||
}
|
||||
|
||||
// we should handle all messages:
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::handle_message()
|
||||
{
|
||||
if (!_handle_message()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// store ack info for sending an ack later:
|
||||
ack_info.ackType = msg.preamble.msgtype;
|
||||
ack_info.ackId = msg.preamble.msgid;
|
||||
ack_info.send = true;
|
||||
}
|
||||
|
||||
bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::Installation& opmsg)
|
||||
{
|
||||
assert_storage_size<Installation, 36> _assert_storage_size_Installation;
|
||||
(void)_assert_storage_size_Installation;
|
||||
|
||||
if (operating_mode != OperatingMode::OFF &&
|
||||
operating_mode != OperatingMode::MAINTENANCE) {
|
||||
// see page 10 - ignored if not in one of those. We could
|
||||
// return silently here if there are race conditions
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
}
|
||||
|
||||
// do something!
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::assert_good_flight_id()
|
||||
{
|
||||
static_assert(sizeof(flight_id) == 8, "correct storage size");
|
||||
|
||||
bool space_seen = false;
|
||||
for (uint8_t i=0; i<sizeof(flight_id); i++) {
|
||||
// this doesn't seem right; they really allow any ASCII
|
||||
// character? vertical tabs, for example?! The standard is
|
||||
// ~US$750. Null (0x0) is a valid ascii character!
|
||||
if (!isascii(flight_id[i])) {
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
}
|
||||
if (flight_id[i] == ' ') {
|
||||
space_seen = true;
|
||||
}
|
||||
if (space_seen && flight_id[i] != ' ') {
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::FlightIDMessage& _msg)
|
||||
{
|
||||
assert_storage_size<FlightIDMessage, 12> _assert_storage_size_FlightIDMessage;
|
||||
(void)_assert_storage_size_FlightIDMessage;
|
||||
|
||||
// do something!
|
||||
flight_id_set_time_ms = AP_HAL::millis();
|
||||
memcpy(flight_id, _msg.flight_id, sizeof(flight_id));
|
||||
assert_good_flight_id();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::Operating& _msg)
|
||||
{
|
||||
assert_storage_size<Operating, 12> _assert_storage_size_Operating;
|
||||
(void)_assert_storage_size_Operating;
|
||||
|
||||
// do something!
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#define ORD(x) (x - '0')
|
||||
|
||||
// buffer contains DDDMM.MMMMM (degrees, minutes including fraction)
|
||||
double ADSB_Sagetech_MXS::lon_string_to_double(const uint8_t *str)
|
||||
{
|
||||
return ORD(str[0]) * 100 + lat_string_to_double(&str[1]);
|
||||
}
|
||||
|
||||
// buffer contains DDMM.MMMMM (degrees, minutes including fraction)
|
||||
double ADSB_Sagetech_MXS::lat_string_to_double(const uint8_t *str)
|
||||
{
|
||||
double degrees = ORD(str[0]) * 10 + ORD(str[1]);
|
||||
double minutes = ORD(str[2]) * 10.0 + ORD(str[3]) + ORD(str[5])*0.1 + ORD(str[6])*0.01 + ORD(str[7]) * 0.001 + ORD(str[8]) * 0.0001 + ORD(str[9]) * 0.00001;
|
||||
return degrees + minutes/60.0;
|
||||
}
|
||||
|
||||
#undef ORD
|
||||
|
||||
bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::GPS& _msg)
|
||||
{
|
||||
assert_storage_size<GPS, 63> _assert_storage_size_GPS;
|
||||
(void)_assert_storage_size_GPS;
|
||||
|
||||
// store data to transmit via ADSB
|
||||
info_from_vehicle.gps.lat = lat_string_to_double(_msg.latitude);
|
||||
info_from_vehicle.gps.lng = lon_string_to_double(_msg.longitude);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::DataRequest& _msg)
|
||||
{
|
||||
assert_storage_size<DataRequest, 4> _assert_storage_size_DataRequest;
|
||||
(void)_assert_storage_size_DataRequest;
|
||||
|
||||
// handle request to send data to vehicle. Note that the
|
||||
// specification says (on page 32) that the ack is sent before the
|
||||
// data.
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::TargetRequest& _msg)
|
||||
{
|
||||
assert_storage_size<TargetRequest, 7> _assert_storage_size_TargetRequest;
|
||||
(void)_assert_storage_size_TargetRequest;
|
||||
|
||||
// handle request to send adsb data to vehicle as it is received
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void ADSB_Sagetech_MXS::update_serial_output(const Aircraft *sitl_model)
|
||||
{
|
||||
// TODO: space checks here
|
||||
|
||||
if (ack_info.send) {
|
||||
uint8_t alt[3] {};
|
||||
uint8_t state = 0;
|
||||
Ack ack{
|
||||
ack_info.ackType,
|
||||
ack_info.ackId,
|
||||
state, // bitmask field
|
||||
alt
|
||||
};
|
||||
const PackedMessage<Ack> packed { ack, MsgType::ACK, msgid++ };
|
||||
write_to_autopilot((const char*)&packed, sizeof(packed));
|
||||
ack_info.send = false;
|
||||
}
|
||||
|
||||
update_serial_output_vehicles(sitl_model);
|
||||
}
|
||||
|
||||
|
||||
void ADSB_Sagetech_MXS::update_serial_output_vehicles(const SITL::Aircraft *sitl_model)
|
||||
{
|
||||
if (sitl_model->adsb == nullptr) {
|
||||
return;
|
||||
}
|
||||
for (uint8_t i=0; i<sitl_model->adsb->num_vehicles; i++) {
|
||||
const ADSB_Vehicle &vehicle = sitl_model->adsb->vehicles[i];
|
||||
if (!vehicle.initialised) {
|
||||
continue;
|
||||
}
|
||||
send_vehicle_message(vehicle);
|
||||
}
|
||||
}
|
||||
|
||||
// pack a floating-point latitude or longitude into three bytes
|
||||
// according to Sagetech definitions:
|
||||
void ADSB_Sagetech_MXS::pack_scaled_geocoord(uint8_t buf[3], float coord)
|
||||
{
|
||||
const int32_t scaled = coord * (1U<<23) / 180.0;
|
||||
pack_int32_into_uint8_ts(scaled, buf);
|
||||
}
|
||||
|
||||
// pack a floating-point altitude in metres into three bytes
|
||||
// according to Sagetech definitions:
|
||||
void ADSB_Sagetech_MXS::pack_scaled_alt(uint8_t buf[3], float alt_m)
|
||||
{
|
||||
const int32_t scaled = METRES_TO_FEET*alt_m * (1/0.015625);
|
||||
pack_int32_into_uint8_ts(scaled, buf);
|
||||
}
|
||||
|
||||
// discards the top 8 bits from source and shoves the rest into dest
|
||||
void ADSB_Sagetech_MXS::pack_int32_into_uint8_ts(int32_t source, uint8_t dest[3])
|
||||
{
|
||||
// FIXME: cast &source instead?
|
||||
dest[0] = source >> 16;
|
||||
dest[1] = source >> 8;
|
||||
dest[2] = source >> 0;
|
||||
}
|
||||
|
||||
uint8_t ADSB_Sagetech_MXS::scaled_groundspeed(float speed_m_s) const
|
||||
{
|
||||
if (is_zero(speed_m_s)) {
|
||||
return 0x01;
|
||||
}
|
||||
const float knots = M_PER_SEC_TO_KNOTS * speed_m_s;
|
||||
if (knots < 0.125) {
|
||||
return 0x02;
|
||||
}
|
||||
static const struct Threshold {
|
||||
float min;
|
||||
uint8_t code;
|
||||
float increment;
|
||||
} thresholds[] {
|
||||
{ 0.125, 0x03, 0.146 },
|
||||
{ 1, 0x09, 0.25 },
|
||||
{ 2, 0x0D, 0.50 },
|
||||
{ 15, 0x27, 1 },
|
||||
{ 70, 0x5e, 2 },
|
||||
{ 100, 0x6d, 5 },
|
||||
{ 175, 0x7c, 0 },
|
||||
};
|
||||
auto *entry = &thresholds[0];
|
||||
for (uint8_t i=1; i<ARRAY_SIZE(thresholds); i++) {
|
||||
auto *next_entry = &thresholds[i];
|
||||
if (knots > entry->min && knots < next_entry->min) {
|
||||
const uint8_t code_delta = next_entry->code - entry->code;
|
||||
return entry->code + uint8_t((knots - entry->min) / code_delta);
|
||||
}
|
||||
entry = next_entry;
|
||||
}
|
||||
return 0x7c;
|
||||
}
|
||||
void ADSB_Sagetech_MXS::pack_scaled_groundspeed(uint8_t dest[1], float speed_m_s) const
|
||||
{
|
||||
dest[0] = scaled_groundspeed(speed_m_s);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "speed in: %f", speed_m_s);
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::pack_scaled_airspeed(uint8_t dest[2], float speed_m_s) const
|
||||
{
|
||||
const int16_t scaled = M_PER_SEC_TO_KNOTS * speed_m_s * 8;
|
||||
dest[0] = scaled >> 8;
|
||||
dest[1] = scaled >> 0;
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::pack_scaled_vertical_rate(uint8_t dest[2], float speed_m_s) const
|
||||
{
|
||||
const int16_t scaled = METRES_TO_FEET * speed_m_s;
|
||||
dest[0] = scaled >> 8;
|
||||
dest[1] = scaled >> 0;
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::send_vehicle_message(const SITL::ADSB_Vehicle &vehicle)
|
||||
{
|
||||
send_vehicle_message_state_vector(vehicle);
|
||||
send_vehicle_message_status_report(vehicle);
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::send_vehicle_message_state_vector(const SITL::ADSB_Vehicle &vehicle)
|
||||
{
|
||||
enum class SVR_ValidityFlag : uint8_t {
|
||||
LatitudeAndLongitude = (1U << 7),
|
||||
Altitude_Geometric = (1U << 6),
|
||||
NS_and_EW_Velocity = (1U << 5),
|
||||
GroundSpeedWhileOnSurface = (1U << 4),
|
||||
HeadingWhileOnSurface = (1U << 3),
|
||||
Altitude_Barometric = (1U << 2),
|
||||
VerticalRate_Geometric = (1U << 1),
|
||||
VerticalRate_Barometric = (1U << 0),
|
||||
};
|
||||
enum class SVR_EstimateValidityFlag : uint8_t {
|
||||
LatitudeAndLongitude = (1U << 7),
|
||||
NS_and_EW_Velocity = (1U << 6),
|
||||
};
|
||||
|
||||
// this struct-based approach may prove to be too inflexible
|
||||
// if we want to change the shape of the report at runtime for
|
||||
// some reason.
|
||||
struct PACKED {
|
||||
StateVectorReport_ReportStructure report_structure;
|
||||
uint8_t validity_flags; // see SVR_ValidityFlag enum
|
||||
uint8_t estimated_validity_flags; // see SVR_EstimateValidityFlag enum
|
||||
uint8_t participant_address[3];
|
||||
uint8_t address_qualifier;
|
||||
uint16_t epos_toa;
|
||||
uint16_t pos_toa;
|
||||
uint16_t vel_toa;
|
||||
uint8_t latitude[3];
|
||||
uint8_t longitude[3];
|
||||
uint8_t alt_geometric[3];
|
||||
uint8_t ns_velocity[2];
|
||||
uint8_t ew_velocity[2];
|
||||
uint8_t up_velocity[2];
|
||||
// uint8_t groundspeed;
|
||||
// uint8_t ground_heading;
|
||||
} my_report {};
|
||||
|
||||
my_report.report_structure.rs0.msn = 1;
|
||||
|
||||
// note that the Sagetech SDK parser requires all of these fields
|
||||
// to be 1 to be an airborne vehicle::
|
||||
my_report.report_structure.rs0.time_of_applicability_for_estimated_positon = 1;
|
||||
my_report.report_structure.rs0.position_time_of_applicability = 1;
|
||||
my_report.report_structure.rs0.velocity_time_of_applicability = 1;
|
||||
|
||||
my_report.report_structure.rs2.estimated_longitude = 0;
|
||||
|
||||
my_report.validity_flags = 0;
|
||||
|
||||
pack_int32_into_uint8_ts(vehicle.ICAO_address, my_report.participant_address);
|
||||
my_report.address_qualifier = 0x02; // aircraft
|
||||
|
||||
Location loc;
|
||||
if (!vehicle.location(loc)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// pack in latitude/longitude:
|
||||
pack_scaled_geocoord(my_report.latitude, loc.lat * 1e-7);
|
||||
pack_scaled_geocoord(my_report.longitude, loc.lng * 1e-7);
|
||||
my_report.report_structure.rs0.lat_and_lng = 1;
|
||||
my_report.validity_flags |= (uint8_t)SVR_ValidityFlag::LatitudeAndLongitude;
|
||||
|
||||
// pack in altitude:
|
||||
pack_scaled_alt(my_report.alt_geometric, loc.alt*0.01);
|
||||
my_report.report_structure.rs1.altitude_geometric = 1;
|
||||
my_report.validity_flags |= (uint8_t)SVR_ValidityFlag::Altitude_Geometric;
|
||||
|
||||
// pack in north/south and east/west velocity
|
||||
pack_scaled_airspeed(my_report.ns_velocity, vehicle.velocity_ef.x);
|
||||
pack_scaled_airspeed(my_report.ew_velocity, vehicle.velocity_ef.y);
|
||||
my_report.report_structure.rs1.north_south_east_west_vel = 1;
|
||||
my_report.validity_flags |= (uint8_t)SVR_ValidityFlag::NS_and_EW_Velocity;
|
||||
|
||||
// pack in surface speed and heading:
|
||||
// pack_scaled_groundspeed(&my_report.groundspeed, vehicle.velocity_ef.length());
|
||||
// my_report.report_structure.rs1.ground_speed_on_ground = 1;
|
||||
// my_report.validity_flags |= (uint8_t)SVR_ValidityFlag::GroundSpeedWhileOnSurface;
|
||||
// my_report.report_structure.rs1.heading_on_ground = 1;
|
||||
// my_report.ground_heading = wrap_180(vehicle.velocity_ef.xy().angle()) / 1.40625;
|
||||
// my_report.validity_flags |= (uint8_t)SVR_ValidityFlag::HeadingWhileOnSurface;
|
||||
|
||||
// pack in vertical rate
|
||||
pack_scaled_vertical_rate(my_report.up_velocity, -vehicle.velocity_ef.z);
|
||||
my_report.report_structure.rs1.vertical_rate_geometric_barometric = 1;
|
||||
my_report.validity_flags |= (uint8_t)SVR_ValidityFlag::VerticalRate_Geometric;
|
||||
|
||||
StateVectorReportBuffer<sizeof(my_report)> report;
|
||||
memcpy(report.buffer, &my_report, sizeof(report.buffer));
|
||||
|
||||
const PackedMessage<StateVectorReportBuffer<sizeof(my_report)>> packed {
|
||||
report,
|
||||
MsgType::STATEVECTORREPORT,
|
||||
msgid++
|
||||
};
|
||||
write_to_autopilot((const char*)&packed, sizeof(packed));
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::send_vehicle_message_status_report(const SITL::ADSB_Vehicle &vehicle)
|
||||
{
|
||||
enum class RS0 : uint8_t {
|
||||
TimeOfApplicability = (1U << 3),
|
||||
ADSB_Version = (1U << 2),
|
||||
CallSign = (1U << 1),
|
||||
EmitterCategory = (1U << 0),
|
||||
};
|
||||
enum class RS1 : uint8_t {
|
||||
AVLengthAndWidthCode = (1U << 7),
|
||||
EmergencyPriorityStatus = (1U << 6),
|
||||
CapabilityCodes = (1U << 5),
|
||||
OperationalMode = (1U << 4),
|
||||
SVQualityNACp = (1U << 3),
|
||||
SVQualityNACv = (1U << 2),
|
||||
SVQualitySIL = (1U << 1),
|
||||
SQQualtity_GVA = (1U << 0),
|
||||
};
|
||||
enum class RS2 : uint8_t {
|
||||
SVQualityNICbaro = (1U << 7),
|
||||
TrueMagneticHeading = (1U << 6),
|
||||
VerticalRateType = (1U << 5),
|
||||
FlightModeSpecific = (1U << 4),
|
||||
Reserved3 = (1U << 3),
|
||||
Reserved2 = (1U << 2),
|
||||
Reserved1 = (1U << 1),
|
||||
Reserved0 = (1U << 0),
|
||||
};
|
||||
|
||||
// this struct-based approach may prove to be too inflexible
|
||||
// if we want to change the shape of the report at runtime for
|
||||
// some reason.
|
||||
struct PACKED {
|
||||
uint32_t report_structure : 24;
|
||||
uint8_t validity_flags; // see SVR_ValidityFlag enum
|
||||
uint8_t participant_address[3];
|
||||
uint8_t address_qualifier;
|
||||
uint8_t callsign[8];
|
||||
} my_report {};
|
||||
|
||||
my_report.report_structure = uint8_t(RS0::CallSign) | (0x2 << 4);
|
||||
my_report.validity_flags = 0;
|
||||
|
||||
pack_int32_into_uint8_ts(vehicle.ICAO_address, my_report.participant_address);
|
||||
my_report.address_qualifier = 0x02; // aircraft
|
||||
|
||||
memset(my_report.callsign, '\0', ARRAY_SIZE(my_report.callsign));
|
||||
memcpy(my_report.callsign, vehicle.callsign, MIN(ARRAY_SIZE(vehicle.callsign), ARRAY_SIZE(my_report.callsign)));
|
||||
|
||||
ModeStatusReportBuffer<sizeof(my_report)> report;
|
||||
memcpy(report.buffer, &my_report, sizeof(report.buffer));
|
||||
|
||||
const PackedMessage<ModeStatusReportBuffer<sizeof(my_report)>> packed {
|
||||
report,
|
||||
MsgType::MODESTATUSREPORT,
|
||||
msgid++
|
||||
};
|
||||
write_to_autopilot((const char*)&packed, sizeof(packed));
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::update_rf_input()
|
||||
{
|
||||
// could selectivly copy from sitl->adsb here
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::update_rf_output()
|
||||
{
|
||||
// print a message here when we are transmitting
|
||||
}
|
||||
|
||||
void ADSB_Sagetech_MXS::update(const SITL::Aircraft *sitl_model)
|
||||
{
|
||||
// see if we should do a report.
|
||||
if ((AP::sitl()->adsb_types & (1U << (uint8_t)SIM::ADSBType::SageTechMXS)) == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
update_serial_input();
|
||||
update_serial_output(sitl_model);
|
||||
|
||||
update_rf_input();
|
||||
update_rf_output();
|
||||
}
|
||||
|
||||
#endif // AP_SIM_ADSB_SAGETECH_MXS_ENABLED
|
||||
402
libraries/SITL/SIM_ADSB_Sagetech_MXS.h
Normal file
402
libraries/SITL/SIM_ADSB_Sagetech_MXS.h
Normal file
@ -0,0 +1,402 @@
|
||||
#pragma once
|
||||
|
||||
/*
|
||||
|
||||
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:sagetech_mxs --no-configure --map
|
||||
|
||||
param set SERIAL5_PROTOCOL 35 # ADSB
|
||||
param set ADSB_TYPE 4 # Sagetech MX Series
|
||||
param set SIM_ADSB_TYPES 8 # Sagetech MX Series
|
||||
param set SIM_ADSB_COUNT 5
|
||||
param set SIM_ADSB_RADIUS 2000 # same as ADSB_LIST_RADIUS
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_ADSB_SAGETECH_MXS_ENABLED
|
||||
|
||||
#include "SIM_ADSB_Device.h"
|
||||
|
||||
#include <AP_Common/Location.h>
|
||||
#include <AP_InternalError/AP_InternalError.h>
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class ADSB_Sagetech_MXS : public ADSB_Device {
|
||||
public:
|
||||
using ADSB_Device::ADSB_Device;
|
||||
|
||||
void update(const class Aircraft *sitl_model);
|
||||
|
||||
private:
|
||||
void update_serial_input();
|
||||
void update_serial_output(const Aircraft *sitl_model);
|
||||
void update_serial_output_vehicles(const Aircraft *sitl_model);
|
||||
|
||||
// pack a floating-point latitude or longitude into three bytes
|
||||
// according to Sagetech definitions:
|
||||
void pack_scaled_geocoord(uint8_t buf[3], float coord);
|
||||
void pack_scaled_alt(uint8_t buf[3], float alt_m);
|
||||
uint8_t scaled_groundspeed(float speed_m_s) const;
|
||||
void pack_scaled_groundspeed(uint8_t dest[1], float speed_m_s) const;
|
||||
void pack_scaled_airspeed(uint8_t dest[2], float speed_m_s) const;
|
||||
void pack_scaled_vertical_rate(uint8_t dest[2], float speed_m_s) const;
|
||||
// pack the three lower bytes of source into dest:
|
||||
void pack_int32_into_uint8_ts(int32_t source, uint8_t dest[3]);
|
||||
|
||||
void send_vehicle_message(const class ADSB_Vehicle &vehicle);
|
||||
void send_vehicle_message_state_vector(const class ADSB_Vehicle &vehicle);
|
||||
void send_vehicle_message_status_report(const class ADSB_Vehicle &vehicle);
|
||||
|
||||
void update_rf_output();
|
||||
void update_rf_input();
|
||||
|
||||
enum class MsgType : uint8_t {
|
||||
INSTALLATION = 0x01,
|
||||
FLIGHTID = 0x02,
|
||||
OPMSG = 0x03,
|
||||
GPS = 0x04,
|
||||
DATAREQ = 0x05,
|
||||
TARGETREQUEST = 0x0B,
|
||||
|
||||
ACK = 0x80,
|
||||
STATEVECTORREPORT = 0x91,
|
||||
MODESTATUSREPORT = 0x92,
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
class PACKED PackedMessage {
|
||||
public:
|
||||
PackedMessage(T _msg, MsgType _msgtype, uint8_t _msgid) :
|
||||
msg{_msg},
|
||||
msgtype{_msgtype},
|
||||
msgid{_msgid}
|
||||
{
|
||||
update_checksum();
|
||||
}
|
||||
uint8_t preamble { 0xAA };
|
||||
MsgType msgtype;
|
||||
uint8_t msgid;
|
||||
uint8_t payloadlen = sizeof(T);
|
||||
T msg;
|
||||
uint8_t checksum;
|
||||
|
||||
uint16_t calculate_checksum(uint16_t len) const WARN_IF_UNUSED {
|
||||
uint8_t ret = 0;
|
||||
for (uint8_t i=0; i<len; i++) {
|
||||
ret += ((const char*)this)[i];
|
||||
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
uint16_t calculate_checksum() const WARN_IF_UNUSED {
|
||||
return calculate_checksum(4+sizeof(T));
|
||||
}
|
||||
|
||||
void update_checksum() {
|
||||
checksum = calculate_checksum();
|
||||
}
|
||||
};
|
||||
|
||||
enum class OperatingMode {
|
||||
OFF,
|
||||
MAINTENANCE,
|
||||
} operating_mode;
|
||||
|
||||
// 0x01 - Installation Message
|
||||
class PACKED Installation {
|
||||
public:
|
||||
Installation()
|
||||
{ }
|
||||
uint8_t ICAO_address[3];
|
||||
uint8_t aircraft_registration[7];
|
||||
uint8_t reserved1[2];
|
||||
uint8_t com_port_0;
|
||||
uint8_t com_port_1;
|
||||
uint8_t ip_adress[4];
|
||||
uint8_t net_mask[4];
|
||||
uint8_t port_number[2];
|
||||
uint8_t gps_integrity;
|
||||
uint8_t emitter_category_set;
|
||||
uint8_t emitter_category;
|
||||
uint8_t aircraft_size;
|
||||
uint8_t max_airspeed;
|
||||
uint8_t altitude_encoder_offset[2];
|
||||
uint8_t reserved2[2];
|
||||
uint8_t install_configuration;
|
||||
uint8_t reserved3[2];
|
||||
};
|
||||
|
||||
// 0x01 - Flight ID Message
|
||||
class PACKED FlightIDMessage {
|
||||
public:
|
||||
FlightIDMessage()
|
||||
{ }
|
||||
uint8_t flight_id[8];
|
||||
uint8_t reserved[4];
|
||||
};
|
||||
|
||||
// 0x03 - Operating Message
|
||||
class PACKED Operating {
|
||||
public:
|
||||
Operating()
|
||||
{ }
|
||||
|
||||
uint16_t squawk;
|
||||
uint8_t config;
|
||||
uint8_t emergency_flag;
|
||||
uint16_t alt;
|
||||
uint16_t climb_rate;
|
||||
uint16_t heading;
|
||||
uint16_t airspeed;
|
||||
};
|
||||
|
||||
// 0x04 - GPS Navigation data message
|
||||
class PACKED GPS {
|
||||
public:
|
||||
GPS()
|
||||
{ }
|
||||
uint8_t longitude[11];
|
||||
uint8_t latitude[10];
|
||||
uint8_t speed[6];
|
||||
uint8_t ground_track[8];
|
||||
uint8_t hemisphere;
|
||||
uint8_t time_of_fix[10];
|
||||
float height;
|
||||
float hpl;
|
||||
float hfom;
|
||||
float vfom;
|
||||
uint8_t nac;
|
||||
};
|
||||
|
||||
// 0x05 - Data Request Message
|
||||
class PACKED DataRequest {
|
||||
public:
|
||||
DataRequest(uint8_t _reqtype) :
|
||||
reqtype(_reqtype)
|
||||
{ }
|
||||
uint8_t reqtype;
|
||||
uint8_t reserved[3];
|
||||
|
||||
enum class RequestDataType {
|
||||
HEALTH = 0x8D,
|
||||
};
|
||||
};
|
||||
|
||||
// 0x0B - Target Request
|
||||
class PACKED TargetRequest {
|
||||
public:
|
||||
TargetRequest()
|
||||
{ }
|
||||
uint8_t reqtype;
|
||||
uint16_t number_of_participants;
|
||||
uint8_t participant_id[3];
|
||||
uint8_t requested_reports;
|
||||
};
|
||||
|
||||
// 0x80 - Acknowledgement Message
|
||||
class PACKED Ack {
|
||||
public:
|
||||
Ack(MsgType _ackType, uint8_t _ackId, uint8_t _state, uint8_t _alt[3]) :
|
||||
ackType{_ackType},
|
||||
ackId{_ackId},
|
||||
state{_state}
|
||||
{
|
||||
memcpy(alt, _alt, sizeof(alt));
|
||||
}
|
||||
MsgType ackType;
|
||||
uint8_t ackId;
|
||||
uint8_t state;
|
||||
uint8_t alt[3];
|
||||
};
|
||||
|
||||
// 0x91 - ADSB State Vector Report. Note that this structure is
|
||||
// full-sized, assuming that the report structure field is indicating
|
||||
// all fields present. Actual parsing of this packet would
|
||||
// require looking at the report structure field.
|
||||
union PACKED StateVectorReport_ReportStructure {
|
||||
struct {
|
||||
struct PACKED {
|
||||
uint8_t time_of_applicability_for_estimated_positon : 1;
|
||||
uint8_t position_time_of_applicability : 1;
|
||||
uint8_t velocity_time_of_applicability : 1;
|
||||
uint8_t lat_and_lng : 1;
|
||||
uint8_t msn : 4;
|
||||
} rs0;
|
||||
struct PACKED {
|
||||
uint8_t estimated_latitude : 1;
|
||||
uint8_t navigation_integrity_category : 1;
|
||||
uint8_t vertical_rate_geometric_barometric : 1;
|
||||
uint8_t altimiter_barometric : 1;
|
||||
uint8_t heading_on_ground : 1;
|
||||
uint8_t ground_speed_on_ground : 1;
|
||||
uint8_t north_south_east_west_vel : 1;
|
||||
uint8_t altitude_geometric : 1;
|
||||
} rs1;
|
||||
struct PACKED {
|
||||
uint8_t reserved1 : 3;
|
||||
uint8_t report_mode : 1;
|
||||
uint8_t surveillance_status : 1;
|
||||
uint8_t estimated_east_west_velocity : 1;
|
||||
uint8_t estimated_north_south_velocity : 1;
|
||||
uint8_t estimated_longitude : 1;
|
||||
} rs2;
|
||||
};
|
||||
uint8_t raw[3];
|
||||
};
|
||||
|
||||
class PACKED StateVectorReport {
|
||||
public:
|
||||
StateVectorReport_ReportStructure report_structure;
|
||||
uint16_t validity_flags;
|
||||
uint8_t participant_address[3];
|
||||
uint8_t address_qualifier;
|
||||
uint8_t report_times_of_applicability[6];
|
||||
uint8_t latitude[3];
|
||||
uint8_t longitude[3];
|
||||
uint8_t geometric_altitude[3];
|
||||
uint16_t vel_n;
|
||||
uint16_t vel_e;
|
||||
uint8_t groundspeed_on_surface;
|
||||
uint8_t heading_on_surface;
|
||||
uint8_t barometric_alt[3];
|
||||
uint8_t vertical_rate[2];
|
||||
uint8_t NIC;
|
||||
uint8_t estimated_latitude[3];
|
||||
uint8_t estimated_longitude[3];
|
||||
uint16_t estimated_vel_n;
|
||||
uint16_t estimated_vel_e;
|
||||
uint8_t surveillance_status;
|
||||
uint8_t report_mode;
|
||||
};
|
||||
template <uint8_t SIZE>
|
||||
class PACKED StateVectorReportBuffer {
|
||||
public:
|
||||
uint8_t buffer[SIZE];
|
||||
};
|
||||
|
||||
|
||||
// 0x92 - ADSB Status Report. Note that this structure is
|
||||
// full-sized, assuming that the report structure field is indicating
|
||||
// all fields present. Actual parsing of this packet would
|
||||
// require looking at the report structure.
|
||||
class PACKED ModeStatusReport {
|
||||
public:
|
||||
uint32_t report_structure : 24;
|
||||
uint16_t validity_flags;
|
||||
uint8_t participant_address[3];
|
||||
uint8_t address_qualifier;
|
||||
uint8_t report_times_of_applicability[6];
|
||||
uint8_t callsign[8];
|
||||
uint8_t emitter_category;
|
||||
uint8_t av_length_and_width_code;
|
||||
uint8_t emergency_priority_status;
|
||||
uint32_t capability_class_codes : 24;
|
||||
uint16_t operational_mode;
|
||||
uint8_t sv_quality_nacp;
|
||||
uint8_t sv_quality_nacv;
|
||||
uint8_t sv_quality_sil_and_sda;
|
||||
uint8_t sv_quality_gva;
|
||||
uint8_t sv_quality_nic;
|
||||
uint8_t sv_quality_track_heading_and_horizontal_reference_direction;
|
||||
uint8_t vertical_rate_type;
|
||||
uint8_t reserved[2];
|
||||
};
|
||||
template <uint8_t SIZE>
|
||||
class PACKED ModeStatusReportBuffer {
|
||||
public:
|
||||
uint8_t buffer[SIZE];
|
||||
};
|
||||
|
||||
union u {
|
||||
u() {}
|
||||
char buffer[280]; // from-autopilot
|
||||
PackedMessage<Installation> packed_installation;
|
||||
PackedMessage<FlightIDMessage> packed_flightidmessage;
|
||||
PackedMessage<Operating> packed_operating;
|
||||
PackedMessage<GPS> packed_gps;
|
||||
PackedMessage<DataRequest> packed_data_req;
|
||||
PackedMessage<TargetRequest> packed_target_req;
|
||||
PackedMessage<Ack> packed_ack; // we probably don't get ACKs from the autopilot....
|
||||
PackedMessage<StateVectorReport> packed_state_vector_report;
|
||||
PackedMessage<ModeStatusReport> packed_status_report;
|
||||
|
||||
struct PACKED {
|
||||
uint8_t start_byte;
|
||||
MsgType msgtype;
|
||||
uint8_t msgid;
|
||||
uint8_t payload_length;
|
||||
} preamble;
|
||||
} msg;
|
||||
uint8_t buflen;
|
||||
|
||||
// called when there is a message in the buffer which should be
|
||||
// handled. If they return true then an ACK is sent.
|
||||
bool handle_message(const SITL::ADSB_Sagetech_MXS::Installation&);
|
||||
bool handle_message(const SITL::ADSB_Sagetech_MXS::FlightIDMessage&);
|
||||
bool handle_message(const SITL::ADSB_Sagetech_MXS::Operating&);
|
||||
bool handle_message(const SITL::ADSB_Sagetech_MXS::GPS&);
|
||||
bool handle_message(const SITL::ADSB_Sagetech_MXS::DataRequest&);
|
||||
bool handle_message(const SITL::ADSB_Sagetech_MXS::TargetRequest&);
|
||||
// bool handle_message(const SITL::ADSB_Sagetech_MXS::StateVectorReport&);
|
||||
|
||||
bool _handle_message();
|
||||
void handle_message();
|
||||
|
||||
double lon_string_to_double(const uint8_t *lat);
|
||||
double lat_string_to_double(const uint8_t *lng);
|
||||
|
||||
/*
|
||||
* Simulated device state and methods
|
||||
*/
|
||||
struct {
|
||||
bool send;
|
||||
MsgType ackType;
|
||||
uint8_t ackId;
|
||||
} ack_info;
|
||||
|
||||
uint8_t msgid;
|
||||
|
||||
struct {
|
||||
struct {
|
||||
double lat;
|
||||
double lng;
|
||||
} gps;
|
||||
} info_from_vehicle;
|
||||
|
||||
void assert_good_flight_id();
|
||||
char flight_id[8];
|
||||
uint32_t flight_id_set_time_ms;
|
||||
|
||||
/*
|
||||
serial parsing methods and data
|
||||
*/
|
||||
|
||||
// packet structure:
|
||||
// 1-byte start-byte
|
||||
// 1-byte msgtype
|
||||
// 1-byte msgid (sequence number)
|
||||
// 1-byte payloadlen
|
||||
// n-bytes payload
|
||||
// 1-byte checksum (sum of all previous bytes in packet % 256
|
||||
|
||||
void move_preamble_in_buffer(uint8_t search_start_pos=0);
|
||||
|
||||
enum class InputState {
|
||||
WANT_START_BYTE = 56,
|
||||
WANT_PREAMBLE = 57,
|
||||
WANT_PAYLOAD = 60,
|
||||
WANT_CHECKSUM = 61,
|
||||
} input_state = InputState::WANT_START_BYTE;
|
||||
void set_input_state(InputState newstate) {
|
||||
// ::fprintf(stderr, "Moving from inputstate (%u) to (%u)\n", (uint8_t)_inputstate, (uint8_t)newstate);
|
||||
input_state = newstate;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_ADSB_SAGETECH_MXS_ENABLED
|
||||
@ -141,6 +141,7 @@ public:
|
||||
void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; }
|
||||
void set_parachute(Parachute *_parachute) { parachute = _parachute; }
|
||||
void set_richenpower(RichenPower *_richenpower) { richenpower = _richenpower; }
|
||||
void set_adsb(class ADSB *_adsb) { adsb = _adsb; }
|
||||
void set_fetteconewireesc(FETtecOneWireESC *_fetteconewireesc) { fetteconewireesc = _fetteconewireesc; }
|
||||
void set_ie24(IntelligentEnergy24 *_ie24) { ie24 = _ie24; }
|
||||
void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; }
|
||||
@ -153,6 +154,8 @@ public:
|
||||
float get_battery_voltage() const { return battery_voltage; }
|
||||
float get_battery_temperature() const { return battery.get_temperature(); }
|
||||
|
||||
ADSB *adsb;
|
||||
|
||||
protected:
|
||||
SIM *sitl;
|
||||
// origin of position vector
|
||||
|
||||
@ -29,3 +29,7 @@
|
||||
#ifndef AP_SIM_TSYS03_ENABLED
|
||||
#define AP_SIM_TSYS03_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_ADSB_SAGETECH_MXS_ENABLED
|
||||
#define AP_SIM_ADSB_SAGETECH_MXS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
@ -448,6 +448,12 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
|
||||
AP_SUBGROUPINFO(airspeed[1], "ARSPD2_", 51, SIM, AirspeedParm),
|
||||
#endif
|
||||
|
||||
// @Param: ADSB_TYPES
|
||||
// @DisplayName: Simulated ADSB Type mask
|
||||
// @Description: specifies which simulated ADSB types are active
|
||||
// @User: Advanced
|
||||
// @Bitmask: 0:MAVLink,1:SageTechMXS
|
||||
AP_GROUPINFO("ADSB_TYPES", 52, SIM, adsb_types, 1),
|
||||
|
||||
#ifdef SFML_JOYSTICK
|
||||
AP_SUBGROUPEXTENSION("", 63, SIM, var_sfml_joystick),
|
||||
|
||||
@ -27,6 +27,7 @@
|
||||
#include "SIM_Ship.h"
|
||||
#include "SIM_GPS.h"
|
||||
#include "SIM_DroneCANDevice.h"
|
||||
#include "SIM_ADSB_Sagetech_MXS.h"
|
||||
|
||||
namespace SITL {
|
||||
|
||||
@ -319,6 +320,11 @@ public:
|
||||
AP_Int16 mag_delay; // magnetometer data delay in ms
|
||||
|
||||
// ADSB related run-time options
|
||||
enum class ADSBType {
|
||||
Shortcut = 0,
|
||||
SageTechMXS = 3,
|
||||
};
|
||||
AP_Enum<ADSBType> adsb_types; // bitmask of active ADSB types
|
||||
AP_Int16 adsb_plane_count;
|
||||
AP_Float adsb_radius_m;
|
||||
AP_Float adsb_altitude_m;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user