diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp index b11f13f237..546bbd2bf7 100644 --- a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp @@ -614,7 +614,7 @@ void SagetechMXS::handle_svr(sg_svr_t svr) } if (svr.validity.surfHeading) { - t.heading = matrix::wrap_pi((float)svr.surface.heading * (M_PI_F / 180.0f) + M_PI_F); + t.heading = matrix::wrap_pi((float)svr.surface.heading * (M_PI_F / 180.0f)); t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; } } @@ -622,7 +622,7 @@ void SagetechMXS::handle_svr(sg_svr_t svr) if (svr.type == svrAirborne) { if (svr.validity.airSpeed) { t.hor_velocity = (svr.airborne.speed * SAGETECH_SCALE_KNOTS_TO_M_PER_SEC); //Convert from knots to meters/second - t.heading = matrix::wrap_pi((float)svr.airborne.heading * (M_PI_F / 180.0f) + M_PI_F); + t.heading = matrix::wrap_pi((float)svr.airborne.heading * (M_PI_F / 180.0f)); t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; } diff --git a/src/lib/adsb/AdsbConflict.cpp b/src/lib/adsb/AdsbConflict.cpp index 22f393cd30..08d13e12a4 100644 --- a/src/lib/adsb/AdsbConflict.cpp +++ b/src/lib/adsb/AdsbConflict.cpp @@ -180,7 +180,7 @@ bool AdsbConflict::handle_traffic_conflict() case TRAFFIC_STATE::ADD_CONFLICT: case TRAFFIC_STATE::REMIND_CONFLICT: { - take_action = send_traffic_warning((int)(math::degrees(_transponder_report.heading) + 180.f), + take_action = send_traffic_warning((int)math::degrees(_transponder_report.heading), (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, _transponder_report.callsign, _transponder_report.icao_address, diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cf683eaa10..11a47d07b8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2548,7 +2548,7 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) t.lon = adsb.lon * 1e-7; t.altitude_type = adsb.altitude_type; t.altitude = adsb.altitude / 1000.0f; - t.heading = adsb.heading / 100.0f / 180.0f * M_PI_F - M_PI_F; + t.heading = adsb.heading / 100.0f / 180.0f * M_PI_F; t.hor_velocity = adsb.hor_velocity / 100.0f; t.ver_velocity = adsb.ver_velocity / 100.0f; memcpy(&t.callsign[0], &adsb.callsign[0], sizeof(t.callsign)); diff --git a/src/modules/mavlink/streams/ADSB_VEHICLE.hpp b/src/modules/mavlink/streams/ADSB_VEHICLE.hpp index 56f2a94e42..fe61f412b7 100644 --- a/src/modules/mavlink/streams/ADSB_VEHICLE.hpp +++ b/src/modules/mavlink/streams/ADSB_VEHICLE.hpp @@ -77,7 +77,7 @@ private: msg.lon = pos.lon * 1e7; msg.altitude_type = pos.altitude_type; msg.altitude = pos.altitude * 1e3f; - msg.heading = (pos.heading + M_PI_F) / M_PI_F * 180.0f * 100.0f; + msg.heading = pos.heading / M_PI_F * 180.0f * 100.0f; msg.hor_velocity = pos.hor_velocity * 100.0f; msg.ver_velocity = pos.ver_velocity * 100.0f; memcpy(&msg.callsign[0], &pos.callsign[0], sizeof(msg.callsign));