forked from Archive/PX4-Autopilot
mavlink: add support for uAvionix transmitters
This commit is contained in:
Igor Mišić
Beat Küng
parent
c35ae7260b
commit
1c5750b292
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -116,6 +116,7 @@ px4_add_module(
|
|||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
adsb
|
||||
airspeed
|
||||
component_general_json # for checksums.h
|
||||
drivers_accelerometer
|
||||
|
|
|
|||
|
|
@ -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 <mavlink.h>
|
||||
#if !MAVLINK_FTP_UNIT_TEST
|
||||
#include <uAvionix.h>
|
||||
#endif
|
||||
|
||||
__END_DECLS
|
||||
|
||||
|
|
|
|||
|
|
@ -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, "<interface_name>", "wifi/ethernet interface name", true);
|
||||
#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)
|
||||
|
|
|
|||
|
|
@ -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";
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<MavlinkStreamEfiStatus>(),
|
||||
#endif // EFI_STATUS_HPP
|
||||
#if defined(GPS_RTCM_DATA_HPP)
|
||||
create_stream_list_item<MavlinkStreamGPSRTCMData>()
|
||||
create_stream_list_item<MavlinkStreamGPSRTCMData>(),
|
||||
#endif // GPS_RTCM_DATA_HPP
|
||||
#if defined(UAVIONIX_ADSB_OUT_CFG_HPP)
|
||||
create_stream_list_item<MavlinkStreamUavionixADSBOutCfg>(),
|
||||
#endif // UAVIONIX_ADSB_OUT_CFG_HPP
|
||||
#if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP)
|
||||
create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>()
|
||||
#endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
|
||||
};
|
||||
|
||||
const char *get_stream_name(const uint16_t msg_id)
|
||||
|
|
|
|||
|
|
@ -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]
|
||||
|
|
|
|||
|
|
@ -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 <uORB/topics/parameter_update.h>
|
||||
|
||||
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<uint16_t>(fw_airspd_stall * 100.0f); // convert [m/s] to [cm/s]
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::ADSB_ICAO_ID>) _adsb_icao,
|
||||
(ParamInt<px4::params::ADSB_LEN_WIDTH>) _adsb_len_width,
|
||||
(ParamInt<px4::params::ADSB_EMIT_TYPE>) _adsb_emit_type,
|
||||
(ParamInt<px4::params::ADSB_GPS_OFF_LAT>) _adsb_gps_offset_lat,
|
||||
(ParamInt<px4::params::ADSB_GPS_OFF_LON>) _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<uint32_t>(_adsb_icao.get()),
|
||||
.stallSpeed = _stall_speed,
|
||||
.callsign = {'\0'},
|
||||
.emitterType = static_cast<uint8_t>(_adsb_emit_type.get()),
|
||||
.aircraftSize = static_cast<uint8_t>(_adsb_len_width.get()),
|
||||
.gpsOffsetLat = static_cast<uint8_t>(_adsb_gps_offset_lat.get()),
|
||||
.gpsOffsetLon = static_cast<uint8_t>(_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
|
||||
|
|
@ -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 <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
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<px4::params::ADSB_SQUAWK>) _adsb_squawk,
|
||||
(ParamInt<px4::params::ADSB_EMERGC>) _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<uint32_t>(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<int32_t>(vehicle_air_data.baro_pressure_pa / 100.0f), // convert [Pa] to [mBar]
|
||||
.accuracyHor = static_cast<uint32_t>(vehicle_gps_position.eph * 1000.0f), // convert [m] to [mm]
|
||||
.accuracyVert = static_cast<uint16_t>(vehicle_gps_position.epv * 100.0f), // convert [m] to [cm]
|
||||
.accuracyVel = static_cast<uint16_t>(vehicle_gps_position.s_variance_m_s * 1000.f), // convert [m/s] to [mm/s],
|
||||
.velVert = static_cast<int16_t>(-1.0f * vehicle_gps_position.vel_d_m_s * 100.0f), // convert [m/s] to [cm/s]
|
||||
.velNS = static_cast<int16_t>(vehicle_gps_position.vel_n_m_s * 100.0f), // convert [m/s] to [cm/s]
|
||||
.VelEW = static_cast<int16_t>(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<uint16_t>(_adsb_squawk.get()),
|
||||
.gpsFix = vehicle_gps_position.fix_type,
|
||||
.numSats = vehicle_gps_position.satellites_used,
|
||||
.emergencyStatus = static_cast<uint8_t>(_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
|
||||
Loading…
Reference in New Issue