From 85b61d6bf3c3e5d60e29b9f9ad26a9ccfd5dcc69 Mon Sep 17 00:00:00 2001 From: yaapu Date: Thu, 3 Sep 2020 11:15:35 +0200 Subject: [PATCH] AP_GPS: added support for MSP GPS sensors --- libraries/AP_GPS/AP_GPS.cpp | 23 ++++++- libraries/AP_GPS/AP_GPS.h | 10 ++++ libraries/AP_GPS/AP_GPS_MSP.cpp | 100 +++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_MSP.h | 45 ++++++++++++++ libraries/AP_GPS/GPS_Backend.cpp | 7 ++- libraries/AP_GPS/GPS_Backend.h | 3 + 6 files changed, 185 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_GPS/AP_GPS_MSP.cpp create mode 100644 libraries/AP_GPS/AP_GPS_MSP.h diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index a3115d3475..87e23441ff 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -35,6 +35,7 @@ #include "AP_GPS_SIRF.h" #include "AP_GPS_UBLOX.h" #include "AP_GPS_MAV.h" +#include "AP_GPS_MSP.h" #include "GPS_Backend.h" #if HAL_ENABLE_LIBUAVCAN_DRIVERS @@ -73,7 +74,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: TYPE // @DisplayName: GPS type // @Description: GPS type - // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover + // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), @@ -82,7 +83,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: TYPE2 // @DisplayName: 2nd GPS type // @Description: GPS type of 2nd GPS - // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover + // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0), @@ -344,6 +345,7 @@ bool AP_GPS::needs_uart(GPS_Type type) const case GPS_TYPE_HIL: case GPS_TYPE_UAVCAN: case GPS_TYPE_MAV: + case GPS_TYPE_MSP: return false; default: break; @@ -515,6 +517,12 @@ void AP_GPS::detect_instance(uint8_t instance) goto found_gps; #endif return; // We don't do anything here if UAVCAN is not supported +#if HAL_MSP_GPS_ENABLED + case GPS_TYPE_MSP: + dstate->auto_detected_baud = false; // specified, not detected + new_gps = new AP_GPS_MSP(*this, state[instance], nullptr); + goto found_gps; +#endif default: break; } @@ -1020,6 +1028,17 @@ void AP_GPS::handle_msg(const mavlink_message_t &msg) } } +#if HAL_MSP_GPS_ENABLED +void AP_GPS::handle_msp(const MSP::msp_gps_data_message_t &pkt) +{ + for (uint8_t i=0; ihandle_msp(pkt); + } + } +} +#endif // HAL_MSP_GPS_ENABLED + /* set HIL (hardware in the loop) status for a GPS instance */ diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 76619682ad..39bee5460e 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -21,6 +21,7 @@ #include #include "GPS_detect_state.h" #include +#include /** maximum number of GPS instances available on this platform. If more @@ -46,6 +47,10 @@ #define GPS_UBLOX_MOVING_BASELINE !HAL_MINIMIZE_FEATURES && GPS_MAX_RECEIVERS>1 #endif +#ifndef HAL_MSP_GPS_ENABLED +#define HAL_MSP_GPS_ENABLED HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES +#endif + class AP_GPS_Backend; /// @class AP_GPS @@ -55,6 +60,7 @@ class AP_GPS friend class AP_GPS_ERB; friend class AP_GPS_GSOF; friend class AP_GPS_MAV; + friend class AP_GPS_MSP; friend class AP_GPS_MTK; friend class AP_GPS_MTK19; friend class AP_GPS_NMEA; @@ -103,6 +109,7 @@ public: GPS_TYPE_HEMI = 16, // hemisphere NMEA GPS_TYPE_UBLOX_RTK_BASE = 17, GPS_TYPE_UBLOX_RTK_ROVER = 18, + GPS_TYPE_MSP = 19, }; /// GPS status codes @@ -198,6 +205,9 @@ public: // Pass mavlink data to message handlers (for MAV type) void handle_msg(const mavlink_message_t &msg); +#if HAL_MSP_GPS_ENABLED + void handle_msp(const MSP::msp_gps_data_message_t &pkt); +#endif // Accessor functions diff --git a/libraries/AP_GPS/AP_GPS_MSP.cpp b/libraries/AP_GPS/AP_GPS_MSP.cpp new file mode 100644 index 0000000000..7500778dfa --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_MSP.cpp @@ -0,0 +1,100 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +// +// MSP GPS driver +// +#include +#include "AP_GPS_MSP.h" + +#if HAL_MSP_GPS_ENABLED + +AP_GPS_MSP::AP_GPS_MSP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : + AP_GPS_Backend(_gps, _state, _port) +{ +} + +// Reading does nothing in this class; we simply return whether or not +// the latest reading has been consumed. By calling this function we assume +// the caller is consuming the new data; +bool AP_GPS_MSP::read(void) +{ + if (new_data) { + new_data = false; + return true; + } + return false; +} + +// handles an incoming msp message and sets +// corresponding gps data appropriately; +void AP_GPS_MSP::handle_msp(const MSP::msp_gps_data_message_t &pkt) +{ + check_new_itow(pkt.ms_tow, sizeof(pkt)); + + state.time_week = pkt.gps_week; + state.time_week_ms = pkt.ms_tow; + state.status = (AP_GPS::GPS_Status)pkt.fix_type; + state.num_sats = pkt.satellites_in_view; + + Location loc = {}; + loc.lat = pkt.latitude; + loc.lng = pkt.longitude; + loc.alt = pkt.msl_altitude; + + state.location = loc; + state.hdop = pkt.hdop; + state.vdop = GPS_UNKNOWN_DOP; + + state.have_vertical_velocity = true; + Vector3f vel; + vel.x = pkt.ned_vel_north * 0.01; + vel.y = pkt.ned_vel_east * 0.01; + vel.z = pkt.ned_vel_down * 0.01; + state.velocity = vel; + + state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); + state.ground_speed = norm(state.velocity.y, state.velocity.x); + + state.have_speed_accuracy = true; + state.have_horizontal_accuracy = true; + state.have_vertical_accuracy = true; + state.have_vertical_velocity = true; + + state.horizontal_accuracy = pkt.horizontal_pos_accuracy * 0.01; + state.vertical_accuracy = pkt.vertical_pos_accuracy * 0.01; + state.speed_accuracy = pkt.horizontal_vel_accuracy * 0.01; + + if (pkt.true_yaw != 65535) { + state.gps_yaw = wrap_360(pkt.true_yaw*0.01); + state.have_gps_yaw = true; + } + + state.last_gps_time_ms = AP_HAL::millis(); + + new_data = pkt.fix_type>0; +} + +/* + return velocity lag in seconds + */ +bool AP_GPS_MSP::get_lag(float &lag_sec) const +{ + // measured on Matek M8Q + lag_sec = 0.11; + return true; +} + +#endif // HAL_MSP_GPS_ENABLED diff --git a/libraries/AP_GPS/AP_GPS_MSP.h b/libraries/AP_GPS/AP_GPS_MSP.h new file mode 100644 index 0000000000..aaf3db6eec --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_MSP.h @@ -0,0 +1,45 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +// +// MSP GPS driver which accepts gps position data from an external source +// +#pragma once + +#include +#include + +#include "AP_GPS.h" +#include "GPS_Backend.h" + +#if HAL_MSP_GPS_ENABLED + +class AP_GPS_MSP : public AP_GPS_Backend +{ +public: + AP_GPS_MSP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); + + bool read() override; + void handle_msp(const MSP::msp_gps_data_message_t &pkt) override; + + const char *name() const override { return "MSP"; } + + bool get_lag(float &lag_sec) const override; + +private: + bool new_data; +}; + +#endif // HAL_MSP_GPS_ENABLED diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index a60f83e710..6169ce69c2 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -252,7 +252,12 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length) const uint32_t gps_min_period_ms = 50; // get the time the packet arrived on the UART - uint64_t uart_us = port->receive_time_constraint_us(msg_length); + uint64_t uart_us; + if (port) { + uart_us = port->receive_time_constraint_us(msg_length); + } else { + uart_us = AP_HAL::micros64(); + } uint32_t now = AP_HAL::millis(); uint32_t dt_ms = now - _last_ms; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 0548859e61..a9303b122a 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -51,6 +51,9 @@ public: virtual void broadcast_configuration_failure_reason(void) const { return ; } virtual void handle_msg(const mavlink_message_t &msg) { return ; } +#if HAL_MSP_GPS_ENABLED + virtual void handle_msp(const MSP::msp_gps_data_message_t &pkt) { return; } +#endif // driver specific lag, returns true if the driver is confident in the provided lag virtual bool get_lag(float &lag) const { lag = 0.2f; return true; }