Copter: implement sending of position_target_global_int

ArduCopter/GCS_Mavlink.cpp

@@ -98,6 +98,30 @@ MAV_STATE GCS_MAVLINK_Copter::system_status() const
 }
 
 
+void GCS_MAVLINK_Copter::send_position_target_global_int()
+{
+    Location_Class target;
+    if (!copter.flightmode->get_wp(target)) {
+        return;
+    }
+    mavlink_msg_position_target_global_int_send(
+        chan,
+        AP_HAL::millis(), // time_boot_ms
+        MAV_FRAME_GLOBAL_INT, // targets are always global altitude
+        0xFFF8, // ignore everything except the x/y/z components
+        target.lat, // latitude as 1e7
+        target.lng, // longitude as 1e7
+        target.alt * 0.01f, // altitude is sent as a float
+        0.0f, // vx
+        0.0f, // vy
+        0.0f, // vz
+        0.0f, // afx
+        0.0f, // afy
+        0.0f, // afz
+        0.0f, // yaw
+        0.0f); // yaw_rate
+}
+
 #if AC_FENCE == ENABLED
 NOINLINE void Copter::send_fence_status(mavlink_channel_t chan)
 {
@@ -360,7 +384,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
 
     case MSG_LIMITS_STATUS:
     case MSG_WIND:
-    case MSG_POSITION_TARGET_GLOBAL_INT:
     case MSG_SERVO_OUT:
     case MSG_AOA_SSA:
     case MSG_LANDING:
@@ -493,7 +516,8 @@ static const uint8_t STREAM_EXTENDED_STATUS_msgs[] = {
     MSG_GPS2_RAW,
     MSG_GPS2_RTK,
     MSG_NAV_CONTROLLER_OUTPUT,
-    MSG_FENCE_STATUS
+    MSG_FENCE_STATUS,
+    MSG_POSITION_TARGET_GLOBAL_INT,
 };
 static const uint8_t STREAM_POSITION_msgs[] = {
     MSG_LOCATION,

ArduCopter/GCS_Mavlink.h

@@ -34,6 +34,8 @@ protected:
 
     MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
 
+    void send_position_target_global_int() override;
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;

ArduCopter/mode.h

@@ -93,6 +93,7 @@ protected:
     virtual void run_autopilot() {}
     virtual uint32_t wp_distance() const { return 0; }
     virtual int32_t wp_bearing() const { return 0; }
+    virtual bool get_wp(Location_Class &loc) { return false; };
     virtual bool in_guided_mode() const { return false; }
 
     // pilot input processing
@@ -279,6 +280,7 @@ protected:
 
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
+    bool get_wp(Location_Class &loc) override;
     void run_autopilot() override;
 
 private:
@@ -755,6 +757,7 @@ public:
     void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
     bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
     bool set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
+    bool get_wp(Location_Class &loc) override;
     void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
     bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
 

ArduCopter/mode_auto.cpp

@@ -616,6 +616,18 @@ int32_t Copter::ModeAuto::wp_bearing() const
     return wp_nav->get_wp_bearing_to_destination();
 }
 
+bool Copter::ModeAuto::get_wp(Location_Class& destination)
+{
+    switch (_mode) {
+    case Auto_NavGuided:
+        return copter.mode_guided.get_wp(destination);
+    case Auto_WP:
+        return wp_nav->get_wp_destination(destination);
+    default:
+        return false;
+    }
+}
+
 // update mission
 void Copter::ModeAuto::run_autopilot()
 {

ArduCopter/mode_guided.cpp

@@ -203,6 +203,14 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y
     return true;
 }
 
+bool Copter::ModeGuided::get_wp(Location_Class& destination)
+{
+    if (guided_mode != Guided_WP) {
+        return false;
+    }
+    return wp_nav->get_wp_destination(destination);
+}
+
 // sets guided mode's target from a Location object
 // returns false if destination could not be set (probably caused by missing terrain data)
 // or if the fence is enabled and guided waypoint is outside the fence