Copter: guided mode takeoff failure leaves submode unchanged

This commit is contained in:
Randy Mackay 2022-03-16 12:18:37 +09:00
parent fc668b4bca
commit f6166906e0
1 changed files with 6 additions and 5 deletions

View File

@ -112,8 +112,6 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const
// takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
{
guided_mode = SubMode::TakeOff;
// calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain)
int32_t alt_target_cm;
bool alt_target_terrain = false;
@ -139,6 +137,8 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
}
}
guided_mode = SubMode::TakeOff;
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
@ -166,9 +166,10 @@ void ModeGuided::wp_control_start()
// initialise wpnav to stopping point
Vector3f stopping_point;
wp_nav->get_wp_stopping_point(stopping_point);
// no need to check return status because terrain data is not used
wp_nav->set_wp_destination(stopping_point, false);
if (!wp_nav->set_wp_destination(stopping_point, false)) {
// this should never happen because terrain data is not used
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
// initialise yaw
auto_yaw.set_mode_to_default(false);