fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 18:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink app: Read MAVLink transponder report

Browse Source
This commit is contained in:
Lorenz Meier
2016-04-17 17:57:00 +02:00
parent 1da25db617
commit 2c2a87cea1
2 changed files with 41 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+38
View File
@@ -128,6 +128,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_land_detector_pub(nullptr),
_time_offset_pub(nullptr),
_follow_target_pub(nullptr),
_transponder_report_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@@ -228,9 +229,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
handle_message_distance_sensor(msg);
break;
case MAVLINK_MSG_ID_FOLLOW_TARGET:
handle_message_follow_target(msg);
break;
case MAVLINK_MSG_ID_ADSB_VEHICLE:
handle_message_adsb_vehicle(msg);
break;
default:
break;
}
@@ -1646,6 +1652,38 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
}
}
void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
{
mavlink_adsb_vehicle_t adsb;
transponder_report_s t = { };
mavlink_msg_adsb_vehicle_decode(msg, &adsb);
t.timestamp = hrt_absolute_time();
t.ICAO_address = adsb.ICAO_address;
t.lat = adsb.lat*1e-7;
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.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));
t.emitter_type = adsb.emitter_type;
t.tslc = adsb.tslc;
t.flags = adsb.flags;
t.squawk = adsb.squawk;
//warnx("code: %d callsign: %s, vel: %8.4f", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity);
if (_transponder_report_pub == nullptr) {
_transponder_report_pub = orb_advertise(ORB_ID(transponder_report), &t);
} else {
orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t);
}
}
void
MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
{