diff --git a/src/lib/adsb/parameters.c b/src/lib/adsb/parameters.c index 2ac2e97843..6bf8865cc5 100644 --- a/src/lib/adsb/parameters.c +++ b/src/lib/adsb/parameters.c @@ -192,3 +192,37 @@ PARAM_DEFINE_INT32(ADSB_ICAO_SPECL, 0); * @group ADSB */ PARAM_DEFINE_INT32(ADSB_EMERGC, 0); + +/** + * ADSB-Out GPS Offset lat + * + * Sets GPS lataral offset encoding + * + * @reboot_required false + * @min 0 + * @max 7 + * @value 0 NoData + * @value 1 LatLeft2M + * @value 2 LatLeft4M + * @value 3 LatLeft6M + * @value 4 LatRight0M + * @value 5 LatRight2M + * @value 6 LatRight4M + * @value 7 LatRight6M + * @group ADSB + */ +PARAM_DEFINE_INT32(ADSB_GPS_OFF_LAT, 0); + +/** + * ADSB-Out GPS Offset lon + * + * Sets GPS longitudinal offset encoding + * + * @reboot_required false + * @min 0 + * @max 1 + * @value 0 NoData + * @value 1 AppliedBySensor + * @group ADSB + */ +PARAM_DEFINE_INT32(ADSB_GPS_OFF_LON, 0); diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 28bf3af54d..a3bf27ece3 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -116,6 +116,7 @@ px4_add_module( MODULE_CONFIG module.yaml DEPENDS + adsb airspeed component_general_json # for checksums.h drivers_accelerometer diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 6d3a0152a4..3e2866ae6c 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -93,6 +93,9 @@ extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); #include +#if !MAVLINK_FTP_UNIT_TEST +#include +#endif __END_DECLS diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 83d51437f7..1c57f34cdf 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1833,6 +1833,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) #endif // !CONSTRAINED_FLASH break; + case MAVLINK_MODE_UAVIONIX: + configure_stream_local("UAVIONIX_ADSB_OUT_CFG", 0.1f); + configure_stream_local("UAVIONIX_ADSB_OUT_DYNAMIC", 5.0f); + break; + default: ret = -1; break; @@ -2046,6 +2051,9 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "onboard_low_bandwidth") == 0) { _mode = MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH; + } else if (strcmp(myoptarg, "uavionix") == 0) { + _mode = MAVLINK_MODE_UAVIONIX; + } else { PX4_ERR("invalid mode"); err_flag = true; @@ -3307,7 +3315,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 PRINT_MODULE_USAGE_PARAM_INT('o', 14550, 0, 65536, "Select UDP Network Port (remote)", true); PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr, "Partner IP (broadcasting can be enabled via -p flag)", true); #endif - PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal", + PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal|uavionix", "Mode: sets default streams and rates", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "", "wifi/ethernet interface name", true); #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 23c2fd5e4b..a3938f3408 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -210,6 +210,7 @@ public: MAVLINK_MODE_EXTVISIONMIN, MAVLINK_MODE_GIMBAL, MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH, + MAVLINK_MODE_UAVIONIX, MAVLINK_MODE_COUNT }; @@ -264,6 +265,9 @@ public: case MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH: return "OnboardLowBandwidth"; + case MAVLINK_MODE_UAVIONIX: + return "uAvionix"; + default: return "Unknown"; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d0128c6280..0b80bb064e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -139,6 +139,8 @@ # include "streams/SCALED_PRESSURE2.hpp" # include "streams/SCALED_PRESSURE3.hpp" # include "streams/SMART_BATTERY_INFO.hpp" +# include "streams/UAVIONIX_ADSB_OUT_CFG.hpp" +# include "streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp" # include "streams/UTM_GLOBAL_POSITION.hpp" #endif // !CONSTRAINED_FLASH @@ -474,8 +476,14 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), #endif // EFI_STATUS_HPP #if defined(GPS_RTCM_DATA_HPP) - create_stream_list_item() + create_stream_list_item(), #endif // GPS_RTCM_DATA_HPP +#if defined(UAVIONIX_ADSB_OUT_CFG_HPP) + create_stream_list_item(), +#endif // UAVIONIX_ADSB_OUT_CFG_HPP +#if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP) + create_stream_list_item() +#endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP }; const char *get_stream_name(const uint16_t msg_id) diff --git a/src/modules/mavlink/module.yaml b/src/modules/mavlink/module.yaml index 73a2c828c6..6bfe8b23a5 100644 --- a/src/modules/mavlink/module.yaml +++ b/src/modules/mavlink/module.yaml @@ -68,6 +68,7 @@ parameters: #9: External Vision Minimal # hidden 10: Gimbal 11: Onboard Low Bandwidth + 12: uAvionix reboot_required: true num_instances: *max_num_config_instances default: [0, 2, 0] diff --git a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp new file mode 100644 index 0000000000..a8767b76d2 --- /dev/null +++ b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef UAVIONIX_ADSB_OUT_CFG_HPP +#define UAVIONIX_ADSB_OUT_CFG_HPP + +#include + +class MavlinkStreamUavionixADSBOutCfg : public ModuleParams, public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamUavionixADSBOutCfg(mavlink); } + + static constexpr const char *get_name_static() { return "UAVIONIX_ADSB_OUT_CFG"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + bool const_rate() override { return true; } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamUavionixADSBOutCfg(Mavlink *mavlink) : ModuleParams(nullptr), MavlinkStream(mavlink) + { + param_t param_fw_airspd_stall = param_find("FW_AIRSPD_STALL"); + + if (param_fw_airspd_stall != PARAM_INVALID) { + float fw_airspd_stall; + + if (param_get(param_fw_airspd_stall, &fw_airspd_stall) == PX4_OK) { + _stall_speed = static_cast(fw_airspd_stall * 100.0f); // convert [m/s] to [cm/s] + } + } + } + + DEFINE_PARAMETERS( + (ParamInt) _adsb_icao, + (ParamInt) _adsb_len_width, + (ParamInt) _adsb_emit_type, + (ParamInt) _adsb_gps_offset_lat, + (ParamInt) _adsb_gps_offset_lon + ); + + uint16_t _stall_speed{0}; // [cm/s] + + bool send() override + { + // Required update for static message is 0.1 [Hz] + mavlink_uavionix_adsb_out_cfg_t cfg_msg = { + .ICAO = static_cast(_adsb_icao.get()), + .stallSpeed = _stall_speed, + .callsign = {'\0'}, + .emitterType = static_cast(_adsb_emit_type.get()), + .aircraftSize = static_cast(_adsb_len_width.get()), + .gpsOffsetLat = static_cast(_adsb_gps_offset_lat.get()), + .gpsOffsetLon = static_cast(_adsb_gps_offset_lon.get()), + .rfSelect = UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED + }; + + //memcpy(cfg_msg.callsign, "PX4 UAV ", sizeof(cfg_msg.callsign)); //TODO: add support for callsign + + mavlink_msg_uavionix_adsb_out_cfg_send_struct(_mavlink->get_channel(), &cfg_msg); + + return true; + } +}; + +#endif // UAVIONIX_ADSB_OUT_CFG_HPP diff --git a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp new file mode 100644 index 0000000000..fdc8ae1ecc --- /dev/null +++ b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef UAVIONIX_ADSB_OUT_DYNAMIC_HPP +#define UAVIONIX_ADSB_OUT_DYNAMIC_HPP + +#include +#include +#include +#include + +class MavlinkStreamUavionixADSBOutDynamic : public ModuleParams, public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamUavionixADSBOutDynamic(mavlink); } + + static constexpr const char *get_name_static() { return "UAVIONIX_ADSB_OUT_DYNAMIC"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + bool const_rate() override { return true; } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamUavionixADSBOutDynamic(Mavlink *mavlink) : ModuleParams(nullptr), MavlinkStream(mavlink) {} + + + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + DEFINE_PARAMETERS( + (ParamInt) _adsb_squawk, + (ParamInt) _adsb_emergc + ); + + + bool send() override + { + + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + + sensor_gps_s vehicle_gps_position; + _vehicle_gps_position_sub.copy(&vehicle_gps_position); + + vehicle_air_data_s vehicle_air_data; + _vehicle_air_data_sub.copy(&vehicle_air_data); + + // Required update for dynamic message is 5 [Hz] + mavlink_uavionix_adsb_out_dynamic_t dynamic_msg = { + .utcTime = static_cast(vehicle_gps_position.time_utc_usec / 1000000ULL), + .gpsLat = vehicle_gps_position.lat, + .gpsLon = vehicle_gps_position.lon, + .gpsAlt = vehicle_gps_position.alt_ellipsoid, + .baroAltMSL = static_cast(vehicle_air_data.baro_pressure_pa / 100.0f), // convert [Pa] to [mBar] + .accuracyHor = static_cast(vehicle_gps_position.eph * 1000.0f), // convert [m] to [mm] + .accuracyVert = static_cast(vehicle_gps_position.epv * 100.0f), // convert [m] to [cm] + .accuracyVel = static_cast(vehicle_gps_position.s_variance_m_s * 1000.f), // convert [m/s] to [mm/s], + .velVert = static_cast(-1.0f * vehicle_gps_position.vel_d_m_s * 100.0f), // convert [m/s] to [cm/s] + .velNS = static_cast(vehicle_gps_position.vel_n_m_s * 100.0f), // convert [m/s] to [cm/s] + .VelEW = static_cast(vehicle_gps_position.vel_e_m_s * 100.0f), // convert [m/s] to [cm/s] + .state = UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND, + .squawk = static_cast(_adsb_squawk.get()), + .gpsFix = vehicle_gps_position.fix_type, + .numSats = vehicle_gps_position.satellites_used, + .emergencyStatus = static_cast(_adsb_emergc.get()) + }; + + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + dynamic_msg.state |= ~UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND; + } + + mavlink_msg_uavionix_adsb_out_dynamic_send_struct(_mavlink->get_channel(), &dynamic_msg); + + return true; + } +}; + +#endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP