diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1435317734..05052bfdcd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1836,6 +1836,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VISION_POSITION_NED", 10.0f); configure_stream("NAMED_VALUE_FLOAT", 1.0f); configure_stream("ESTIMATOR_STATUS", 0.5f); + configure_stream("ADSB_VEHICLE", 2.0f); break; case MAVLINK_MODE_ONBOARD: @@ -1866,6 +1867,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VISION_POSITION_NED", 10.0f); configure_stream("NAMED_VALUE_FLOAT", 10.0f); configure_stream("ESTIMATOR_STATUS", 1.0f); + configure_stream("ADSB_VEHICLE", 10.0f); break; case MAVLINK_MODE_OSD: @@ -1916,6 +1918,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VISION_POSITION_NED", 10.0f); configure_stream("NAMED_VALUE_FLOAT", 50.0f); configure_stream("ESTIMATOR_STATUS", 5.0f); + configure_stream("ADSB_VEHICLE", 20.0f); default: break; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9c666f6703..d4c5b5dff8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -75,6 +75,7 @@ #include #include #include +#include #include #include #include @@ -1113,6 +1114,73 @@ protected: } }; +class MavlinkStreamADSBVehicle : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamADSBVehicle::get_name_static(); + } + + static const char *get_name_static() + { + return "ADSB_VEHICLE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ADSB_VEHICLE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamADSBVehicle(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamADSBVehicle(MavlinkStreamADSBVehicle &); + MavlinkStreamADSBVehicle& operator = (const MavlinkStreamADSBVehicle &); + +protected: + explicit MavlinkStreamADSBVehicle(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(transponder_report))), + _pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct transponder_report_s pos; + + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_adsb_vehicle_t msg = {}; + + msg.ICAO_address = pos.ICAO_address; + msg.lat = pos.lat * 1e7; + 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.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)); + msg.emitter_type = pos.emitter_type; + msg.tslc = pos.tslc; + msg.flags = pos.flags; + msg.squawk = pos.squawk; + + _mavlink->send_message(MAVLINK_MSG_ID_ADSB_VEHICLE, &msg); + } + } +}; class MavlinkStreamCameraTrigger : public MavlinkStream { @@ -2855,5 +2923,6 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static), new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static), + new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static), nullptr };