From 047531b924fc430944d3ccba837f8a1b45a059f8 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Fri, 2 Oct 2020 10:00:06 -0600 Subject: [PATCH] Add MavlinkStreamGPSStatus status class stream via GPS_STATUS.hpp and add PRN code to satellite_info.msg. --- msg/satellite_info.msg | 3 +- src/drivers/gps/definitions.h | 2 +- src/drivers/gps/gps.cpp | 6 +- src/modules/mavlink/mavlink_main.cpp | 3 + src/modules/mavlink/mavlink_messages.cpp | 5 +- src/modules/mavlink/mavlink_messages.h | 2 + src/modules/mavlink/streams/GPS_STATUS.hpp | 97 ++++++++++++++++++++++ 7 files changed, 112 insertions(+), 6 deletions(-) create mode 100644 src/modules/mavlink/streams/GPS_STATUS.hpp diff --git a/msg/satellite_info.msg b/msg/satellite_info.msg index 2192bd3e32..2980ffcbdc 100644 --- a/msg/satellite_info.msg +++ b/msg/satellite_info.msg @@ -1,9 +1,10 @@ uint64 timestamp # time since system start (microseconds) uint8 SAT_INFO_MAX_SATELLITES = 20 -uint8 count # Number of satellites in satellite info +uint8 count # Number of satellites visible to the receiver uint8[20] svid # Space vehicle ID [1..255], see scheme below uint8[20] used # 0: Satellite not used, 1: used for navigation uint8[20] elevation # Elevation (0: right on top of receiver, 90: on the horizon) of satellite uint8[20] azimuth # Direction of satellite, 0: 0 deg, 255: 360 deg. uint8[20] snr # dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. +uint8[20] prn # Satellite PRN code assignment, (psuedorandom number SBAS, valid codes are 120-144) diff --git a/src/drivers/gps/definitions.h b/src/drivers/gps/definitions.h index 51dc284eda..fee9bfb6f2 100644 --- a/src/drivers/gps/definitions.h +++ b/src/drivers/gps/definitions.h @@ -41,8 +41,8 @@ #include #include -#include #include +#include #define GPS_INFO(...) PX4_INFO(__VA_ARGS__) #define GPS_WARN(...) PX4_WARN(__VA_ARGS__) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 8d893dbf21..fc3368d8eb 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -83,7 +83,7 @@ typedef enum { /* struct for dynamic allocation of satellite info data */ struct GPS_Sat_Info { - struct satellite_info_s _data; + satellite_info_s _data; }; static constexpr int TASK_STACK_SIZE = 1760; @@ -152,7 +152,7 @@ private: char _port[20] {}; ///< device / serial port path bool _healthy{false}; ///< flag to signal if the GPS is ok - bool _mode_auto; ///< if true, auto-detect which GPS is attached + bool _mode_auto; ///< if true, auto-detect which GPS is attached gps_driver_mode_t _mode; ///< current mode @@ -169,7 +169,7 @@ private: float _rate{0.0f}; ///< position update rate float _rate_rtcm_injection{0.0f}; ///< RTCM message injection rate - unsigned _last_rate_rtcm_injection_count{0}; ///< counter for number of RTCM messages + unsigned _last_rate_rtcm_injection_count{0}; ///< counter for number of RTCM messages const bool _fake_gps; ///< fake gps output diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c4f2578a7c..4c8821c169 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1582,6 +1582,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GLOBAL_POSITION_INT", 5.0f); configure_stream_local("GPS2_RAW", 1.0f); configure_stream_local("GPS_RAW_INT", 1.0f); + configure_stream_local("GPS_STATUS", 1.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("LOCAL_POSITION_NED", 1.0f); configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); @@ -1632,6 +1633,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GLOBAL_POSITION_INT", 50.0f); configure_stream_local("GPS2_RAW", unlimited_rate); configure_stream_local("GPS_RAW_INT", unlimited_rate); + configure_stream_local("GPS_STATUS", 1.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("NAMED_VALUE_FLOAT", 10.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); @@ -1754,6 +1756,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GLOBAL_POSITION_INT", 10.0f); configure_stream_local("GPS2_RAW", unlimited_rate); configure_stream_local("GPS_RAW_INT", unlimited_rate); + configure_stream_local("GPS_STATUS", 1.0f); configure_stream_local("HIGHRES_IMU", 50.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("MANUAL_CONTROL", 5.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d18b681507..f8bcb6f36a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -118,6 +118,7 @@ using matrix::wrap_2pi; #include "streams/ESC_STATUS.hpp" #include "streams/EXTENDED_SYS_STATE.hpp" #include "streams/FLIGHT_INFORMATION.hpp" +#include "streams/GPS_STATUS.hpp" #include "streams/HIGH_LATENCY2.hpp" #include "streams/HIL_STATE_QUATERNION.hpp" #include "streams/OBSTACLE_DISTANCE.hpp" @@ -1761,7 +1762,6 @@ protected: } }; - class MavlinkStreamGPSRawInt : public MavlinkStream { public: @@ -5021,6 +5021,9 @@ static const StreamListItem streams_list[] = { #if defined(FLIGHT_INFORMATION_HPP) create_stream_list_item(), #endif // FLIGHT_INFORMATION_HPP +#if defined(GPS_STATUS_HPP) + create_stream_list_item(), +#endif // GPS_STATUS_HPP #if defined(STORAGE_INFORMATION_HPP) create_stream_list_item(), #endif // STORAGE_INFORMATION_HPP diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 153b529848..eed7b675d6 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -67,7 +67,9 @@ static StreamListItem create_stream_list_item() } const char *get_stream_name(const uint16_t msg_id); + MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink); + MavlinkStream *create_mavlink_stream(const uint16_t msg_id, Mavlink *mavlink); void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, uint8_t *mavlink_base_mode, diff --git a/src/modules/mavlink/streams/GPS_STATUS.hpp b/src/modules/mavlink/streams/GPS_STATUS.hpp new file mode 100644 index 0000000000..f6700f066d --- /dev/null +++ b/src/modules/mavlink/streams/GPS_STATUS.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 GPS_STATUS_HPP +#define GPS_STATUS_HPP + +#include + +class MavlinkStreamGPSStatus : public MavlinkStream +{ +public: + const char *get_name() const override { return MavlinkStreamGPSStatus::get_name_static(); } + + static constexpr const char *get_name_static() { return "GPS_STATUS"; } + + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS_STATUS; } + + uint16_t get_id() override { return get_id_static(); } + + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSStatus(mavlink); } + + unsigned get_size() override + { + return _satellite_info_sub.advertised() ? MAVLINK_MSG_ID_GPS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + uORB::Subscription _satellite_info_sub{ORB_ID(satellite_info)}; + + // Disallow copy and move construction. + MavlinkStreamGPSStatus(MavlinkStreamGPSStatus &) = delete; + MavlinkStreamGPSStatus &operator = (const MavlinkStreamGPSStatus &) = delete; + +protected: + explicit MavlinkStreamGPSStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send() override + { + satellite_info_s sat {}; + + if (_satellite_info_sub.update(&sat)) { + mavlink_gps_status_t msg{}; + + msg.satellites_visible = sat.count; + + size_t sat_count = math::min(static_cast(sat.count), + sizeof(msg.satellite_used) / sizeof(msg.satellite_used[0])); + + for (size_t i = 0; i < sat_count; i++) { + msg.satellite_used[i] = sat.used[i]; + msg.satellite_elevation[i] = sat.elevation[i]; + msg.satellite_azimuth[i] = sat.azimuth[i]; + msg.satellite_snr[i] = sat.snr[i]; + msg.satellite_prn[i] = sat.prn[i]; + } + + mavlink_msg_gps_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // GPS_STATUS_HPP