diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp index d355098235..cd0f5b11e5 100644 --- a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp @@ -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; diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp index 0c19d33f62..f63661b629 100644 --- a/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp @@ -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);