Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Do not allow automatic yaw while prec land retry

This commit is contained in:
Rishabh 2022-05-19 10:58:48 +05:30 committed by Randy Mackay
parent e1d7b81f9d
commit 1e7203616b
1 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/mode.cpp
View File

@ -764,6 +764,7 @@ void Mode::land_run_normal_or_precland(bool pause_descent)
// The passed in location is expected to be NED and in m // The passed in location is expected to be NED and in m
void Mode::precland_retry_position(const Vector3f &retry_pos) void Mode::precland_retry_position(const Vector3f &retry_pos)
{ {
float target_yaw_rate = 0;
if (!copter.failsafe.radio) { if (!copter.failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
@ -790,6 +791,11 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
copter.ap.land_repo_active = true; copter.ap.land_repo_active = true;
} }
} }
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
} }
Vector3p retry_pos_NEU{retry_pos.x, retry_pos.y, retry_pos.z * -1.0f}; Vector3p retry_pos_NEU{retry_pos.x, retry_pos.y, retry_pos.z * -1.0f};
@ -803,8 +809,14 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
const Vector3f thrust_vector{pos_control->get_thrust_vector()}; const Vector3f thrust_vector{pos_control->get_thrust_vector()};
// roll, pitch from position controller, yaw heading from auto_heading() // call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(thrust_vector, target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.yaw()); attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.yaw());
}
} }
// Run precland statemachine. This function should be called from any mode that wants to do precision landing. // Run precland statemachine. This function should be called from any mode that wants to do precision landing.