// 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();
}
}