GCS_MAVLink: move rally-point handling up

libraries/GCS_MAVLink/GCS.h

@@ -218,6 +218,7 @@ protected:
     // enforcement of GCS sysid
     virtual bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) { return true; }
     virtual AP_Mission *get_mission() = 0;
+    virtual AP_Rally *get_rally() const = 0;
 
     bool            waypoint_receiving; // currently receiving
     // the following two variables are only here because of Tracker
@@ -248,6 +249,10 @@ protected:
     void handle_param_request_list(mavlink_message_t *msg);
     void handle_param_request_read(mavlink_message_t *msg);
 
+    void handle_common_rally_message(mavlink_message_t *msg);
+    void handle_rally_fetch_point(mavlink_message_t *msg);
+    void handle_rally_point(mavlink_message_t *msg);
+
     void handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg) const;
     void handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio);
     void handle_serial_control(mavlink_message_t *msg, AP_GPS &gps);

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1759,6 +1759,12 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
     case MAVLINK_MSG_ID_STATUSTEXT:
         handle_statustext(msg);
         break;
+
+    case MAVLINK_MSG_ID_RALLY_POINT:
+        /* fall through */
+    case MAVLINK_MSG_ID_RALLY_FETCH_POINT:
+        handle_common_rally_message(msg);
+        break;
     }
 
 }

libraries/GCS_MAVLink/GCS_Dummy.h

@@ -15,6 +15,8 @@ class GCS_MAVLINK_Dummy : public GCS_MAVLINK
 protected:
 
     AP_Mission *get_mission() override { return nullptr; }
+    AP_Rally *get_rally() const override { return nullptr; };
+
     uint8_t sysid_my_gcs() const override { return 1; }
 
 };

libraries/GCS_MAVLink/GCS_Rally.cpp

@@ -0,0 +1,100 @@
+/*
+   GCS MAVLink functions related to rally points
+
+   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/>.
+ */
+
+#include "GCS.h"
+
+void GCS_MAVLINK::handle_rally_point(mavlink_message_t *msg)
+{
+    AP_Rally *r = get_rally();
+    if (r == nullptr) {
+        return;
+    }
+
+    mavlink_rally_point_t packet;
+    mavlink_msg_rally_point_decode(msg, &packet);
+
+    if (packet.idx >= r->get_rally_total() ||
+        packet.idx >= r->get_rally_max()) {
+        send_text(MAV_SEVERITY_NOTICE,"Bad rally point message ID");
+        return;
+    }
+
+    if (packet.count != r->get_rally_total()) {
+        send_text(MAV_SEVERITY_NOTICE,"Bad rally point message count");
+        return;
+    }
+
+    // sanity check location
+    if (!check_latlng(packet.lat, packet.lng)) {
+        return;
+    }
+
+    RallyLocation rally_point;
+    rally_point.lat = packet.lat;
+    rally_point.lng = packet.lng;
+    rally_point.alt = packet.alt;
+    rally_point.break_alt = packet.break_alt;
+    rally_point.land_dir = packet.land_dir;
+    rally_point.flags = packet.flags;
+
+    if (!r->set_rally_point_with_index(packet.idx, rally_point)) {
+        send_text(MAV_SEVERITY_CRITICAL, "Error setting rally point");
+    }
+}
+
+void GCS_MAVLINK::handle_rally_fetch_point(mavlink_message_t *msg)
+{
+    AP_Rally *r = get_rally();
+    if (r == nullptr) {
+        return;
+    }
+
+    mavlink_rally_fetch_point_t packet;
+    mavlink_msg_rally_fetch_point_decode(msg, &packet);
+
+    if (packet.idx > r->get_rally_total()) {
+        send_text(MAV_SEVERITY_NOTICE, "Bad rally point index");
+        return;
+    }
+
+    RallyLocation rally_point;
+    if (!r->get_rally_point_with_index(packet.idx, rally_point)) {
+        send_text(MAV_SEVERITY_NOTICE, "Failed to set rally point");
+        return;
+    }
+
+    mavlink_msg_rally_point_send_buf(msg,
+                                     chan, msg->sysid, msg->compid, packet.idx,
+                                     r->get_rally_total(), rally_point.lat, rally_point.lng,
+                                     rally_point.alt, rally_point.break_alt, rally_point.land_dir,
+                                     rally_point.flags);
+}
+
+void GCS_MAVLINK::handle_common_rally_message(mavlink_message_t *msg)
+{
+    switch (msg->msgid) {
+    case MAVLINK_MSG_ID_RALLY_POINT:
+        handle_rally_point(msg);
+        break;
+    case MAVLINK_MSG_ID_RALLY_FETCH_POINT:
+        handle_rally_fetch_point(msg);
+        break;
+    default:
+        break;
+    }
+}
+

libraries/GCS_MAVLink/examples/routing/routing.cpp

@@ -22,6 +22,7 @@ protected:
 
     uint32_t telem_delay() const override { return 0; }
     AP_Mission *get_mission() override { return nullptr; }
+    AP_Rally *get_rally() const override { return nullptr; }
     uint8_t sysid_my_gcs() const override { return 1; }
 
 private: