AP_Landing: Add MAVLink reporting interface

libraries/AP_Landing/AP_Landing.cpp

@@ -263,6 +263,21 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing
     }
 }
 
+// send out any required mavlink messages
+bool AP_Landing::send_landing_message(mavlink_channel_t chan) {
+    if (!flags.in_progress) {
+        return false;
+    }
+
+    switch (type) {
+    case TYPE_DEEPSTALL:
+        return deepstall.send_deepstall_message(chan);
+    case TYPE_STANDARD_GLIDE_SLOPE:
+    default:
+        return false;
+    }
+}
+
 bool AP_Landing::is_flaring(void) const
 {
     if (!flags.in_progress) {

libraries/AP_Landing/AP_Landing.h

@@ -20,6 +20,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_SpdHgtControl/AP_SpdHgtControl.h>
 #include <AP_Navigation/AP_Navigation.h>
+#include <GCS_MAVLink/GCS.h>
 #include "AP_Landing_Deepstall.h"
 
 /// @class  AP_Landing
@@ -72,6 +73,7 @@ public:
     void handle_flight_stage_change(const bool _in_landing_stage);
     int32_t constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd);
     bool get_target_altitude_location(Location &location);
+    bool send_landing_message(mavlink_channel_t chan);
 
     // helper functions
     bool restart_landing_sequence(void);

libraries/AP_Landing/AP_Landing_Deepstall.cpp

@@ -364,6 +364,24 @@ int32_t AP_Landing_Deepstall::get_target_airspeed_cm(void) const
     }
 }
 
+bool AP_Landing_Deepstall::send_deepstall_message(mavlink_channel_t chan) const
+{
+    CHECK_PAYLOAD_SIZE2(DEEPSTALL);
+    mavlink_msg_deepstall_send(
+        chan,
+        landing_point.lat,
+        landing_point.lng,
+        stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_exit.lat : 0.0f,
+        stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_exit.lng : 0.0f,
+        stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_entry.lat : 0.0f,
+        stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_entry.lng : 0.0f,
+        landing_point.alt * 0.01,
+        stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? predicted_travel_distance : 0.0f,
+        stage == DEEPSTALL_STAGE_LAND ? crosstrack_error : 0.0f,
+        stage);
+    return true;
+}
+
 const DataFlash_Class::PID_Info& AP_Landing_Deepstall::get_pid_info(void) const
 {
     return ds_PID.get_pid_info();
@@ -414,7 +432,7 @@ void AP_Landing_Deepstall::build_approach_path(void)
 
 }
 
-float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const float height) const
+float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const float height)
 {
     bool reverse = false;
 
@@ -446,11 +464,13 @@ float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const f
 
     float estimated_forward = cosf(estimated_crab_angle) * forward_speed_ms + cosf(theta) * wind_length;
 
+    predicted_travel_distance = estimated_forward * height / down_speed + stall_distance;
+
 #ifdef DEBUG_PRINTS
-    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Predict: %f %f", stall_distance,  estimated_forward * height / down_speed + stall_distance);
+    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Predict: %f %f", stall_distance,  predicted_travel_distance);
 #endif // DEBUG_PRINTS
 
-    return estimated_forward * height / down_speed + stall_distance;
+    return predicted_travel_distance;
 }
 
 bool AP_Landing_Deepstall::verify_breakout(const Location &current_loc, const Location &target_loc,
@@ -488,7 +508,7 @@ float AP_Landing_Deepstall::update_steering()
     ab.normalize();
     Vector2f a_air = location_diff(arc_exit, current_loc);
 
-    float crosstrack_error = a_air % ab;
+    crosstrack_error = a_air % ab;
     float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
     float nu1 = asinf(sine_nu1);
 

libraries/AP_Landing/AP_Landing_Deepstall.h

@@ -20,12 +20,13 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_SpdHgtControl/AP_SpdHgtControl.h>
 #include <AP_Navigation/AP_Navigation.h>
+#include <GCS_MAVLink/GCS.h>
 #include <PID/PID.h>
 
 class AP_Landing;
 
-/// @class  AP_Landing
-/// @brief  Class managing ArduPlane landing methods
+/// @class  AP_Landing_Deepstall
+/// @brief  Class managing Plane Deepstall landing methods
 class AP_Landing_Deepstall
 {
 private:
@@ -81,6 +82,8 @@ private:
     float L1_xtrack_i;             // L1 integrator for navigation
     PID ds_PID;
     int32_t last_target_bearing;   // used for tracking the progress on loitering
+    float crosstrack_error; // current crosstrack error
+    float predicted_travel_distance; // distance the aircraft is perdicted to travel during deepstall
 
     //public AP_Landing interface
     void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
@@ -97,11 +100,13 @@ private:
     bool is_throttle_suppressed(void) const;
     bool is_flying_forward(void) const;
 
+    bool send_deepstall_message(mavlink_channel_t chan) const;
+
     const DataFlash_Class::PID_Info& get_pid_info(void) const;
 
     //private helpers
     void build_approach_path();
-    float predict_travel_distance(const Vector3f wind, const float height) const;
+    float predict_travel_distance(const Vector3f wind, const float height);
     bool verify_breakout(const Location &current_loc, const Location &target_loc, const float height_error) const;
     float update_steering(void);