diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f5cbbd20df..1fe7c9c187 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -202,6 +202,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_adsb_vehicle(msg); break; + case MAVLINK_MSG_ID_UTM_GLOBAL_POSITION: + handle_message_utm_global_position(msg); + break; + case MAVLINK_MSG_ID_COLLISION: handle_message_collision(msg); break; @@ -2272,6 +2276,53 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) +{ + mavlink_utm_global_position_t utm_pos{}; + transponder_report_s t{}; + + mavlink_msg_utm_global_position_decode(msg, &utm_pos); + + // Convert cm/s to m/s + float vx = utm_pos.vx / 100.0f; + float vy = utm_pos.vy / 100.0f; + float vz = utm_pos.vz / 100.0f; + + t.timestamp = hrt_absolute_time(); + // TODO: ID + t.lat = utm_pos.lat * 1e-7; + t.lon = utm_pos.lon * 1e-7; + t.altitude = utm_pos.alt / 1000.0f; + t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC; + t.heading = atan2f(vy, vx); + t.hor_velocity = sqrtf(vy * vy + vx * vx); + t.ver_velocity = -vz; + // TODO: 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; + + t.flags = 0; + + if (utm_pos.flags | UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE) { + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; + } + + if (utm_pos.flags | UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE) { + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; + } + + if (utm_pos.flags | UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE) { + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; + } + + // Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not + // provide these. + + _last_utm_global_pos_com = t.timestamp; +} + void MavlinkReceiver::handle_message_collision(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ff59c41b92..23982cd590 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -119,6 +119,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_adsb_vehicle(mavlink_message_t *msg); + void handle_message_utm_global_position(mavlink_message_t *msg); void handle_message_att_pos_mocap(mavlink_message_t *msg); void handle_message_battery_status(mavlink_message_t *msg); void handle_message_collision(mavlink_message_t *msg); @@ -273,6 +274,8 @@ private: uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; + hrt_abstime _last_utm_global_pos_com{0}; + DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr,