aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: make SITL::ADSB a SITL::SerialDevice

Browse Source
This commit is contained in:
Peter Barker 2021-11-09 17:23:35 +11:00 committed by Andrew Tridgell
parent cdccc67fb8
commit 494dcc6ba3
2 changed files with 23 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/SITL/SIM_ADSB.cpp
View File

@ -29,14 +29,17 @@
namespace SITL {
SIM *_sitl;
/*
update a simulated vehicle
*/
void ADSB_Vehicle::update(float delta_t)
{
if (!initialised) {
const SIM *_sitl = AP::sitl();
if (_sitl == nullptr) {
return;
}
initialised = true;
ICAO_address = (uint32_t)(rand() % 10000);
snprintf(callsign, sizeof(callsign), "SIM%u", ICAO_address);
@ -80,7 +83,7 @@ void ADSB_Vehicle::update(float delta_t)
/*
update the ADSB peripheral state
*/
void ADSB::update(void)
void ADSB::update(const class Aircraft &aircraft)
{
if (_sitl == nullptr) {
_sitl = AP::sitl();
@ -109,32 +112,25 @@ void ADSB::update(void)
}
// see if we should do a report
send_report();
send_report(aircraft);
}
/*
send a report to the vehicle control code over MAVLink
*/
void ADSB::send_report(void)
void ADSB::send_report(const class Aircraft &aircraft)
{
if (AP_HAL::millis() < 10000) {
// simulated aircraft don't appear until 10s after startup. This avoids a windows
// threading issue with non-blocking sockets and the initial wait on uartA
return;
}
if (!mavlink.connected && mav_socket.connect(target_address, target_port_base + 10 * instance)) {
::printf("ADSB connected to %s:%u\n", target_address, (unsigned)target_port_base + 10 * instance);
mavlink.connected = true;
}
if (!mavlink.connected) {
return;
}
// check for incoming MAVLink messages
uint8_t buf[100];
ssize_t ret;
while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) {
while ((ret=read_from_autopilot((char*)buf, sizeof(buf))) > 0) {
for (uint8_t i=0; i<ret; i++) {
mavlink_message_t msg;
mavlink_status_t status;
@ -185,7 +181,7 @@ void ADSB::send_report(void)
&msg, &heartbeat);
chan0_status->current_tx_seq = saved_seq;
mav_socket.send(&msg.magic, len);
write_to_autopilot((char*)&msg.magic, len);
last_heartbeat_ms = now;
}
@ -194,6 +190,8 @@ void ADSB::send_report(void)
/*
send a ADSB_VEHICLE messages
*/
const Location &home = aircraft.get_home();
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++) {
@ -246,7 +244,7 @@ void ADSB::send_report(void)
uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg);
if (len > 0) {
mav_socket.send(msgbuf, len);
write_to_autopilot((char*)msgbuf, len);
}
}
}
@ -270,7 +268,7 @@ void ADSB::send_report(void)
uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg);
if (len > 0) {
mav_socket.send(msgbuf, len);
write_to_autopilot((char*)msgbuf, len);
::printf("ADSBsim send tx health packet\n");
}
}

26
libraries/SITL/SIM_ADSB.h
View File

@ -50,43 +50,35 @@ private:
uint64_t stationary_object_created_ms; // allows expiring of slow/stationary objects
};
class ADSB {
class ADSB : public SerialDevice {
public:
ADSB(const struct sitl_fdm &_fdm, const Location& _home, const uint8_t _instance) : home(_home), instance(_instance) {};
void update(void);
ADSB() {};
void update(const class Aircraft &aircraft);
private:
const char *target_address = "127.0.0.1";
const uint16_t target_port_base = 5762;
const uint8_t instance = 0;
const Location& home;
uint8_t num_vehicles = 0;
uint8_t num_vehicles;
static const uint8_t num_vehicles_MAX = 200;
ADSB_Vehicle vehicles[num_vehicles_MAX];
// reporting period in ms
const float reporting_period_ms = 1000;
uint32_t last_report_us = 0;
uint32_t last_update_us = 0;
uint32_t last_tx_report_ms = 0;
uint32_t last_report_us;
uint32_t last_update_us;
uint32_t last_tx_report_ms;
uint32_t last_heartbeat_ms = 0;
uint32_t last_heartbeat_ms;
bool seen_heartbeat = false;
uint8_t vehicle_system_id;
uint8_t vehicle_component_id;
SocketAPM mav_socket { false };
struct {
// socket to telem2 on aircraft
bool connected;
mavlink_message_t rxmsg;
mavlink_status_t status;
uint8_t seq;
} mavlink {};
void send_report(void);
void send_report(const SITL::Aircraft&);
};
} // namespace SITL