// 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;
}
}
void Copter::ModeSmartRTL::run()
{
switch (smart_rtl_state) {
case SmartRTL_WaitForPathCleanup:
wait_cleanup_run();
break;
case SmartRTL_PathFollow:
path_follow_run();
break;
case SmartRTL_PreLandPosition:
pre_land_position_run();
break;
case SmartRTL_Descend:
descent_run(); // Re-using the descend method from normal rtl mode.
break;
case SmartRTL_Land:
land_run(true); // Re-using the land method from normal rtl mode.
break;
}
}
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
void Copter::update_flight_mode()
{
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
switch (control_mode) {
case ACRO:
#if FRAME_CONFIG == HELI_FRAME
heli_acro_run();
#else
acro_run();
#endif
break;
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
heli_stabilize_run();
#else
stabilize_run();
#endif
break;
case ALT_HOLD:
althold_run();
break;
case AUTO:
auto_run();
break;
case CIRCLE:
circle_run();
break;
case LOITER:
loiter_run();
break;
case GUIDED:
guided_run();
break;
case LAND:
land_run();
break;
case RTL:
rtl_run();
break;
case DRIFT:
drift_run();
break;
case SPORT:
sport_run();
break;
case ALT_POS:
altpos_run();
break;
case FLIP:
flip_run();
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
autotune_run();
break;
#endif
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
poshold_run();
break;
#endif
case BRAKE:
brake_run();
break;
case THROW:
throw_run();
break;
case AVOID_ADSB:
avoid_adsb_run();
break;
case GUIDED_NOGPS:
guided_nogps_run();
break;
default:
break;
}
}
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
void Sub::update_flight_mode()
{
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
switch (control_mode) {
case ACRO:
acro_run();
break;
case STABILIZE:
stabilize_run();
break;
case ALT_HOLD:
althold_run();
break;
case AUTO:
auto_run();
break;
case CIRCLE:
circle_run();
break;
case LOITER:
loiter_run();
break;
case GUIDED:
guided_run();
break;
case LAND:
land_run();
break;
case RTL:
rtl_run();
break;
case DRIFT:
drift_run();
break;
case SPORT:
sport_run();
break;
case FLIP:
flip_run();
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
autotune_run();
break;
#endif
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
poshold_run();
break;
#endif
case BRAKE:
brake_run();
break;
case THROW:
throw_run();
break;
}
}
