From 1d42b0281aa38ba6bb05375e347ac9a051890db9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Mar 2016 11:18:19 +1100 Subject: [PATCH] SITL: ADSB: fixed heading and fixed windows startup --- libraries/SITL/SIM_ADSB.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_ADSB.cpp b/libraries/SITL/SIM_ADSB.cpp index f70891f474..b0c07d19aa 100644 --- a/libraries/SITL/SIM_ADSB.cpp +++ b/libraries/SITL/SIM_ADSB.cpp @@ -81,6 +81,11 @@ void ADSB::update(void) */ void ADSB::send_report(void) { + 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)) { ::printf("ADSB connected to %s:%u\n", target_address, (unsigned)target_port); mavlink.connected = true; @@ -174,7 +179,7 @@ void ADSB::send_report(void) adsb_vehicle.lon = loc.lng; adsb_vehicle.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH; adsb_vehicle.altitude = -vehicle.position.z * 1000; - adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x))) / 100; + adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x))); adsb_vehicle.hor_velocity = pythagorous2(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100; adsb_vehicle.ver_velocity = -vehicle.velocity_ef.z * 100; memcpy(adsb_vehicle.callsign, vehicle.callsign, sizeof(adsb_vehicle.callsign));