Skip to content

Commit

Permalink
Fixed overflowing tslc.
Browse files Browse the repository at this point in the history
  • Loading branch information
Timothy Scott authored and bkueng committed Jul 16, 2019
1 parent e25ad34 commit 7c05073
Showing 1 changed file with 26 additions and 1 deletion.
27 changes: 26 additions & 1 deletion src/modules/mavlink/mavlink_receiver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

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

Expand Down

0 comments on commit 7c05073

Please sign in to comment.