// verify_land - returns true if landing has been completed
bool Copter::ModeAuto::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
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;
}
// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
void Copter::ModeRTL::run(bool disarm_on_land)
{
// check if we need to move to next state
if (_state_complete) {
switch (_state) {
case RTL_InitialClimb:
return_start();
break;
case RTL_ReturnHome:
loiterathome_start();
break;
case RTL_LoiterAtHome:
if (rtl_path.land || copter.failsafe.radio) {
land_start();
}else{
descent_start();
}
break;
case RTL_FinalDescent:
// do nothing
break;
case RTL_Land:
// do nothing - rtl_land_run will take care of disarming motors
break;
}
}
// call the correct run function
switch (_state) {
case RTL_InitialClimb:
climb_return_run();
break;
case RTL_ReturnHome:
climb_return_run();
break;
case RTL_LoiterAtHome:
loiterathome_run();
break;
case RTL_FinalDescent:
descent_run();
break;
case RTL_Land:
land_run(disarm_on_land);
break;
}
}
// do_land - initiate landing procedure
void Copter::ModeAuto::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);
wp_start(target_loc);
}else{
// set landing state
land_state = LandStateType_Descending;
// initialise landing controller
land_start();
}
}
void Copter::ModeSmartRTL::pre_land_position_run()
{
// if we are close to 2m above start point, we are ready to land.
if (wp_nav->reached_wp_destination()) {
// choose descend and hold, or land based on user parameter rtl_alt_final
if (g.rtl_alt_final <= 0 || copter.failsafe.radio) {
land_start();
smart_rtl_state = SmartRTL_Land;
} else {
set_descent_target_alt(copter.g.rtl_alt_final);
descent_start();
smart_rtl_state = SmartRTL_Descend;
}
}
// update controllers
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
}
