mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-06 16:49:51 +08:00
Fixed overflowing tslc.
This commit is contained in:
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user