Browse Source

Fixed overflowing tslc.

sbg
Timothy Scott 6 years ago committed by Beat Küng
parent
commit
7c05073044
  1. 27
      src/modules/mavlink/mavlink_receiver.cpp

27
src/modules/mavlink/mavlink_receiver.cpp

@ -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;
}

Loading…
Cancel
Save