fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/modules/mavlink/mavlink_receiver.cpp
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)
{

3
src/modules/mavlink/mavlink_receiver.h
View File

@ -75,6 +75,7 @@
#include <uORB/topics/time_offset.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/follow_target.h>
#include <uORB/topics/transponder_report.h>
#include "mavlink_ftp.h"
@ -138,6 +139,7 @@ private:
void handle_message_hil_state_quaternion(mavlink_message_t *msg);
void handle_message_distance_sensor(mavlink_message_t *msg);
void handle_message_follow_target(mavlink_message_t *msg);
void handle_message_adsb_vehicle(mavlink_message_t *msg);
void *receive_thread(void *arg);
@ -200,6 +202,7 @@ private:
orb_advert_t _land_detector_pub;
orb_advert_t _time_offset_pub;
orb_advert_t _follow_target_pub;
orb_advert_t _transponder_report_pub;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;