diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1fe7c9c187..bd5f6c2ac4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2295,12 +2295,30 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) t.lon = utm_pos.lon * 1e-7; t.altitude = utm_pos.alt / 1000.0f; t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC; + // UTM_GLOBAL_POSIION uses NED (north, east, down) coordinates for velocity, in cm / s. t.heading = atan2f(vy, vx); t.hor_velocity = sqrtf(vy * vy + vx * vx); t.ver_velocity = -vz; // TODO: Callsign + // For now, set it to all 0s. This is a null-terminated string, so not explicitly giving it a null + // terminator could cause problems. + memset(&t.callsign[0], 0, sizeof(t.callsign)); t.emitter_type = ADSB_EMITTER_TYPE_NO_INFO; // TODO: Is this correct? - t.tslc = _last_utm_global_pos_com == 0 ? 0 : (t.timestamp - _last_utm_global_pos_com) / 1000000; + + // The Mavlink docs do not specify what to do if tslc (time since last communication) is out of range of + // an 8-bit int, or if this is the first communication. + // Here, I assume that if this is the first communication, tslc = 0. + // If tslc > 255, then tslc = 255. + unsigned long time_passed = (t.timestamp - _last_utm_global_pos_com) / 1000000; + + if (_last_utm_global_pos_com == 0) { + time_passed = 0; + + } else if (time_passed > UINT8_MAX) { + time_passed = UINT8_MAX; + } + + t.tslc = (uint8_t) time_passed; t.flags = 0; @@ -2320,6 +2338,13 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) // Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not // provide these. + if (_transponder_report_pub == nullptr) { + _transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH); + + } else { + orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t); + } + _last_utm_global_pos_com = t.timestamp; }