Copter: added TKOFF_NAV_ALT parameter

this adds TKOFF_NAV_ALT which controls the altitude above takeoff that
navigation can begin. It is meant for unstable vehicles such as helis
to prevent blade strike during initial takeoff.

This also adds a new parameter class ParametersG2 which can hold 64
parameters. This is to avoid running out of parameters in the first
256 block

ArduCopter/Copter.h

@@ -140,6 +140,7 @@ private:
 
     // Global parameters are all contained within the 'g' class.
     Parameters g;
+    ParametersG2 g2;
 
     // main loop scheduler
     AP_Scheduler scheduler;
@@ -271,6 +272,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
@@ -568,6 +572,8 @@ private:
         uint8_t dynamic_flight          : 1;    // 0   // true if we are moving at a significant speed (used to turn on/off leaky I terms)
         uint8_t init_targets_on_arming  : 1;    // 1   // true if we have been disarmed, and need to reset rate controller targets when we arm
     } heli_flags;
+
+    int16_t hover_roll_trim_scalar_slew;
 #endif
 
 #if GNDEFFECT_COMPENSATION == ENABLED
@@ -631,6 +637,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);

ArduCopter/Parameters.cpp

@@ -947,9 +947,30 @@ const AP_Param::Info Copter::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

ArduCopter/Parameters.h

@@ -54,6 +54,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,
@@ -530,4 +531,18 @@ 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[];

ArduCopter/control_auto.cpp

@@ -138,6 +138,9 @@ void Copter::auto_takeoff_start(const Location& dest_loc)
 
     // clear i term when we're taking off
     set_throttle_takeoff();
+
+    // get initial alt for TKOFF_NAV_ALT
+    auto_takeoff_set_start_alt();
 }
 
 // auto_takeoff_run - takeoff in auto mode
@@ -178,8 +181,8 @@ void Copter::auto_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);
 }
 
 // auto_wp_start - initialises waypoint controller to implement flying to a particular destination

ArduCopter/control_guided.cpp

@@ -72,6 +72,9 @@ bool Copter::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;
 }
 
@@ -350,8 +353,8 @@ void Copter::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

ArduCopter/heli.cpp

@@ -75,8 +75,6 @@ void Copter::check_dynamic_flight(void)
 // should be run between the rate controller and the servo updates.
 void Copter::update_heli_control_dynamics(void)
 {
-    static int16_t hover_roll_trim_scalar_slew = 0;
-
     // Use Leaky_I if we are not moving fast
     attitude_control.use_leaky_i(!heli_flags.dynamic_flight);
 

ArduCopter/takeoff.cpp

@@ -143,3 +143,42 @@ void Copter::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_cli
         }
     }
 }
+
+void Copter::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 Copter::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());
+}