// verify_land - returns true if landing has been completed bool Copter::verify_land() { bool retval = false; switch (land_state) { case LandStateType_FlyToLocation: // check if we've reached the location if (wp_nav.reached_wp_destination()) { // get destination so we can use it for loiter target Vector3f dest = wp_nav.get_wp_destination(); // initialise landing controller auto_land_start(dest); // advance to next state land_state = LandStateType_Descending; } break; case LandStateType_Descending: // rely on THROTTLE_LAND mode to correctly update landing status retval = ap.land_complete; break; default: // this should never happen // TO-DO: log an error retval = true; break; } // true is returned if we've successfully landed return retval; }
// do_land - initiate landing procedure void Copter::do_land(const AP_Mission::Mission_Command& cmd) { // To-Do: check if we have already landed // 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; // 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); } auto_wp_start(target_loc); }else{ // set landing state land_state = LAND_STATE_DESCENDING; // initialise landing controller auto_land_start(); } }
// auto_land_start - initialises controller to implement a landing void Copter::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); }
// do_land - initiate landing procedure void Copter::do_land(const AP_Mission::Mission_Command& cmd) { // To-Do: check if we have already landed // 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 = LandStateType_FlyToLocation; Location_Class target_loc = terrain_adjusted_location(cmd); auto_wp_start(target_loc); }else{ // set landing state land_state = LandStateType_Descending; // initialise landing controller auto_land_start(); } }
// do_land - initiate landing procedure void Copter::do_land(const AP_Mission::Mission_Command& cmd) { // To-Do: check if we have already landed // 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; // calculate and set desired location above landing target Vector3f pos = pv_location_to_vector(cmd.content.location); pos.z = inertial_nav.get_altitude(); auto_wp_start(pos); }else{ // set landing state land_state = LAND_STATE_DESCENDING; // initialise landing controller auto_land_start(); } }