diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 16727a4e70..f7cedb56d5 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -8,8 +8,20 @@ bool Copter::ModeLoiter::init(bool ignore_checks) { if (copter.position_ok() || ignore_checks) { + if (!copter.failsafe.radio) { + float target_roll, target_pitch; + // apply SIMPLE mode transform to pilot inputs + update_simple_mode(); - // set target to current position + // convert pilot input to lean angles + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + + // process pilot's roll and pitch input + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + } else { + // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason + loiter_nav->clear_pilot_desired_acceleration(); + } loiter_nav->init_target(); // initialise position and desired velocity @@ -19,7 +31,7 @@ bool Copter::ModeLoiter::init(bool ignore_checks) } return true; - }else{ + } else { return false; } }