Copter: support for NAV_CMD_PLACE

ArduCopter/Copter.h

@@ -381,6 +381,17 @@ private:
     uint32_t wp_distance;
     LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending)
 
+    struct {
+        PayloadPlaceStateType state = PayloadPlaceStateType_Calibrating_Hover_Start; // records state of place (descending, releasing, released, ...)
+        uint32_t hover_start_timestamp; // milliseconds
+        float hover_throttle_level;
+        uint32_t descend_start_timestamp; // milliseconds
+        uint32_t place_start_timestamp; // milliseconds
+        float descend_throttle_level;
+        float descend_start_altitude;
+        float descend_max; // centimetres
+    } nav_payload_place;
+
     // Auto
     AutoMode auto_mode;   // controls which auto controller is run
 
@@ -777,6 +788,7 @@ private:
     void do_RTL(void);
     bool verify_takeoff();
     bool verify_land();
+    bool verify_payload_place();
     bool verify_loiter_unlimited();
     bool verify_loiter_time();
     bool verify_RTL();
@@ -803,6 +815,14 @@ private:
     void auto_land_start();
     void auto_land_start(const Vector3f& destination);
     void auto_land_run();
+    void do_payload_place(const AP_Mission::Mission_Command& cmd);
+    void auto_payload_place_start();
+    void auto_payload_place_start(const Vector3f& destination);
+    void auto_payload_place_run();
+    bool auto_payload_place_run_should_run();
+    void auto_payload_place_run_loiter();
+    void auto_payload_place_run_descend();
+    void auto_payload_place_run_release();
     void auto_rtl_start();
     void auto_rtl_run();
     void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
@@ -1098,6 +1118,8 @@ private:
     bool verify_command(const AP_Mission::Mission_Command& cmd);
     bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
 
+    Location_Class terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const;
+
     bool do_guided(const AP_Mission::Mission_Command& cmd);
     void do_takeoff(const AP_Mission::Mission_Command& cmd);
     void do_nav_wp(const AP_Mission::Mission_Command& cmd);

ArduCopter/commands_logic.cpp

@@ -25,6 +25,10 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
         do_land(cmd);
         break;
 
+    case MAV_CMD_NAV_PAYLOAD_PLACE:              // 94 place at Waypoint
+        do_payload_place(cmd);
+        break;
+
     case MAV_CMD_NAV_LOITER_UNLIM:              // 17 Loiter indefinitely
         do_loiter_unlimited(cmd);
         break;
@@ -186,7 +190,6 @@ Return true if we do not recognize the command so that we move on to the next co
 bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
 {
     switch(cmd.id) {
-
     //
     // navigation commands
     //
@@ -199,6 +202,9 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_LAND:
         return verify_land();
 
+    case MAV_CMD_NAV_PAYLOAD_PLACE:
+        return verify_payload_place();
+
     case MAV_CMD_NAV_LOITER_UNLIM:
         return verify_loiter_unlimited();
 
@@ -334,6 +340,27 @@ void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
     }
 }
 
+// terrain_adjusted_location: returns a Location with lat/lon from cmd
+// and altitude from our current altitude adjusted for location
+Location_Class Copter::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const
+{
+    // convert to location class
+    Location_Class target_loc(cmd.content.location);
+
+    // decide if we will use terrain following
+    int32_t curr_terr_alt_cm, target_terr_alt_cm;
+    if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
+        target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
+        curr_terr_alt_cm = MAX(curr_terr_alt_cm,200);
+        // if using terrain, set target altitude to current altitude above terrain
+        target_loc.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
+    } else {
+        // set target altitude to current altitude above home
+        target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
+    }
+    return target_loc;
+}
+
 // do_land - initiate landing procedure
 void Copter::do_land(const AP_Mission::Mission_Command& cmd)
 {
@@ -344,20 +371,8 @@ void Copter::do_land(const AP_Mission::Mission_Command& cmd)
         // set state to fly to location
         land_state = LandStateType_FlyToLocation;
 
-        // convert to location class
-        Location_Class target_loc(cmd.content.location);
+        Location_Class target_loc = terrain_adjusted_location(cmd);
 
-        // decide if we will use terrain following
-        int32_t curr_terr_alt_cm, target_terr_alt_cm;
-        if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
-            target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
-            curr_terr_alt_cm = MAX(curr_terr_alt_cm,200);
-            // if using terrain, set target altitude to current altitude above terrain
-            target_loc.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
-        } else {
-            // set target altitude to current altitude above home
-            target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
-        }
         auto_wp_start(target_loc);
     }else{
         // set landing state
@@ -368,6 +383,26 @@ void Copter::do_land(const AP_Mission::Mission_Command& cmd)
     }
 }
 
+// do_payload_place - initiate placing procedure
+void Copter::do_payload_place(const AP_Mission::Mission_Command& cmd)
+{
+    // if location provided we fly to that location at current altitude
+    if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
+        // set state to fly to location
+        nav_payload_place.state = PayloadPlaceStateType_FlyToLocation;
+
+        Location_Class target_loc = terrain_adjusted_location(cmd);
+
+        auto_wp_start(target_loc);
+    } else {
+        nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
+
+        // initialise placing controller
+        auto_payload_place_start();
+    }
+    nav_payload_place.descend_max = cmd.p1;
+}
+
 // do_loiter_unlimited - start loitering with no end conditions
 // note: caller should set yaw_mode
 void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
@@ -650,6 +685,139 @@ bool Copter::verify_land()
     return retval;
 }
 
+#define NAV_PAYLOAD_PLACE_DEBUGGING 0
+
+#if NAV_PAYLOAD_PLACE_DEBUGGING
+#include <stdio.h>
+#define debug(fmt, args ...)  do {::fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
+#else
+#define debug(fmt, args ...)
+#endif
+
+// verify_payload_place - returns true if placing has been completed
+bool Copter::verify_payload_place()
+{
+    const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds
+    const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds
+    const float hover_throttle_placed_fraction = 0.8; // i.e. if throttle is less than 80% of hover we have placed
+    const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed
+    const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed
+
+    const float current_throttle_level = motors.get_throttle();
+    const uint32_t now =  AP_HAL::millis();
+    switch (nav_payload_place.state) {
+    case PayloadPlaceStateType_FlyToLocation:
+        if (!wp_nav.reached_wp_destination()) {
+            return false;
+        }
+        // we're there; set loiter target
+        auto_payload_place_start(wp_nav.get_wp_destination());
+        nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
+        // fall through
+    case PayloadPlaceStateType_Calibrating_Hover_Start:
+        // hover for 1 second to get an idea of what our hover
+        // throttle looks like
+        debug("Calibrate start");
+        nav_payload_place.hover_start_timestamp = now;
+        nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover;
+        // fall through
+    case PayloadPlaceStateType_Calibrating_Hover: {
+        if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) {
+            // still calibrating...
+            debug("Calibrate Timer: %d", now - nav_payload_place.hover_start_timestamp);
+            return false;
+        }
+        // we have a valid calibration.  Hopefully.
+        nav_payload_place.hover_throttle_level = current_throttle_level;
+        const float hover_throttle_delta = fabsf(nav_payload_place.hover_throttle_level - motors.get_throttle_hover());
+        gcs_send_text_fmt(MAV_SEVERITY_INFO, "hover throttle delta: %f", hover_throttle_delta);
+        nav_payload_place.state = PayloadPlaceStateType_Descending_Start;
+        // fall through
+    };
+    case PayloadPlaceStateType_Descending_Start:
+        nav_payload_place.descend_start_timestamp = now;
+        nav_payload_place.descend_start_altitude = inertial_nav.get_altitude();
+        nav_payload_place.descend_throttle_level = 0;
+        nav_payload_place.state = PayloadPlaceStateType_Descending;
+        // fall through
+    case PayloadPlaceStateType_Descending:
+        // make sure we don't descend too far:
+        debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()), nav_payload_place.descend_max);
+        if (!is_zero(nav_payload_place.descend_max) &&
+            nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()  > nav_payload_place.descend_max) {
+            nav_payload_place.state = PayloadPlaceStateType_Ascending;
+            gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Reached maximum descent");
+            return false; // we'll do any cleanups required next time through the loop
+        }
+        // see if we've been descending long enough to calibrate a descend-throttle-level:
+        if (nav_payload_place.descend_throttle_level == 0 &&
+            now - nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time) {
+            nav_payload_place.descend_throttle_level = current_throttle_level;
+        }
+        // watch the throttle to determine whether the load has been placed
+        // debug("hover ratio: %f   descend ratio: %f\n", current_throttle_level/nav_payload_place.hover_throttle_level, ((nav_payload_place.descend_throttle_level == 0) ? -1.0f : current_throttle_level/nav_payload_place.descend_throttle_level));
+        if (current_throttle_level/nav_payload_place.hover_throttle_level > hover_throttle_placed_fraction &&
+            (nav_payload_place.descend_throttle_level == 0 ||
+             current_throttle_level/nav_payload_place.descend_throttle_level > descent_throttle_placed_fraction)) {
+            // throttle is above both threshold ratios (or above hover threshold ration and descent threshold ratio not yet valid)
+            nav_payload_place.place_start_timestamp = 0;
+            return false;
+        }
+        if (nav_payload_place.place_start_timestamp == 0) {
+            // we've only just now hit the correct throttle level
+            nav_payload_place.place_start_timestamp = now;
+            return false;
+        } else if (now - nav_payload_place.place_start_timestamp < placed_time) {
+            // keep going down....
+            debug("Place Timer: %d", now - nav_payload_place.place_start_timestamp);
+            return false;
+        }
+        nav_payload_place.state = PayloadPlaceStateType_Releasing_Start;
+        // fall through
+    case PayloadPlaceStateType_Releasing_Start:
+        if (g2.gripper.valid()) {
+            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Releasing the gripper");
+            g2.gripper.release();
+        } else {
+            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Gripper not valid");
+        }
+        nav_payload_place.state = PayloadPlaceStateType_Releasing;
+        // fall through
+    case PayloadPlaceStateType_Releasing:
+        if (g2.gripper.valid() && !g2.gripper.released()) {
+            return false;
+        }
+        nav_payload_place.state = PayloadPlaceStateType_Released;
+        // fall through
+    case PayloadPlaceStateType_Released: {
+        nav_payload_place.state = PayloadPlaceStateType_Ascending_Start;
+    }
+        // fall through
+    case PayloadPlaceStateType_Ascending_Start: {
+        Location_Class target_loc = inertial_nav.get_position();
+        target_loc.alt = nav_payload_place.descend_start_altitude;
+        auto_wp_start(target_loc);
+        nav_payload_place.state = PayloadPlaceStateType_Ascending;
+    }
+        //fall through
+    case PayloadPlaceStateType_Ascending:
+        if (!wp_nav.reached_wp_destination()) {
+            return false;
+        }
+        nav_payload_place.state = PayloadPlaceStateType_Done;
+        // fall through
+    case PayloadPlaceStateType_Done:
+        return true;
+    default:
+        // this should never happen
+        // TO-DO: log an error
+        return true;
+    }
+    // should never get here
+    return true;
+}
+#undef debug
+
 // verify_nav_wp - check if we have reached the next way point
 bool Copter::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 {

ArduCopter/control_auto.cpp

@@ -91,6 +91,10 @@ void Copter::auto_run()
     case Auto_Loiter:
         auto_loiter_run();
         break;
+
+    case Auto_NavPayloadPlace:
+        auto_payload_place_run();
+        break;
     }
 }
 
@@ -746,3 +750,119 @@ float Copter::get_auto_heading(void)
     }
 }
 
+// auto_payload_place_start - initialises controller to implement a placing
+void Copter::auto_payload_place_start()
+{
+    // set target to stopping point
+    Vector3f stopping_point;
+    wp_nav.get_loiter_stopping_point_xy(stopping_point);
+
+    // call location specific place start function
+    auto_payload_place_start(stopping_point);
+
+}
+
+// auto_payload_place_start - initialises controller to implement placement of a load
+void Copter::auto_payload_place_start(const Vector3f& destination)
+{
+    auto_mode = Auto_NavPayloadPlace;
+    nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
+
+    // initialise loiter target destination
+    wp_nav.init_loiter_target(destination);
+
+    // initialise position and desired velocity
+    if (!pos_control.is_active_z()) {
+        pos_control.set_alt_target(inertial_nav.get_altitude());
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    }
+
+    // initialise yaw
+    set_auto_yaw_mode(AUTO_YAW_HOLD);
+}
+
+bool Copter::auto_payload_place_run_should_run()
+{
+    // muts be armed
+    if (!motors.armed()) {
+        return false;
+    }
+    // muts be auto-armed
+    if (!ap.auto_armed) {
+        return false;
+    }
+    // must not be landed
+    if (ap.land_complete) {
+        return false;
+    }
+    // interlock must be enabled (i.e. unsafe)
+    if (!motors.get_interlock()) {
+        return false;
+    }
+
+    return true;
+}
+
+// auto_payload_place_run - places an object in auto mode
+//      called by auto_run at 100hz or more
+void Copter::auto_payload_place_run()
+{
+    if (!auto_payload_place_run_should_run()) {
+#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
+        // call attitude controller
+        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
+        attitude_control.set_throttle_out(0,false,g.throttle_filt);
+#else
+        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
+        // multicopters do not stabilize roll/pitch/yaw when disarmed
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+#endif
+        // set target to current position
+        wp_nav.init_loiter_target();
+        return;
+    }
+
+    // set motors to full range
+    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+
+    switch (nav_payload_place.state) {
+    case PayloadPlaceStateType_FlyToLocation:
+    case PayloadPlaceStateType_Calibrating_Hover_Start:
+    case PayloadPlaceStateType_Calibrating_Hover:
+        return auto_payload_place_run_loiter();
+    case PayloadPlaceStateType_Descending_Start:
+    case PayloadPlaceStateType_Descending:
+        return auto_payload_place_run_descend();
+    case PayloadPlaceStateType_Releasing_Start:
+    case PayloadPlaceStateType_Releasing:
+    case PayloadPlaceStateType_Released:
+    case PayloadPlaceStateType_Ascending_Start:
+    case PayloadPlaceStateType_Ascending:
+    case PayloadPlaceStateType_Done:
+        return auto_payload_place_run_loiter();
+    }
+}
+
+void Copter::auto_payload_place_run_loiter()
+{
+    // loiter...
+    land_run_horizontal_control();
+
+    // run loiter controller
+    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+
+    // call attitude controller
+    const float target_yaw_rate = 0;
+    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
+
+    // update altitude target and call position controller
+    // const float target_climb_rate = 0;
+    // pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
+    pos_control.update_z_controller();
+}
+
+void Copter::auto_payload_place_run_descend()
+{
+    land_run_horizontal_control();
+    land_run_vertical_control();
+}

ArduCopter/defines.h

@@ -202,7 +202,8 @@ enum AutoMode {
     Auto_Circle,
     Auto_Spline,
     Auto_NavGuided,
-    Auto_Loiter
+    Auto_Loiter,
+    Auto_NavPayloadPlace,
 };
 
 // Guided modes
@@ -277,6 +278,20 @@ enum LandStateType {
     LandStateType_Descending = 1
 };
 
+enum PayloadPlaceStateType {
+    PayloadPlaceStateType_FlyToLocation,
+    PayloadPlaceStateType_Calibrating_Hover_Start,
+    PayloadPlaceStateType_Calibrating_Hover,
+    PayloadPlaceStateType_Descending_Start,
+    PayloadPlaceStateType_Descending,
+    PayloadPlaceStateType_Releasing_Start,
+    PayloadPlaceStateType_Releasing,
+    PayloadPlaceStateType_Released,
+    PayloadPlaceStateType_Ascending_Start,
+    PayloadPlaceStateType_Ascending,
+    PayloadPlaceStateType_Done,
+};
+
 // bit options for DEV_OPTIONS parameter
 enum DevOptions {
     DevOptionADSBMAVLink = 1,