GCS_MAVLink: retry sending home/origin if it fails

Also grants us the ability to stream these messages

libraries/GCS_MAVLink/GCS.cpp

@@ -49,16 +49,6 @@ void GCS::send_named_float(const char *name, float value) const
     FOR_EACH_ACTIVE_CHANNEL(send_named_float(name, value));
 }
 
-void GCS::send_home() const
-{
-    FOR_EACH_ACTIVE_CHANNEL(send_home());
-}
-
-void GCS::send_ekf_origin() const
-{
-    FOR_EACH_ACTIVE_CHANNEL(send_ekf_origin());
-}
-
 /*
   install an alternative protocol handler. This allows another
   protocol to take over the link if MAVLink goes idle. It is used to

libraries/GCS_MAVLink/GCS.h

@@ -95,6 +95,8 @@ enum ap_message : uint8_t {
     MSG_AOA_SSA,
     MSG_LANDING,
     MSG_ESC_TELEMETRY,
+    MSG_ORIGIN,
+    MSG_HOME,
     MSG_NAMED_FLOAT,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };
@@ -219,8 +221,8 @@ public:
     void send_mount_status() const;
     void send_named_float(const char *name, float value) const;
     void send_gimbal_report() const;
-    void send_home() const;
+    void send_home_position() const;
-    void send_ekf_origin() const;
+    void send_gps_global_origin() const;
     virtual void send_position_target_global_int() { };
     void send_servo_output_raw();
     static void send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour);
@@ -742,8 +744,6 @@ public:
     void send_message(enum ap_message id);
     void send_mission_item_reached_message(uint16_t mission_index);
     void send_named_float(const char *name, float value) const;
-    void send_home() const;
-    void send_ekf_origin() const;
 
     void send_parameter_value(const char *param_name,
                               ap_var_type param_type,

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -883,7 +883,6 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
 
     // MSG_NAMED_FLOAT messages can't really be "streamed"...
 
-    // this list is ordered by AP_MESSAGE ID - the value being returned:
     static const struct {
         uint32_t mavlink_id;
         ap_message msg_id;
@@ -891,6 +890,8 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
         { MAVLINK_MSG_ID_HEARTBEAT,             MSG_HEARTBEAT},
         { MAVLINK_MSG_ID_ATTITUDE,              MSG_ATTITUDE},
         { MAVLINK_MSG_ID_GLOBAL_POSITION_INT,   MSG_LOCATION},
+        { MAVLINK_MSG_ID_HOME_POSITION,         MSG_HOME},
+        { MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN,     MSG_ORIGIN},
         { MAVLINK_MSG_ID_SYS_STATUS,            MSG_SYS_STATUS},
         { MAVLINK_MSG_ID_POWER_STATUS,          MSG_POWER_STATUS},
         { MAVLINK_MSG_ID_MEMINFO,               MSG_MEMINFO},
@@ -2226,11 +2227,8 @@ void GCS_MAVLINK::send_named_float(const char *name, float value) const
     mavlink_msg_named_value_float_send(chan, AP_HAL::millis(), float_name, value);
 }
 
-void GCS_MAVLINK::send_home() const
+void GCS_MAVLINK::send_home_position() const
 {
-    if (!HAVE_PAYLOAD_SPACE(chan, HOME_POSITION)) {
-        return;
-    }
     if (!AP::ahrs().home_is_set()) {
         return;
     }
@@ -2249,11 +2247,8 @@ void GCS_MAVLINK::send_home() const
         AP_HAL::micros64());
 }
 
-void GCS_MAVLINK::send_ekf_origin() const
+void GCS_MAVLINK::send_gps_global_origin() const
 {
-    if (!HAVE_PAYLOAD_SPACE(chan, GPS_GLOBAL_ORIGIN)) {
-        return;
-    }
     Location ekf_origin;
     if (!AP::ahrs().get_origin(ekf_origin)) {
         return;
@@ -2764,7 +2759,10 @@ void GCS_MAVLINK::set_ekf_origin(const Location& loc)
     ahrs.Log_Write_Home_And_Origin();
 
     // send ekf origin to GCS
-    send_ekf_origin();
+    if (!try_send_message(MSG_ORIGIN)) {
+        // try again later
+        send_message(MSG_ORIGIN);
+    }
 }
 
 void GCS_MAVLINK::handle_set_gps_global_origin(const mavlink_message_t *msg)
@@ -3451,8 +3449,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_get_home_position(const mavlink_command_l
     if (!AP::ahrs().home_is_set()) {
         return MAV_RESULT_FAILED;
     }
-    send_home();
+    if (!try_send_message(MSG_HOME)) {
-    send_ekf_origin();
+        // try again later
+        send_message(MSG_HOME);
+    }
+    if (!try_send_message(MSG_ORIGIN)) {
+        // try again later
+        send_message(MSG_ORIGIN);
+    }
 
     return MAV_RESULT_ACCEPTED;
 }
@@ -3950,6 +3954,16 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         send_global_position_int();
         break;
 
+    case MSG_HOME:
+        CHECK_PAYLOAD_SIZE(HOME_POSITION);
+        send_home_position();
+        break;
+
+    case MSG_ORIGIN:
+        CHECK_PAYLOAD_SIZE(GPS_GLOBAL_ORIGIN);
+        send_gps_global_origin();
+        break;
+
     case MSG_CURRENT_WAYPOINT:
     case MSG_MISSION_ITEM_REACHED:
     case MSG_NEXT_MISSION_REQUEST: