Sub: Changes to match upstream Copter changes.

ArduSub/ArduSub.cpp

@@ -103,7 +103,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
     SCHED_TASK(compass_accumulate,   100,    100),
     SCHED_TASK(barometer_accumulate,  50,     90),
 #if PRECISION_LANDING == ENABLED
-    SCHED_TASK(update_precland,       50,     50),
+    SCHED_TASK(update_precland,      400,     50),
 #endif
 #if FRAME_CONFIG == HELI_FRAME
     SCHED_TASK(check_dynamic_flight,  50,     75),
@@ -432,6 +432,11 @@ void Sub::fifty_hz_logging_loop()
         DataFlash.Log_Write_IMU(ins);
     }
 #endif
+
+#if PRECISION_LANDING == ENABLED
+    // log output
+    Log_Write_Precland();
+#endif
 }
 
 // full_rate_logging_loop

ArduSub/Parameters.cpp

@@ -1000,9 +1000,28 @@ const AP_Param::Info Sub::var_info[] = {
 	// @User: Standard
 	GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0),
 
+	// @Group:
+    // @Path: Parameters.cpp
+    GOBJECT(g2, "",  ParametersG2),
+
     AP_VAREND
 };
 
+/*
+  2nd group of parameters
+ */
+const AP_Param::GroupInfo ParametersG2::var_info[] = {
+
+    // @Param: TKOFF_NAV_ALT
+    // @DisplayName: Takeoff navigation altitude
+    // @Description: This is the altitude in meters above the takeoff point that attitude changes for navigation can begin
+    // @Range: 0 5
+    // @User: Standard
+    AP_GROUPINFO("WP_TKOFF_NAV_ALT", 1, ParametersG2, takeoff_nav_alt, 0),
+
+    AP_GROUPEND
+};
+
 /*
   This is a conversion table from old parameter values to new
   parameter names. The startup code looks for saved values of the old

ArduSub/Parameters.h

@@ -55,6 +55,7 @@ public:
         k_param_ins,                            // libraries/AP_InertialSensor variables
 		k_param_NavEKF2_old, // deprecated
 		k_param_NavEKF2,
+		k_param_g2, // 2nd block of parameters
 
         // simulation
         k_param_sitl = 10,
@@ -572,5 +573,19 @@ public:
     }
 };
 
+/*
+  2nd block of parameters, to avoid going past 256 top level keys
+*/
+class ParametersG2 {
+public:
+    ParametersG2(void) { AP_Param::setup_object_defaults(this, var_info); }
+
+    // var_info for holding Parameter information
+    static const struct AP_Param::GroupInfo var_info[];
+
+    // altitude at which nav control can start in takeoff
+    AP_Float takeoff_nav_alt;
+};
+
 extern const AP_Param::Info        var_info[];
 

ArduSub/Sub.h

@@ -143,6 +143,7 @@ private:
 
     // Global parameters are all contained within the 'g' class.
     Parameters g;
+    ParametersG2 g2;
 
     // main loop scheduler
     AP_Scheduler scheduler;
@@ -279,6 +280,9 @@ private:
 
     uint32_t precland_last_update_ms;
 
+    // altitude below which we do no navigation in auto takeoff
+    float auto_takeoff_no_nav_alt_cm;
+
     RCMapper rcmap;
 
     // board specific config
@@ -615,6 +619,8 @@ private:
     float get_takeoff_trigger_throttle();
     float get_throttle_pre_takeoff(float input_thr);
     float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
+    void auto_takeoff_set_start_alt(void);
+    void auto_takeoff_attitude_run(float target_yaw_rate);
     void set_accel_throttle_I_from_pilot_throttle(float pilot_throttle);
     void update_poscon_alt_max();
     void rotate_body_frame_to_NE(float &x, float &y);
@@ -784,6 +790,8 @@ private:
     void land_run();
     void land_gps_run();
     void land_nogps_run();
+    void land_run_vertical_control(bool pause_descent = false);
+    void land_run_horizontal_control();
     float get_land_descent_speed();
     void land_do_not_use_GPS();
     void set_mode_land_with_pause(mode_reason_t reason);

ArduSub/control_guided.cpp

@@ -72,6 +72,9 @@ bool Sub::guided_takeoff_start(float final_alt_above_home)
 	// clear i term when we're taking off
 	set_throttle_takeoff();
 
+	// get initial alt for WP_TKOFF_NAV_ALT
+	auto_takeoff_set_start_alt();
+
 	return true;
 }
 
@@ -334,8 +337,8 @@ void Sub::guided_takeoff_run()
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control.update_z_controller();
 
-    // roll & pitch from waypoint controller, yaw rate from pilot
-    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
+    // call attitude controller
+    auto_takeoff_attitude_run(target_yaw_rate);
 }
 
 // guided_pos_control_run - runs the guided position controller

ArduSub/precision_landing.cpp

@@ -8,21 +8,23 @@
 
 #if PRECISION_LANDING == ENABLED
 
-void Copter::init_precland()
+void Sub::init_precland()
 {
-    copter.precland.init();
+    sub.precland.init();
 }
 
-void Copter::update_precland()
+void Sub::update_precland()
 {
-    float final_alt = current_loc.alt;
+	int32_t height_above_ground_cm = current_loc.alt;
 
-    // use range finder altitude if it is valid
-    if (rangefinder_alt_ok()) {
-        final_alt = rangefinder_state.alt_cm;
+	// use range finder altitude if it is valid, else try to get terrain alt
+	if (rangefinder_alt_ok()) {
+		height_above_ground_cm = rangefinder_state.alt_cm;
+	} else if (terrain_use()) {
+		current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm);
     }
 
-    copter.precland.update(final_alt);
+    sub.precland.update(height_above_ground_cm);
 
     // log output
     Log_Write_Precland();

ArduSub/takeoff.cpp

@@ -136,3 +136,42 @@ void Sub::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_
         }
     }
 }
+
+void Sub::auto_takeoff_set_start_alt(void)
+{
+    // start with our current altitude
+    auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude();
+
+    if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
+        // we are not flying, add the takeoff_nav_alt
+        auto_takeoff_no_nav_alt_cm += g2.takeoff_nav_alt * 100;
+    }
+}
+
+
+/*
+  call attitude controller for automatic takeoff, limiting roll/pitch
+  if below takeoff_nav_alt
+ */
+void Sub::auto_takeoff_attitude_run(float target_yaw_rate)
+{
+    float nav_roll, nav_pitch;
+
+    if (g2.takeoff_nav_alt > 0 && inertial_nav.get_altitude() < auto_takeoff_no_nav_alt_cm) {
+        // we haven't reached the takeoff navigation altitude yet
+        nav_roll = 0;
+        nav_pitch = 0;
+#if FRAME_CONFIG == HELI_FRAME
+        // prevent hover roll starting till past specified altitude
+        hover_roll_trim_scalar_slew = 0;
+#endif
+        // tell the position controller that we have limited roll/pitch demand to prevent integrator buildup
+        pos_control.set_limit_accel_xy();
+    } else {
+        nav_roll = wp_nav.get_roll();
+        nav_pitch = wp_nav.get_pitch();
+    }
+
+    // roll & pitch from waypoint controller, yaw rate from pilot
+    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain());
+}