AP_GPS: Add GPS_MAV type and accept GPS_INPUT message

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; i<num_instances; i++) {
+        if ((drivers[i] != NULL) && (_type[i] != GPS_TYPE_NONE)) {
+            drivers[i]->handle_msg(msg);
+        }
+    }
+}
+
 /*
   set HIL (hardware in the loop) status for a GPS instance
  */

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

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 <http://www.gnu.org/licenses/>.
+ */
+
+//
+//  MAVLINK GPS driver
+//
+#include "AP_GPS_MAV.h"
+#include <stdint.h>
+
+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;
+}

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 <http://www.gnu.org/licenses/>.
+ */
+
+//
+//  Mavlink GPS driver which accepts gps position data from an external
+//  companion computer
+//
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+
+#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;
+};

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)