Copter: control_stabilize, - integrate changes to get_throttle_surface_tracking

ArduCopter/control_auto.pde

@@ -7,12 +7,11 @@
 // auto_init - initialise auto controller
 static bool auto_init(bool ignore_checks)
 {
-    if (GPS_ok() || ignore_checks) {
+    if ((GPS_ok() && g.command_total > 1) || ignore_checks) {
         // set target to current position
         wp_nav.init_loiter_target();
         // initialise auto_yaw_mode
         set_auto_yaw_mode(get_default_auto_yaw_mode(false));
-        cliSerial->printf_P(PSTR("\nYM:%d\n"),(int)auto_yaw_mode);
         // clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function
         init_commands();
         return true;
@@ -169,7 +168,11 @@ float get_auto_heading(void)
 // guided_init - initialise guided controller
 static bool guided_init(bool ignore_checks)
 {
-    return true;
+    if (GPS_ok() || ignore_checks) {
+        return true;
+    }else{
+        return false;
+    }
 }
 
 // guided_run - runs the guided controller
@@ -194,7 +197,12 @@ static void land_run()
 // rtl_init - initialise rtl controller
 static bool rtl_init(bool ignore_checks)
 {
-    return true;
+    if (GPS_ok() || ignore_checks) {
+        do_RTL();
+        return true;
+    }else{
+        return false;
+    }
 }
 
 // rtl_run - runs the return-to-launch controller

ArduCopter/control_stabilize.pde

@@ -170,11 +170,12 @@ static void althold_run()
         // call throttle controller
         if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
             // if sonar is ok, use surface tracking
-            get_throttle_surface_tracking(target_climb_rate);
-        }else{
-            // if no sonar fall back stabilize rate controller
-            pos_control.climb_at_rate(target_climb_rate);
+            target_climb_rate = get_throttle_surface_tracking(target_climb_rate, G_Dt);
         }
+
+        // call position controller
+        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
+        pos_control.update_z_controller();
     }
 
     // refetch angle targets for reporting
@@ -187,12 +188,16 @@ static void althold_run()
 // circle_init - initialise circle controller
 static bool circle_init(bool ignore_checks)
 {
+    if (GPS_ok() || ignore_checks) {
+        return true;
+    }else{
+        return false;
+    }
     // set yaw to point to center of circle
     // yaw_look_at_WP = circle_center;
     // initialise bearing to current heading
     //yaw_look_at_WP_bearing = ahrs.yaw_sensor;
     //yaw_initialised = true;
-    return true;
 }
 
 // circle_run - runs the circle controller
@@ -275,11 +280,12 @@ static void loiter_run()
         // run altitude controller
         if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
             // if sonar is ok, use surface tracking
-            get_throttle_surface_tracking(target_climb_rate);
-        }else{
-            // if no sonar fall back stabilize rate controller
-            pos_control.climb_at_rate(target_climb_rate);
+            target_climb_rate = get_throttle_surface_tracking(target_climb_rate,G_Dt);
         }
+
+        // update altitude target and call position controller
+        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
+        pos_control.update_z_controller();
     }
 
     // refetch angle targets for reporting
@@ -292,7 +298,11 @@ static void loiter_run()
 // ofloiter_init - initialise ofloiter controller
 static bool ofloiter_init(bool ignore_checks)
 {
-    return true;
+    if (g.optflow_enabled || ignore_checks) {
+        return true;
+    }else{
+        return false;
+    }
 }
 
 // ofloiter_run - runs the optical flow loiter controller