From 4249f7dbe69bab0f2bbd50ecba146f4046ae3bc4 Mon Sep 17 00:00:00 2001 From: Allan Matthew Date: Thu, 19 May 2016 16:24:08 -0700 Subject: [PATCH] AP_GPS: Add GPS_MAV type and accept GPS_INPUT message --- libraries/AP_GPS/AP_GPS.cpp | 21 +++++++ libraries/AP_GPS/AP_GPS.h | 4 ++ libraries/AP_GPS/AP_GPS_MAV.cpp | 108 ++++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_MAV.h | 41 ++++++++++++ libraries/AP_GPS/GPS_Backend.h | 2 + 5 files changed, 176 insertions(+) create mode 100644 libraries/AP_GPS/AP_GPS_MAV.cpp create mode 100644 libraries/AP_GPS/AP_GPS_MAV.h diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 5a61053460..0689a14b5d 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -32,6 +32,7 @@ #include "AP_GPS_SBP.h" #include "AP_GPS_SIRF.h" #include "AP_GPS_UBLOX.h" +#include "AP_GPS_MAV.h" #include "GPS_Backend.h" extern const AP_HAL::HAL &hal; @@ -312,6 +313,12 @@ AP_GPS::detect_instance(uint8_t instance) AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) { _broadcast_gps_type("ERB", instance, dstate->last_baud); new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]); + } + // user has to explicitly set the MAV type, do not use AUTO + // Do not try to detect the MAV type, assume its there + else if (_type[instance] == GPS_TYPE_MAV) { + _broadcast_gps_type("MAV", instance, dstate->last_baud); + new_gps = new AP_GPS_MAV(*this, state[instance], NULL); } else if (now - dstate->detect_started_ms > (ARRAY_SIZE(_baudrates) * GPS_BAUD_TIME_MS)) { // prevent false detection of NMEA mode in @@ -461,6 +468,20 @@ AP_GPS::update(void) AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats; } +/* + pass along a mavlink message (for MAV type) + */ +void +AP_GPS::handle_msg(mavlink_message_t *msg) +{ + uint8_t i; + for (i=0; ihandle_msg(msg); + } + } +} + /* 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 5357efc06f..86aa839866 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -69,6 +69,7 @@ public: GPS_TYPE_GSOF = 11, GPS_TYPE_QURT = 12, GPS_TYPE_ERB = 13, + GPS_TYPE_MAV = 14, }; /// GPS status codes @@ -127,6 +128,9 @@ public: uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds }; + // Pass mavlink data to message handlers (for MAV type) + void handle_msg(mavlink_message_t *msg); + // Accessor functions // return number of active GPS sensors. Note that if the first GPS diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp new file mode 100644 index 0000000000..a3f4b2976e --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_MAV.cpp @@ -0,0 +1,108 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +// +// MAVLINK GPS driver +// +#include "AP_GPS_MAV.h" +#include + +AP_GPS_MAV::AP_GPS_MAV(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : + AP_GPS_Backend(_gps, _state, _port) +{ + _new_data = false; +} + +// 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_MAV::read(void) +{ + if (_new_data) { + _new_data = false; + return true; + } + + return false; +} + +// handles an incoming mavlink message (HIL_GPS) and sets +// corresponding gps data appropriately; +void +AP_GPS_MAV::handle_msg(mavlink_message_t *msg) +{ + mavlink_gps_input_t packet; + mavlink_msg_gps_input_decode(msg, &packet); + + bool have_alt = ((packet.ignore_flags & (1<<0)) == 0); + bool have_hdop = ((packet.ignore_flags & (1<<1)) == 0); + bool have_vdop = ((packet.ignore_flags & (1<<2)) == 0); + bool have_vel_h = ((packet.ignore_flags & (1<<3)) == 0); + bool have_vel_v = ((packet.ignore_flags & (1<<4)) == 0); + bool have_sa = ((packet.ignore_flags & (1<<5)) == 0); + bool have_ha = ((packet.ignore_flags & (1<<6)) == 0); + bool have_va = ((packet.ignore_flags & (1<<7)) == 0); + + state.time_week = packet.time_week; + state.time_week_ms = packet.time_week_ms; + state.status = (AP_GPS::GPS_Status)packet.fix_type; + + Location loc; + loc.lat = packet.lat; + loc.lng = packet.lon; + if (have_alt) + loc.alt = packet.alt; + state.location = loc; + state.location.options = 0; + + if (have_hdop) + state.hdop = packet.hdop * 10; //In centimeters + + if (have_vdop) + state.vdop = packet.vdop * 10; //In centimeters + + if (have_vel_h) { + Vector3f vel(packet.vn, packet.ve, 0); + if (have_vel_v) + vel.z = packet.vd; + + state.velocity = vel; + state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x))); + state.ground_speed = norm(vel.x, vel.y); + } + + if (have_sa) { + state.speed_accuracy = packet.speed_accuracy; + state.have_speed_accuracy = 1; + } + + if (have_ha) { + state.horizontal_accuracy = packet.horiz_accuracy; + state.have_horizontal_accuracy = 1; + } + + if (have_va) { + state.vertical_accuracy = packet.vert_accuracy; + state.have_vertical_accuracy = 1; + } + + state.num_sats = packet.satellites_visible; + + state.last_gps_time_ms = AP_HAL::millis(); + + _new_data = true; +} diff --git a/libraries/AP_GPS/AP_GPS_MAV.h b/libraries/AP_GPS/AP_GPS_MAV.h new file mode 100644 index 0000000000..eb632313dc --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_MAV.h @@ -0,0 +1,41 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +// +// Mavlink GPS driver which accepts gps position data from an external +// companion computer +// +#pragma once + +#include +#include + +#include "AP_GPS.h" +#include "GPS_Backend.h" + +class AP_GPS_MAV : public AP_GPS_Backend { +public: + AP_GPS_MAV(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); + + bool read(); + + static bool _detect(struct MAV_detect_state &state, uint8_t data); + + void handle_msg(mavlink_message_t *msg); + +private: + bool _new_data; +}; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index b230091b7c..185f37676b 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -51,6 +51,8 @@ public: virtual void broadcast_configuration_failure_reason(void) const { return ; } + virtual void handle_msg(mavlink_message_t *msg) { return ; } + protected: AP_HAL::UARTDriver *port; ///< UART we are attached to AP_GPS &gps; ///< access to frontend (for parameters)