diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index acf8bc32a5..bdccb9f66a 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -312,7 +312,6 @@ private: int32_t home_distance; // distance between plane and next waypoint in cm. uint32_t wp_distance; - uint8_t land_state; // records state of land (flying to location, descending) // Auto AutoMode auto_mode; // controls which auto controller is run @@ -605,9 +604,6 @@ private: void auto_wp_start(const Location_Class& dest_loc); void auto_wp_run(); void auto_spline_run(); - void auto_land_start(); - void auto_land_start(const Vector3f& destination); - void auto_land_run(); void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m); void auto_circle_start(); void auto_circle_run(); @@ -789,7 +785,7 @@ private: bool do_guided(const AP_Mission::Mission_Command& cmd); void do_nav_wp(const AP_Mission::Mission_Command& cmd); - void do_land(const AP_Mission::Mission_Command& cmd); + void do_surface(const AP_Mission::Mission_Command& cmd); 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); @@ -814,6 +810,7 @@ private: void do_gripper(const AP_Mission::Mission_Command& cmd); #endif bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); + bool verify_surface(const AP_Mission::Mission_Command& cmd); bool verify_circle(const AP_Mission::Mission_Command& cmd); bool verify_spline_wp(const AP_Mission::Mission_Command& cmd); #if NAV_GUIDED == ENABLED diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 44b710813a..b584d11548 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -1,5 +1,7 @@ #include "Sub.h" +static enum AutoSurfaceState auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION; + // start_command - this function will be called when the ap_mission lib wishes to start a new command bool Sub::start_command(const AP_Mission::Mission_Command& cmd) { @@ -18,7 +20,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint - do_land(cmd); + do_surface(cmd); break; case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely @@ -174,6 +176,9 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_WAYPOINT: return verify_nav_wp(cmd); + case MAV_CMD_NAV_LAND: + return verify_surface(cmd); + case MAV_CMD_NAV_LOITER_UNLIM: return verify_loiter_unlimited(); @@ -282,39 +287,40 @@ void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd) } } -// do_land - initiate landing procedure -void Sub::do_land(const AP_Mission::Mission_Command& cmd) +// do_surface - initiate surface procedure +void Sub::do_surface(const AP_Mission::Mission_Command& cmd) { - // To-Do: check if we have already landed + Location_Class target_location; // 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 - land_state = LAND_STATE_FLY_TO_LOCATION; + // set state to go to location + auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION; - // calculate and set desired location above landing target + // calculate and set desired location below surface target // convert to location class - Location_Class target_loc(cmd.content.location); + target_location = Location_Class(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); + target_location.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) { // 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); + target_location.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); + target_location.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); } - auto_wp_start(target_loc); } else { - // set landing state - land_state = LAND_STATE_DESCENDING; + // set surface state to ascend + auto_surface_state = AUTO_SURFACE_STATE_ASCEND; - // initialise landing controller - auto_land_start(); + // Set waypoint destination to current location at zero depth + target_location = Location_Class(current_loc.lat, current_loc.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME); } + + // Go to wp location + auto_wp_start(target_location); } // do_loiter_unlimited - start loitering with no end conditions @@ -563,6 +569,44 @@ bool Sub::verify_nav_wp(const AP_Mission::Mission_Command& cmd) } } +// verify_land - returns true if landing has been completed +bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd) +{ + bool retval = false; + + switch (auto_surface_state) { + case AUTO_SURFACE_STATE_GO_TO_LOCATION: + // check if we've reached the location + if (wp_nav.reached_wp_destination()) { + // Set target to current xy and zero depth + // TODO get xy target from current wp destination, because current location may be acceptance-radius away from original destination + Location_Class target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME); + + auto_wp_start(target_location); + + // advance to next state + auto_surface_state = AUTO_SURFACE_STATE_ASCEND; + } + break; + + case AUTO_SURFACE_STATE_ASCEND: + // rely on THROTTLE_LAND mode to correctly update landing status + if (wp_nav.reached_wp_destination()) { + retval = true; + } + break; + + default: + // this should never happen + // TO-DO: log an error + retval = true; + break; + } + + // true is returned if we've successfully surfaced + return retval; +} + bool Sub::verify_loiter_unlimited() { return false; diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 214cbdc474..3b6bb20a8f 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -56,10 +56,6 @@ void Sub::auto_run() auto_wp_run(); break; - case Auto_Land: - auto_land_run(); - break; - case Auto_Circle: auto_circle_run(); break; @@ -249,56 +245,6 @@ void Sub::auto_spline_run() } } -// auto_land_start - initialises controller to implement a landing -void Sub::auto_land_start() -{ - // set target to stopping point - Vector3f stopping_point; - wp_nav.get_loiter_stopping_point_xy(stopping_point); - - // call location specific land start function - auto_land_start(stopping_point); -} - -// auto_land_start - initialises controller to implement a landing -void Sub::auto_land_start(const Vector3f& destination) -{ - auto_mode = Auto_Land; - - // initialise loiter target destination - wp_nav.init_loiter_target(destination); - - // initialise position and desired velocity - 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); -} - -// auto_land_run - lands in auto mode -// called by auto_run at 100hz or more -void Sub::auto_land_run() -{ - // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - - // 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); - - // land mode replaced by surface mode, does not have this functionality - // land_run_horizontal_control(); - // land_run_vertical_control(); -} - // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location // we assume the caller has set the circle's circle with circle_nav.set_center() // we assume the caller has performed all required GPS_ok checks diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 49669fd383..d2219b7ab6 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -13,6 +13,11 @@ #define BOTTOM_DETECTOR_TRIGGER_SEC 1.0 #define SURFACE_DETECTOR_TRIGGER_SEC 1.0 +enum AutoSurfaceState { + AUTO_SURFACE_STATE_GO_TO_LOCATION, + AUTO_SURFACE_STATE_ASCEND +}; + // Autopilot Yaw Mode enumeration enum autopilot_yaw_mode { AUTO_YAW_HOLD = 0, // pilot controls the heading @@ -166,10 +171,6 @@ enum RTLState { RTL_Land }; -// LAND state -#define LAND_STATE_FLY_TO_LOCATION 0 -#define LAND_STATE_DESCENDING 1 - // Logging parameters #define TYPE_AIRSTART_MSG 0x00 #define TYPE_GROUNDSTART_MSG 0x01