wip:params and framework

ArduPlane/GCS_Mavlink.cpp

@@ -59,6 +59,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
     case Mode::Number::GUIDED:
     case Mode::Number::CIRCLE:
     case Mode::Number::TAKEOFF:
+    case Mode::Number::AUTOLAND:
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QRTL:
     case Mode::Number::LOITER_ALT_QLAND:

ArduPlane/GCS_Plane.cpp

@@ -70,6 +70,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
     case Mode::Number::GUIDED:
     case Mode::Number::CIRCLE:
     case Mode::Number::TAKEOFF:
+    case Mode::Number::AUTOLAND:
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QRTL:
     case Mode::Number::LOITER_ALT_QLAND:

ArduPlane/Parameters.cpp

@@ -1027,6 +1027,10 @@ const AP_Param::Info Plane::var_info[] = {
     // @Group: TKOFF_
     // @Path: mode_takeoff.cpp
     GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff),
+
+    // @Group: AUTOLAND_
+    // @Path: mode_autoland.cpp
+    GOBJECT(mode_autoland, "AUTOLAND_", ModeAutoLand),
 
 #if AP_PLANE_GLIDER_PULLUP_ENABLED
     // @Group: PUP_

ArduPlane/Parameters.h

@@ -100,7 +100,7 @@ class Parameters {
         k_param_sonar_old, // unused
         k_param_log_bitmask,
         k_param_BoardConfig,
-        k_param_rssi_range,     // unused, replaced by rssi_ library parameters
+        k_param_mode_autoland,     // was rssi_range
         k_param_flapin_channel_old,  // unused, moved to RC_OPTION
         k_param_flaperon_output, // unused
         k_param_gps,

ArduPlane/Plane.h

@@ -326,6 +326,7 @@ class Plane : public AP_Vehicle {
 #endif  // QAUTOTUNE_ENABLED
 #endif  // HAL_QUADPLANE_ENABLED
     ModeTakeoff mode_takeoff;
+    ModeAutoLand mode_autoland;
 #if HAL_SOARING_ENABLED
     ModeThermal mode_thermal;
 #endif

ArduPlane/control_modes.cpp

@@ -83,6 +83,9 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::TAKEOFF:
         ret = &mode_takeoff;
         break;
+    case Mode::Number::AUTOLAND:
+        ret = &mode_autoland;
+        break;
     case Mode::Number::THERMAL:
 #if HAL_SOARING_ENABLED
         ret = &mode_thermal;

ArduPlane/events.cpp

@@ -65,7 +65,8 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
         break;
 #endif // HAL_QUADPLANE_ENABLED
 
-    case Mode::Number::AUTO: {
+    case Mode::Number::AUTO:
+    case Mode::Number::AUTOLAND: {
         if (failsafe_in_landing_sequence()) {
             // don't failsafe in a landing sequence
             break;
@@ -169,6 +170,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
 #endif  // HAL_QUADPLANE_ENABLED
 
     case Mode::Number::AUTO:
+    case Mode::Number::AUTOLAND:
         if (failsafe_in_landing_sequence()) {
             // don't failsafe in a landing sequence
             break;

ArduPlane/mode.h

@@ -55,6 +55,7 @@ class Mode
 #if HAL_QUADPLANE_ENABLED
         LOITER_ALT_QLAND = 25,
 #endif
+        AUTOLAND      = 26,
     };
 
     // Constructor
@@ -835,6 +836,43 @@ class ModeTakeoff: public Mode
     // flag that we have already called autoenable fences once in MODE TAKEOFF
     bool have_autoenabled_fences;
 
+};
+
+class ModeAutoLand: public Mode
+{
+public:
+    ModeAutoLand();
+
+    Number mode_number() const override { return Number::AUTOLAND; }
+    const char *name() const override { return "AUTOLAND"; }
+    const char *name4() const override { return "AUTOLAND"; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+    void navigate() override;
+
+    bool allows_throttle_nudging() const override { return true; }
+
+    bool does_auto_navigation() const override { return true; }
+
+    bool does_auto_throttle() const override { return true; }
+
+
+    // var_info for holding parameter information
+    static const struct AP_Param::GroupInfo var_info[];
+
+    AP_Int16 final_wp_alt;
+    AP_Int16 final_wp_dist;
+
+protected:
+    bool _enter() override;
+
+
+private:
+
+
+
 };
 
 #if HAL_SOARING_ENABLED

ArduPlane/mode_autoland.cpp

@@ -0,0 +1,56 @@
+#include "mode.h"
+#include "Plane.h"
+#include <GCS_MAVLink/GCS.h>
+
+/*
+  mode AutoLand parameters
+ */
+const AP_Param::GroupInfo ModeAutoLand::var_info[] = {
+    // @Param: WP_ALT
+    // @DisplayName: Final approach WP altitude
+    // @Description: This is the target altitude for final approach waypoint
+    // @Range: 0 200
+    // @Increment: 1
+    // @Units: m
+    // @User: Standard
+    AP_GROUPINFO("WP_ALT", 1, ModeAutoLand, final_wp_alt, 55),
+
+    // @Param: DIST
+    // @DisplayName: Final approach WP distance
+    // @Description: This is the distance from Home that the final approach waypoint is set. The waypoint point will be in the opposite direction of takeoff (the direction the plane is facing when the plane sets its takeoff heading)
+    // @Range: 0 700
+    // @Increment: 1
+    // @Units: m
+    // @User: Standard
+    AP_GROUPINFO("WP_DIST", 2, ModeAutoLand, final_wp_dist, 400),
+
+    AP_GROUPEND
+};
+
+ModeAutoLand::ModeAutoLand() :
+    Mode()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+bool ModeAutoLand::_enter()
+{
+// upon enter we setup a final wp and land wp based on home and takeoff_dir
+    const Location &home = ahrs.get_home();
+    plane.set_target_altitude_current();
+    plane.next_WP_loc.offset_bearing(bearing_cd, home.alt*0.01f + final_wp_alt);
+    plane.prev_WP_loc = plane.current_loc;
+    plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
+    return true;
+}
+
+void ModeAutoLand::update()
+{
+    plane.calc_nav_roll();
+    plane.update_fbwb_speed_height();
+}
+
+void ModeAutoLand::navigate()
+{
+
+}