From bf51478dd14487e6912a34c80e3020d8950a1289 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 29 Dec 2020 09:42:14 +1100 Subject: [PATCH] AP_GPS: added ExternalAHRS backend --- libraries/AP_GPS/AP_GPS.cpp | 23 +++++- libraries/AP_GPS/AP_GPS.h | 6 ++ libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp | 98 ++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_ExternalAHRS.h | 46 +++++++++++ libraries/AP_GPS/GPS_Backend.h | 5 +- 5 files changed, 175 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp create mode 100644 libraries/AP_GPS/AP_GPS_ExternalAHRS.h diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 1108a0bb8a..ee1f74b717 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -36,6 +36,7 @@ #include "AP_GPS_UBLOX.h" #include "AP_GPS_MAV.h" #include "AP_GPS_MSP.h" +#include "AP_GPS_ExternalAHRS.h" #include "GPS_Backend.h" #if HAL_ENABLE_LIBUAVCAN_DRIVERS @@ -80,7 +81,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,19:MSP,20:AllyStar + // @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,20:AllyStar,21:ExternalAHRS // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), @@ -89,7 +90,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,19:MSP,20:AllyStar + // @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,20:AllyStar,21:ExternalAHRS // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0), @@ -376,6 +377,7 @@ bool AP_GPS::needs_uart(GPS_Type type) const case GPS_TYPE_UAVCAN: case GPS_TYPE_MAV: case GPS_TYPE_MSP: + case GPS_TYPE_EXTERNAL_AHRS: return false; default: break; @@ -556,6 +558,12 @@ void AP_GPS::detect_instance(uint8_t instance) dstate->auto_detected_baud = false; // specified, not detected new_gps = new AP_GPS_MSP(*this, state[instance], nullptr); goto found_gps; +#endif +#if HAL_EXTERNAL_AHRS_ENABLED + case GPS_TYPE_EXTERNAL_AHRS: + dstate->auto_detected_baud = false; // specified, not detected + new_gps = new AP_GPS_ExternalAHRS(*this, state[instance], nullptr); + goto found_gps; #endif default: break; @@ -1086,6 +1094,17 @@ void AP_GPS::handle_msp(const MSP::msp_gps_data_message_t &pkt) } #endif // HAL_MSP_GPS_ENABLED +#if HAL_EXTERNAL_AHRS_ENABLED +void AP_GPS::handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt) +{ + for (uint8_t i=0; ihandle_external(pkt); + } + } +} +#endif // HAL_EXTERNAL_AHRS_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 94842301bd..d3e5c682c0 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -22,6 +22,7 @@ #include "GPS_detect_state.h" #include #include +#include /** maximum number of GPS instances available on this platform. If more @@ -65,6 +66,7 @@ class AP_GPS friend class AP_GPS_GSOF; friend class AP_GPS_MAV; friend class AP_GPS_MSP; + friend class AP_GPS_ExternalAHRS; friend class AP_GPS_MTK; friend class AP_GPS_MTK19; friend class AP_GPS_NMEA; @@ -115,6 +117,7 @@ public: GPS_TYPE_UBLOX_RTK_ROVER = 18, GPS_TYPE_MSP = 19, GPS_TYPE_ALLYSTAR = 20, // AllyStar NMEA + GPS_TYPE_EXTERNAL_AHRS = 21, }; /// GPS status codes @@ -213,6 +216,9 @@ public: #if HAL_MSP_GPS_ENABLED void handle_msp(const MSP::msp_gps_data_message_t &pkt); #endif +#if HAL_EXTERNAL_AHRS_ENABLED + void handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt); +#endif // Accessor functions diff --git a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp new file mode 100644 index 0000000000..928eb1942c --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp @@ -0,0 +1,98 @@ +/* + 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 . + */ + +// +// ExternalAHRS GPS driver +// +#include +#include "AP_GPS_ExternalAHRS.h" + +#if HAL_EXTERNAL_AHRS_ENABLED + +AP_GPS_ExternalAHRS::AP_GPS_ExternalAHRS(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_ExternalAHRS::read(void) +{ + if (new_data) { + new_data = false; + return true; + } + return false; +} + +// handles an incoming ExternalAHRS message and sets +// corresponding gps data appropriately; +void AP_GPS_ExternalAHRS::handle_external(const AP_ExternalAHRS::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; + if (pkt.fix_type == 0) { + state.status = AP_GPS::NO_FIX; + } else { + 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 = pkt.vdop; + + state.have_vertical_velocity = true; + state.velocity.x = pkt.ned_vel_north; + state.velocity.y = pkt.ned_vel_east; + state.velocity.z = pkt.ned_vel_down; + + 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; + state.vertical_accuracy = pkt.vertical_pos_accuracy; + state.speed_accuracy = pkt.horizontal_vel_accuracy; + + state.last_gps_time_ms = AP_HAL::millis(); + + new_data = true; +} + +/* + return velocity lag in seconds + */ +bool AP_GPS_ExternalAHRS::get_lag(float &lag_sec) const +{ + // fixed assumed lag + lag_sec = 0.11; + return true; +} + +#endif // HAL_EXTERNAL_AHRS_ENABLED + diff --git a/libraries/AP_GPS/AP_GPS_ExternalAHRS.h b/libraries/AP_GPS/AP_GPS_ExternalAHRS.h new file mode 100644 index 0000000000..52352a726d --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_ExternalAHRS.h @@ -0,0 +1,46 @@ +/* + 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 . + */ + +// +// ExternalAHRS GPS driver which accepts gps position data from an external AHRS unit +// +#pragma once + +#include +#include + +#include "AP_GPS.h" +#include "GPS_Backend.h" + +#if HAL_EXTERNAL_AHRS_ENABLED + +class AP_GPS_ExternalAHRS : public AP_GPS_Backend +{ +public: + AP_GPS_ExternalAHRS(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); + + bool read() override; + void handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt) override; + + const char *name() const override { return "ExternalAHRS"; } + + bool get_lag(float &lag_sec) const override; + +private: + bool new_data; +}; + +#endif // HAL_EXTERNAL_AHRS_ENABLED + diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index bae8e5f0d6..9244d9e76e 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -54,7 +54,10 @@ public: #if HAL_MSP_GPS_ENABLED virtual void handle_msp(const MSP::msp_gps_data_message_t &pkt) { return; } #endif - +#if HAL_EXTERNAL_AHRS_ENABLED + virtual void handle_external(const AP_ExternalAHRS::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; }