|
|
|
@ -2295,12 +2295,30 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg)
@@ -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)
@@ -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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|