AP_Landing: yaw correction on landing

libraries/AP_Landing/AP_Landing.cpp

@@ -142,6 +142,15 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = {
     // @Path: AP_Landing_Deepstall.cpp
     AP_SUBGROUPINFO(deepstall, "DS_", 15, AP_Landing, AP_Landing_Deepstall),
     
+    // @Param: TD_ALT
+    // @DisplayName: Touch down altitude
+    // @Description: Altitude to trigger touchdown condition if weight on wheels sensor is not available. Disabled when 0. Recommend using an RTK GPS or rangefinder for accurate altitude
+    // @Units: m
+    // @Range: 0 5
+    // @Increment: 0.01
+    // @User: Advanced
+    AP_GROUPINFO("TD_ALT", 16, AP_Landing, touchdown_altitude, 0.0f),
+    
     AP_GROUPEND
 };
 

libraries/AP_Landing/AP_Landing.h

@@ -134,6 +134,9 @@ private:
     adjusted_relative_altitude_cm_fn_t adjusted_relative_altitude_cm_fn;
     disarm_if_autoland_complete_fn_t disarm_if_autoland_complete_fn;
     update_flight_stage_fn_t update_flight_stage_fn;
+    
+    // saved bearing for yaw correction after touchdown
+    float runway_bearing;
 
     // support for deepstall landings
     AP_Landing_Deepstall deepstall;
@@ -152,6 +155,7 @@ private:
     AP_Int8 flap_percent;
     AP_Int8 throttle_slewrate;
     AP_Int8 type;
+    AP_Float touchdown_altitude;
 
     // Land Type STANDARD GLIDE SLOPE
 
@@ -165,8 +169,9 @@ private:
     struct {
         // once landed, post some landing statistics to the GCS
         bool post_stats:1;
-
+        bool force_flare:1;
         bool has_aborted_due_to_slope_recalc:1;
+        bool touched_down:1;
     } type_slope_flags;
 
     void type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);

libraries/AP_Landing/AP_Landing_Slope.cpp

@@ -29,6 +29,8 @@ void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, cons
 
     // once landed, post some landing statistics to the GCS
     type_slope_flags.post_stats = false;
+    
+    type_slope_flags.force_flare = false;
 
     type_slope_stage = SLOPE_STAGE_NORMAL;
     gcs().send_text(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude);
@@ -65,7 +67,26 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
             type_slope_stage = SLOPE_STAGE_APPROACH;
         }
     }
-
+    
+    // Check for weight-on-wheels touchdown
+    AP_LandingGear *landinggear = AP_LandingGear::instance();
+    bool touchdown_debounced = false;
+    if (landinggear != nullptr) {
+        touchdown_debounced = (landinggear->get_wow_state() == AP_LandingGear::LG_WOW) && landinggear->get_wow_state_duration_ms() >= 500;
+     }
+    const bool touchdown_alt_override = !is_zero(touchdown_altitude) &&
+        (height < touchdown_altitude);
+
+    if (!type_slope_flags.touched_down) {
+        if (touchdown_alt_override || touchdown_debounced) {
+            type_slope_flags.touched_down = true; // sticky flag
+            type_slope_flags.force_flare = true; // force flare
+            gcs().send_text(MAV_SEVERITY_INFO, "Touchdown encountered");
+        }
+    } else if (type_slope_stage != SLOPE_STAGE_FINAL) {
+        type_slope_flags.touched_down = false;
+    }
+    
     /* Set land_complete (which starts the flare) under 3 conditions:
        1) we are within LAND_FLARE_ALT meters of the landing altitude
        2) we are within LAND_FLARE_SEC of the landing point vertically
@@ -90,8 +111,10 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
     if ((on_approach_stage && below_flare_alt) ||
         (on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) ||
         (!rangefinder_state_in_range && wp_proportion >= 1) ||
+        type_slope_flags.force_flare ||
         probably_crashed) {
 
+        type_slope_flags.force_flare = false;
         if (type_slope_stage != SLOPE_STAGE_FINAL) {
             type_slope_flags.post_stats = true;
             if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) {
@@ -107,8 +130,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
             
             // Check if the landing gear was deployed before landing
             // If not - go around
-            AP_LandingGear *LG_inst = AP_LandingGear::instance();
-            if (LG_inst != nullptr && !LG_inst->check_before_land()) {
+            if (landinggear != nullptr && !landinggear->check_before_land()) {
                 type_slope_request_go_around();
                 gcs().send_text(MAV_SEVERITY_CRITICAL, "Landing gear was not deployed");
             }
@@ -136,11 +158,14 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
       prevents sudden turns if we overshoot the landing point
      */
     struct Location land_WP_loc = next_WP_loc;
-    int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
     location_update(land_WP_loc,
-                    land_bearing_cd*0.01f,
+                    runway_bearing,
                     get_distance(prev_WP_loc, current_loc) + 200);
-    nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
+    if (!type_slope_flags.touched_down) {
+        nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
+    } else {
+        nav_controller->update_heading_hold(runway_bearing*100);
+    }
 
     // once landed and stationary, post some statistics
     // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
@@ -152,6 +177,13 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
     // check if we should auto-disarm after a confirmed landing
     if (type_slope_stage == SLOPE_STAGE_FINAL) {
         disarm_if_autoland_complete_fn();
+
+        // Check for touchdown
+        if (!type_slope_flags.touched_down &&
+            (touchdown_alt_override || touchdown_debounced)) {
+            type_slope_flags.touched_down = true;
+            gcs().send_text(MAV_SEVERITY_INFO, "Touchdown encountered");
+        }
     }
 
     /*
@@ -288,16 +320,17 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
     // project a point 500 meters past the landing point, passing
     // through the landing point
     const float land_projection = 500;
-    int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
+    runway_bearing = get_bearing_cd(prev_WP_loc, next_WP_loc) * 0.01f;
+    type_slope_flags.touched_down = false;
 
     // now calculate our aim point, which is before the landing
     // point and above it
     Location loc = next_WP_loc;
-    location_update(loc, land_bearing_cd*0.01f, -flare_distance);
+    location_update(loc, runway_bearing, -flare_distance);
     loc.alt += aim_height*100;
 
     // calculate point along that slope 500m ahead
-    location_update(loc, land_bearing_cd*0.01f, land_projection);
+    location_update(loc, runway_bearing, land_projection);
     loc.alt -= slope * land_projection * 100;
 
     // setup the offset_cm for set_target_altitude_proportion()