From 289aba4350992f50b9600d707164d9dc9f8b9ee5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 17 Nov 2016 16:19:22 +1100 Subject: [PATCH] Copter: support for NAV_CMD_PLACE --- ArduCopter/Copter.h | 22 ++++ ArduCopter/commands_logic.cpp | 196 +++++++++++++++++++++++++++++++--- ArduCopter/control_auto.cpp | 120 +++++++++++++++++++++ ArduCopter/defines.h | 17 ++- 4 files changed, 340 insertions(+), 15 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7a36646da8..e7f16f8e97 100644 --- a/ArduCopter/Copter.h +++ b/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); diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 281f61119c..05738e21ef 100644 --- a/ArduCopter/commands_logic.cpp +++ b/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 +#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) { diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index cad44bf50f..fd3b6b4a90 100644 --- a/ArduCopter/control_auto.cpp +++ b/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(); +} diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 1033cd21fc..6f59fca62d 100644 --- a/ArduCopter/defines.h +++ b/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,