2015-11-18 21:20:01 -04:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
/*
|
|
|
|
ADSB simulator class for MAVLink ADSB peripheral
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "SIM_ADSB.h"
|
2016-06-15 00:47:19 -03:00
|
|
|
#include "SITL.h"
|
2015-11-18 21:20:01 -04:00
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
|
|
|
|
#include "SIM_Aircraft.h"
|
2019-08-15 03:53:35 -03:00
|
|
|
#include <AP_HAL_SITL/SITL_State.h>
|
2015-11-18 21:20:01 -04:00
|
|
|
|
|
|
|
namespace SITL {
|
|
|
|
|
2016-06-15 00:47:19 -03:00
|
|
|
SITL *_sitl;
|
|
|
|
|
2015-11-18 21:20:01 -04:00
|
|
|
/*
|
|
|
|
update a simulated vehicle
|
|
|
|
*/
|
|
|
|
void ADSB_Vehicle::update(float delta_t)
|
|
|
|
{
|
|
|
|
if (!initialised) {
|
|
|
|
initialised = true;
|
|
|
|
ICAO_address = (uint32_t)(rand() % 10000);
|
|
|
|
snprintf(callsign, sizeof(callsign), "SIM%u", ICAO_address);
|
2016-06-15 00:47:19 -03:00
|
|
|
position.x = Aircraft::rand_normal(0, _sitl->adsb_radius_m);
|
|
|
|
position.y = Aircraft::rand_normal(0, _sitl->adsb_radius_m);
|
|
|
|
position.z = -fabsf(_sitl->adsb_altitude_m);
|
|
|
|
|
|
|
|
double vel_min = 5, vel_max = 20;
|
|
|
|
if (position.length() > 500) {
|
|
|
|
vel_min *= 3;
|
|
|
|
vel_max *= 3;
|
|
|
|
} else if (position.length() > 10000) {
|
|
|
|
vel_min *= 10;
|
|
|
|
vel_max *= 10;
|
|
|
|
}
|
|
|
|
velocity_ef.x = Aircraft::rand_normal(vel_min, vel_max);
|
|
|
|
velocity_ef.y = Aircraft::rand_normal(vel_min, vel_max);
|
2015-11-18 21:20:01 -04:00
|
|
|
velocity_ef.z = Aircraft::rand_normal(0, 3);
|
|
|
|
}
|
|
|
|
|
|
|
|
position += velocity_ef * delta_t;
|
|
|
|
if (position.z > 0) {
|
|
|
|
// it has crashed! reset
|
|
|
|
initialised = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
update the ADSB peripheral state
|
|
|
|
*/
|
|
|
|
void ADSB::update(void)
|
|
|
|
{
|
2016-06-15 00:47:19 -03:00
|
|
|
if (_sitl == nullptr) {
|
2018-06-11 14:41:13 -03:00
|
|
|
_sitl = AP::sitl();
|
2016-06-15 00:47:19 -03:00
|
|
|
return;
|
|
|
|
} else if (_sitl->adsb_plane_count <= 0) {
|
|
|
|
return;
|
|
|
|
} else if (_sitl->adsb_plane_count >= num_vehicles_MAX) {
|
|
|
|
_sitl->adsb_plane_count.set_and_save(0);
|
|
|
|
num_vehicles = 0;
|
|
|
|
return;
|
|
|
|
} else if (num_vehicles != _sitl->adsb_plane_count) {
|
|
|
|
num_vehicles = _sitl->adsb_plane_count;
|
|
|
|
for (uint8_t i=0; i<num_vehicles_MAX; i++) {
|
|
|
|
vehicles[i].initialised = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-11-18 21:20:01 -04:00
|
|
|
// calculate delta time in seconds
|
|
|
|
uint32_t now_us = AP_HAL::micros();
|
|
|
|
|
|
|
|
float delta_t = (now_us - last_update_us) * 1.0e-6f;
|
|
|
|
last_update_us = now_us;
|
|
|
|
|
|
|
|
for (uint8_t i=0; i<num_vehicles; i++) {
|
|
|
|
vehicles[i].update(delta_t);
|
|
|
|
}
|
|
|
|
|
|
|
|
// see if we should do a report
|
|
|
|
send_report();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
send a report to the vehicle control code over MAVLink
|
|
|
|
*/
|
|
|
|
void ADSB::send_report(void)
|
|
|
|
{
|
2016-03-17 21:18:19 -03:00
|
|
|
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;
|
|
|
|
}
|
2015-11-18 21:20:01 -04:00
|
|
|
if (!mavlink.connected && mav_socket.connect(target_address, target_port)) {
|
|
|
|
::printf("ADSB connected to %s:%u\n", target_address, (unsigned)target_port);
|
|
|
|
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) {
|
|
|
|
for (uint8_t i=0; i<ret; i++) {
|
|
|
|
mavlink_message_t msg;
|
|
|
|
mavlink_status_t status;
|
|
|
|
if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status,
|
|
|
|
buf[i],
|
|
|
|
&msg, &status) == MAVLINK_FRAMING_OK) {
|
|
|
|
switch (msg.msgid) {
|
|
|
|
case MAVLINK_MSG_ID_HEARTBEAT: {
|
|
|
|
if (!seen_heartbeat) {
|
|
|
|
seen_heartbeat = true;
|
|
|
|
vehicle_component_id = msg.compid;
|
|
|
|
vehicle_system_id = msg.sysid;
|
|
|
|
::printf("ADSB using srcSystem %u\n", (unsigned)vehicle_system_id);
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!seen_heartbeat) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
uint32_t now = AP_HAL::millis();
|
|
|
|
mavlink_message_t msg;
|
|
|
|
uint16_t len;
|
|
|
|
|
|
|
|
if (now - last_heartbeat_ms >= 1000) {
|
|
|
|
mavlink_heartbeat_t heartbeat;
|
|
|
|
heartbeat.type = MAV_TYPE_ADSB;
|
|
|
|
heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA;
|
|
|
|
heartbeat.base_mode = 0;
|
|
|
|
heartbeat.system_status = 0;
|
|
|
|
heartbeat.mavlink_version = 0;
|
|
|
|
heartbeat.custom_mode = 0;
|
|
|
|
|
|
|
|
/*
|
|
|
|
save and restore sequence number for chan0, as it is used by
|
|
|
|
generated encode functions
|
|
|
|
*/
|
|
|
|
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
|
|
|
|
uint8_t saved_seq = chan0_status->current_tx_seq;
|
|
|
|
chan0_status->current_tx_seq = mavlink.seq;
|
|
|
|
len = mavlink_msg_heartbeat_encode(vehicle_system_id,
|
|
|
|
vehicle_component_id,
|
|
|
|
&msg, &heartbeat);
|
|
|
|
chan0_status->current_tx_seq = saved_seq;
|
|
|
|
|
|
|
|
mav_socket.send(&msg.magic, len);
|
|
|
|
|
|
|
|
last_heartbeat_ms = now;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
send a ADSB_VEHICLE messages
|
|
|
|
*/
|
|
|
|
uint32_t now_us = AP_HAL::micros();
|
2016-06-15 00:50:32 -03:00
|
|
|
if (now_us - last_report_us >= reporting_period_ms*1000UL) {
|
2015-11-18 21:20:01 -04:00
|
|
|
for (uint8_t i=0; i<num_vehicles; i++) {
|
|
|
|
ADSB_Vehicle &vehicle = vehicles[i];
|
|
|
|
Location loc = home;
|
|
|
|
|
2019-02-24 20:16:28 -04:00
|
|
|
loc.offset(vehicle.position.x, vehicle.position.y);
|
2015-11-18 21:20:01 -04:00
|
|
|
|
2016-06-15 00:47:19 -03:00
|
|
|
// re-init when exceeding radius range
|
2019-02-24 20:16:28 -04:00
|
|
|
if (home.get_distance(loc) > _sitl->adsb_radius_m) {
|
2015-11-18 21:20:01 -04:00
|
|
|
vehicle.initialised = false;
|
|
|
|
}
|
|
|
|
|
|
|
|
mavlink_adsb_vehicle_t adsb_vehicle {};
|
|
|
|
last_report_us = now_us;
|
|
|
|
|
|
|
|
adsb_vehicle.ICAO_address = vehicle.ICAO_address;
|
2015-11-25 19:25:18 -04:00
|
|
|
adsb_vehicle.lat = loc.lat;
|
|
|
|
adsb_vehicle.lon = loc.lng;
|
2015-11-18 21:20:01 -04:00
|
|
|
adsb_vehicle.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;
|
2015-12-08 17:39:33 -04:00
|
|
|
adsb_vehicle.altitude = -vehicle.position.z * 1000;
|
2016-03-17 21:18:19 -03:00
|
|
|
adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x)));
|
2016-04-16 06:58:46 -03:00
|
|
|
adsb_vehicle.hor_velocity = norm(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100;
|
2015-12-08 17:39:33 -04:00
|
|
|
adsb_vehicle.ver_velocity = -vehicle.velocity_ef.z * 100;
|
2015-11-18 21:20:01 -04:00
|
|
|
memcpy(adsb_vehicle.callsign, vehicle.callsign, sizeof(adsb_vehicle.callsign));
|
|
|
|
adsb_vehicle.emitter_type = ADSB_EMITTER_TYPE_LARGE;
|
|
|
|
adsb_vehicle.tslc = 1;
|
|
|
|
adsb_vehicle.flags =
|
|
|
|
ADSB_FLAGS_VALID_COORDS |
|
|
|
|
ADSB_FLAGS_VALID_ALTITUDE |
|
|
|
|
ADSB_FLAGS_VALID_HEADING |
|
|
|
|
ADSB_FLAGS_VALID_VELOCITY |
|
|
|
|
ADSB_FLAGS_VALID_CALLSIGN |
|
|
|
|
ADSB_FLAGS_SIMULATED;
|
2015-12-08 17:39:33 -04:00
|
|
|
adsb_vehicle.squawk = 0; // NOTE: ADSB_FLAGS_VALID_SQUAWK bit is not set
|
2015-11-18 21:20:01 -04:00
|
|
|
|
|
|
|
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
|
|
|
|
uint8_t saved_seq = chan0_status->current_tx_seq;
|
|
|
|
chan0_status->current_tx_seq = mavlink.seq;
|
|
|
|
len = mavlink_msg_adsb_vehicle_encode(vehicle_system_id,
|
|
|
|
MAV_COMP_ID_ADSB,
|
|
|
|
&msg, &adsb_vehicle);
|
|
|
|
chan0_status->current_tx_seq = saved_seq;
|
|
|
|
|
2016-06-15 21:15:07 -03:00
|
|
|
uint8_t msgbuf[len];
|
|
|
|
len = mavlink_msg_to_send_buffer(msgbuf, &msg);
|
|
|
|
if (len > 0) {
|
|
|
|
mav_socket.send(msgbuf, len);
|
|
|
|
}
|
2015-11-18 21:20:01 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-08-16 18:16:03 -03:00
|
|
|
// ADSB_transceiever is enabled, send the status report.
|
|
|
|
if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) {
|
|
|
|
last_tx_report_ms = now;
|
|
|
|
|
|
|
|
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
|
|
|
|
uint8_t saved_seq = chan0_status->current_tx_seq;
|
2016-08-16 19:29:35 -03:00
|
|
|
uint8_t saved_flags = chan0_status->flags;
|
|
|
|
chan0_status->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
|
2016-08-16 18:16:03 -03:00
|
|
|
chan0_status->current_tx_seq = mavlink.seq;
|
|
|
|
const mavlink_uavionix_adsb_transceiver_health_report_t health_report = {UAVIONIX_ADSB_RF_HEALTH_OK};
|
|
|
|
len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode(vehicle_system_id,
|
|
|
|
MAV_COMP_ID_ADSB,
|
|
|
|
&msg, &health_report);
|
|
|
|
chan0_status->current_tx_seq = saved_seq;
|
2016-08-16 19:29:35 -03:00
|
|
|
chan0_status->flags = saved_flags;
|
2016-08-16 18:16:03 -03:00
|
|
|
|
|
|
|
uint8_t msgbuf[len];
|
|
|
|
len = mavlink_msg_to_send_buffer(msgbuf, &msg);
|
|
|
|
if (len > 0) {
|
|
|
|
mav_socket.send(msgbuf, len);
|
2016-08-16 19:29:35 -03:00
|
|
|
::printf("ADSBsim send tx health packet\n");
|
2016-08-16 18:16:03 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-11-18 21:20:01 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
} // namespace SITL
|