Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 16:49:51 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Added message handler for UTM_GLOBAL_POSITION

Browse Source
This commit is contained in:
Timothy Scott
2019-07-10 11:04:15 +02:00
committed by Beat Küng
parent efad34e55e
commit e25ad348e8
2 changed files with 54 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+51
View File
@@ -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)
{
src/modules/mavlink/mavlink_receiver.h
+3
View File
@@ -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<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,