Support LOITER_TO_ALT in Copter

ArduCopter/defines.h

@@ -145,6 +145,7 @@ enum AutoMode {
     Auto_Spline,
     Auto_NavGuided,
     Auto_Loiter,
+    Auto_LoiterToAlt,
     Auto_NavPayloadPlace,
 };
 

ArduCopter/mode.h

@@ -338,6 +338,7 @@ class ModeAuto : public Mode {
     void circle_run();
     void nav_guided_run();
     void loiter_run();
+    void loiter_to_alt_run();
 
     Location_Class loc_from_cmd(const AP_Mission::Mission_Command& cmd) const;
 
@@ -358,6 +359,7 @@ class ModeAuto : public Mode {
     void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
     void do_circle(const AP_Mission::Mission_Command& cmd);
     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
+    void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
     void do_spline_wp(const AP_Mission::Mission_Command& cmd);
 #if NAV_GUIDED == ENABLED
     void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
@@ -392,6 +394,7 @@ class ModeAuto : public Mode {
     bool verify_payload_place();
     bool verify_loiter_unlimited();
     bool verify_loiter_time();
+    bool verify_loiter_to_alt();
     bool verify_RTL();
     bool verify_wait_delay();
     bool verify_within_distance();
@@ -410,6 +413,14 @@ class ModeAuto : public Mode {
     uint16_t loiter_time_max;                // How long we should stay in Loiter Mode for mission scripting (time in seconds)
     uint32_t loiter_time;                    // How long have we been loitering - The start time in millis
 
+    struct {
+        bool reached_destination_xy : 1;
+        bool loiter_start_done : 1;
+        bool reached_alt : 1;
+        float alt_error_cm;
+        int32_t alt;
+    } loiter_to_alt;
+
     // Delay the next navigation command
     int32_t nav_delay_time_max;  // used for delaying the navigation commands (eg land,takeoff etc.)
     uint32_t nav_delay_time_start;

ArduCopter/mode_auto.cpp

@@ -94,6 +94,10 @@ void Copter::ModeAuto::run()
         loiter_run();
         break;
 
+    case Auto_LoiterToAlt:
+        loiter_to_alt_run();
+        break;
+
     case Auto_NavPayloadPlace:
         payload_place_run();
         break;
@@ -412,6 +416,10 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         do_loiter_time(cmd);
         break;
 
+    case MAV_CMD_NAV_LOITER_TO_ALT:
+        do_loiter_to_alt(cmd);
+        break;
+
     case MAV_CMD_NAV_RETURN_TO_LAUNCH:             //20
         do_RTL();
         break;
@@ -674,6 +682,9 @@ bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_LOITER_TIME:
         return verify_loiter_time();
 
+    case MAV_CMD_NAV_LOITER_TO_ALT:
+        return verify_loiter_to_alt();
+
     case MAV_CMD_NAV_RETURN_TO_LAUNCH:
         return verify_RTL();
 
@@ -908,6 +919,58 @@ void Copter::ModeAuto::loiter_run()
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
 }
 
+// auto_loiter_run - loiter to altitude in AUTO flight mode
+//      called by auto_run at 100hz or more
+void Copter::ModeAuto::loiter_to_alt_run()
+{
+    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
+    if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
+        zero_throttle_and_relax_ac();
+        return;
+    }
+
+    // possibly just run the waypoint controller:
+    if (!loiter_to_alt.reached_destination_xy) {
+        loiter_to_alt.reached_destination_xy = wp_nav->reached_wp_destination_xy();
+        if (!loiter_to_alt.reached_destination_xy) {
+            wp_run();
+            return;
+        }
+    }
+
+    if (!loiter_to_alt.loiter_start_done) {
+        loiter_nav->init_target();
+        _mode = Auto_LoiterToAlt;
+        loiter_to_alt.loiter_start_done = true;
+    }
+    const float alt_error_cm = copter.current_loc.alt - loiter_to_alt.alt;
+    if (fabsf(alt_error_cm) < 5.0) { // random numbers R US
+        loiter_to_alt.reached_alt = true;
+    } else if (alt_error_cm * loiter_to_alt.alt_error_cm < 0) {
+        // we were above and are now below, or vice-versa
+        loiter_to_alt.reached_alt = true;
+    }
+    loiter_to_alt.alt_error_cm = alt_error_cm;
+
+    // loiter...
+
+    land_run_horizontal_control();
+
+    // Compute a vertical velocity demand such that the vehicle
+    // approaches the desired altitude.
+    float target_climb_rate = AC_AttitudeControl::sqrt_controller(
+        -alt_error_cm,
+        pos_control->get_pos_z_p().kP(),
+        pos_control->get_max_accel_z(),
+        G_Dt);
+
+    // get avoidance adjusted climb rate
+    target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
+
+    pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
+    pos_control->update_z_controller();
+}
+
 // auto_payload_place_start - initialises controller to implement placement of a load
 void Copter::ModeAuto::payload_place_start(const Vector3f& destination)
 {
@@ -1162,6 +1225,43 @@ void Copter::ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd)
     loiter_time_max = cmd.p1;     // units are (seconds)
 }
 
+// do_loiter_alt - initiate loitering at a point until a given altitude is reached
+// note: caller should set yaw_mode
+void Copter::ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
+{
+    // re-use loiter unlimited
+    do_loiter_unlimited(cmd);
+    _mode = Auto_LoiterToAlt;
+
+    // if we aren't navigating to a location then we have to adjust
+    // altitude for current location
+    Location_Class target_loc(cmd.content.location);
+    const Location_Class &current_loc = copter.current_loc;
+    if (target_loc.lat == 0 && target_loc.lng == 0) {
+        target_loc.lat = current_loc.lat;
+        target_loc.lng = current_loc.lng;
+    }
+
+    if (!target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, loiter_to_alt.alt)) {
+        loiter_to_alt.reached_destination_xy = true;
+        loiter_to_alt.reached_alt = true;
+        gcs().send_text(MAV_SEVERITY_INFO, "bad do_loiter_to_alt");
+        return;
+    }
+    loiter_to_alt.reached_destination_xy = false;
+    loiter_to_alt.loiter_start_done = false;
+    loiter_to_alt.reached_alt = false;
+    loiter_to_alt.alt_error_cm = 0;
+
+    pos_control->set_max_accel_z(wp_nav->get_accel_z());
+    pos_control->set_max_speed_z(wp_nav->get_speed_down(),
+                                 wp_nav->get_speed_up());
+
+    if (pos_control->is_active_z()) {
+        pos_control->freeze_ff_z();
+    }
+}
+
 // do_spline_wp - initiate move to next waypoint
 void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
 {
@@ -1684,6 +1784,17 @@ bool Copter::ModeAuto::verify_loiter_time()
     return (((millis() - loiter_time) / 1000) >= loiter_time_max);
 }
 
+// verify_loiter_to_alt - check if we have reached both destination
+// (roughly) and altitude (precisely)
+bool Copter::ModeAuto::verify_loiter_to_alt()
+{
+    if (loiter_to_alt.reached_destination_xy &&
+        loiter_to_alt.reached_alt) {
+        return true;
+    }
+    return false;
+}
+
 // verify_RTL - handles any state changes required to implement RTL
 // do_RTL should have been called once first to initialise all variables
 // returns true with RTL has completed successfully

Tools/autotest/arducopter.py

@@ -356,6 +356,58 @@ def fly_RTL(self, timeout=250):
                 return
         raise AutoTestTimeoutException("Did not get home and disarm")
 
+    def fly_loiter_to_alt(self):
+        """loiter to alt"""
+
+        self.context_push()
+
+        ex = None
+        try:
+
+            self.set_parameter("PLND_ENABLED", 1)
+            self.fetch_parameters()
+            self.set_parameter("PLND_TYPE", 4)
+
+            self.set_parameter("RNGFND_TYPE", 1)
+            self.set_parameter("RNGFND_MIN_CM", 0)
+            self.set_parameter("RNGFND_MAX_CM", 4000)
+            self.set_parameter("RNGFND_PIN", 0)
+            self.set_parameter("RNGFND_SCALING", 12.12)
+
+            self.reboot_sitl()
+
+            global num_wp
+            num_wp = self.load_mission("copter_loiter_to_alt.txt")
+            if not num_wp:
+                self.progress("load copter_loiter_to_target failed")
+                raise NotAchievedException()
+
+            self.mavproxy.send('switch 5\n')
+            self.wait_mode('LOITER')
+
+            self.wait_ready_to_arm()
+
+            self.arm_vehicle()
+
+            self.mavproxy.send('mode auto\n')
+            self.wait_mode('AUTO')
+
+            self.set_rc(3, 1550)
+
+            self.wait_current_waypoint(2)
+
+            self.set_rc(3, 1500)
+
+            self.mav.motors_disarmed_wait()
+        except Exception as e:
+            ex = e
+
+        self.context_pop()
+        self.reboot_sitl()
+
+        if ex is not None:
+            raise ex
+
     def fly_throttle_failsafe(self, side=60, timeout=180):
         """Fly east, Failsafe, return, land."""
 
@@ -1801,12 +1853,15 @@ def autotest(self):
             self.run_test("Test submode change",
                           self.fly_guided_change_submode)
 
+            self.run_test("Loiter-To-Alt", self.fly_loiter_to_alt)
+
             self.progress("Waiting for location")
             self.homeloc = self.mav.location()
             self.progress("Home location: %s" % self.homeloc)
             self.mavproxy.send('switch 6\n')  # stabilize mode
             self.mav.wait_heartbeat()
             self.wait_mode('STABILIZE')
+            self.set_rc(3, 1000)
             self.progress("Waiting reading for arm")
             self.wait_ready_to_arm()
 

Tools/autotest/common.py

@@ -1074,6 +1074,14 @@ def wait_location(self,
                 return True
         raise WaitLocationTimeout("Failed to attain location")
 
+    def wait_current_waypoint(self, wpnum, timeout=60):
+        tstart = self.get_sim_time()
+        while self.get_sim_time() < tstart + timeout:
+            seq = self.mav.waypoint_current()
+            self.progress("Waiting for wp=%u current=%u" % (wpnum, seq))
+            if seq == wpnum:
+                break;
+
     def wait_waypoint(self,
                       wpnum_start,
                       wpnum_end,

Tools/autotest/copter_loiter_to_alt.txt

@@ -0,0 +1,6 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.362938	149.165085	584.000000	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.362881	149.165222	5.000000	1
+2	0	3	31	0.000000	0.000000	0.000000	0.000000	-35.362989	149.165061	30.00000	1
+3	0	10	31	0.000000	0.000000	0.000000	1.000000	0.000000	0.000000	2.000000	1
+4	0	0	20	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1

libraries/AC_WPNav/AC_WPNav.h

@@ -142,6 +142,11 @@ class AC_WPNav
     /// reached_destination - true when we have come within RADIUS cm of the waypoint
     bool reached_wp_destination() const { return _flags.reached_destination; }
 
+    // reached_wp_destination_xy - true if within RADIUS_CM of waypoint in x/y
+    bool reached_wp_destination_xy() const {
+        return get_wp_distance_to_destination() < _wp_radius_cm;
+    }
+
     /// set_fast_waypoint - set to true to ignore the waypoint radius and consider the waypoint 'reached' the moment the intermediate point reaches it
     void set_fast_waypoint(bool fast) { _flags.fast_waypoint = fast; }