fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sagetech_mxs: Adding fixes for crashes due to ADSB vehicle list initialization failure

Browse Source 
Co-authored-by: cfaber <chuck.faber@sagetech.com>
This commit is contained in:
Chuck 2022-07-06 18:19:46 -07:00 committed by GitHub
parent 44f98ac355
commit 4528341069
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 17 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp
View File

@ -325,14 +325,14 @@ void SagetechMXS::Run()
ret = read(_fd, &data, 1);
if (ret < 0) {
PX4_ERR("Read Err.");
// PX4_ERR("Read Err.");
perf_count(_comms_errors);
continue;
}
// PX4_INFO("GOT BYTE: %02x", (uint8_t)data);
bytes_available -= ret;
parse_byte((uint8_t)data);
bytes_available -= 1;
}
}
@ -455,7 +455,8 @@ void SagetechMXS::determine_furthest_aircraft()
continue;
}
const float distance = get_distance_to_next_waypoint(_gps.lat, _gps.lon, vehicle_list[index].lat,
const float distance = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE,
vehicle_list[index].lat,
vehicle_list[index].lon);
if ((max_distance < distance) || (index == 0)) {
@ -492,7 +493,8 @@ void SagetechMXS::handle_vehicle(const transponder_report_s &vehicle)
// and which to keep, allocating new vehicles, and publishing to the transponder_report topic
uint16_t index = list_size_allocated + 1; // Make invalid to start with.
const bool my_loc_is_zero = (_gps.lat == 0) && (_gps.lon == 0);
const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.lat, _gps.lon, vehicle.lat, vehicle.lon);
const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE,
vehicle.lat, vehicle.lon);
const bool is_tracked_in_list = find_index(vehicle, &index);
// const bool is_special = is_special_vehicle(vehicle.icao_address);
const uint16_t required_flags_position = transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
@ -546,10 +548,10 @@ void SagetechMXS::handle_vehicle(const transponder_report_s &vehicle)
void SagetechMXS::handle_ack(const sg_ack_t ack)
{
if ((ack.ackId != last.msg.id) || (ack.ackType != last.msg.type)) {
// The message id doesn't match the last message sent.
PX4_ERR("Message Id %d of type %d not Acked by MXS", last.msg.id, last.msg.type);
}
// if ((ack.ackId != last.msg.id) || (ack.ackType != last.msg.type)) {
// // The message id doesn't match the last message sent.
// PX4_ERR("Message Id %d of type %d not Acked by MXS", last.msg.id, last.msg.type);
// }
// System health
if (ack.failXpdr && !last.failXpdr) {
@ -820,10 +822,7 @@ void SagetechMXS::send_gps_msg()
snprintf((char *)&gps.grdSpeed, 7, "%03u.%02u", (unsigned)speed_knots,
unsigned((speed_knots - (int)speed_knots) * (float)1.0E2));
// const float heading = math::degrees(matrix::wrap_2pi(_gps.cog_rad));
const float heading = matrix::wrap_2pi(_gps.cog_rad) * (180.0f / M_PI_F);
// PX4_INFO("CoG: %f. Heading: %f", (double) _gps.cog_rad, (double) _gps.heading);
// PX4_INFO("Heading: %f", (double)heading);
snprintf((char *)&gps.grdTrack, 9, "%03u.%04u", unsigned(heading), unsigned((heading - (int)heading) * (float)1.0E4));
@ -836,7 +835,6 @@ void SagetechMXS::send_gps_msg()
struct tm *tm = gmtime(&time_sec);
snprintf((char *)&gps.timeOfFix, 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min,
tm->tm_sec + (_gps.time_utc_usec % 1000000) * 1.0e-6);
// PX4_INFO("send_gps_msg: ToF %s, Longitude %s, Latitude %s, Grd Speed %s, Grd Track %s", gps.timeOfFix, gps.longitude, gps.latitude, gps.grdSpeed, gps.grdTrack);
gps.height = _gps.alt_ellipsoid * 1E-3;
@ -997,12 +995,14 @@ bool SagetechMXS::parse_byte(const uint8_t data)
handle_packet(_message_in.packet);
} else if (data == SG_MSG_START_BYTE) {
PX4_INFO("ERROR: Byte Lost. Catching new packet.");
// PX4_INFO("ERROR: Byte Lost. Catching new packet.");
perf_count(_comms_errors);
_message_in.state = ParseState::WaitingFor_MsgType;
_message_in.checksum = data;
} else {
PX4_INFO("ERROR: Checksum Mismatch. Expected %02x. Received %02x.", _message_in.checksum, data);
// PX4_INFO("ERROR: Checksum Mismatch. Expected %02x. Received %02x.", _message_in.checksum, data);
perf_count(_comms_errors);
}
break;

5
src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp
View File

@ -156,6 +156,7 @@ private:
static constexpr uint16_t INVALID_SQUAWK{7777};
static constexpr unsigned BAUD_460800{0010004}; // B460800 not defined in MacOS termios
static constexpr unsigned BAUD_921600{0010007}; // B921600 not defined in MacOS termios
static constexpr double GPS_SCALE{1.0E-7};
// Stored variables
uint64_t _loop_count;
@ -238,7 +239,7 @@ private:
} last;
// External Vehicle List
transponder_report_s *vehicle_list;
transponder_report_s *vehicle_list = nullptr;
uint16_t list_size_allocated;
uint16_t vehicle_count = 0;
uint16_t furthest_vehicle_index = 0;
@ -256,7 +257,7 @@ private:
bool find_index(const transponder_report_s &vehicle, uint16_t *index) const;
void set_vehicle(const uint16_t index, const transponder_report_s &vehicle);
void delete_vehicle(const uint16_t index);
bool is_special_vehicle(uint32_t icao) const {return _adsb_icao_specl.get() != 0 && (_adsb_icao_specl.get() == (int32_t) icao);}
bool is_special_vehicle(uint32_t icao) const {return (_adsb_icao_specl.get() != 0) && (_adsb_icao_specl.get() == (int32_t) icao);}
void handle_vehicle(const transponder_report_s &vehicle);
void determine_furthest_aircraft();
void send_data_req(const sg_datatype_t dataReqType);