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 4eedb2e948
commit a436ca3cee
1 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/mode.cpp
View File

@ -761,6 +761,7 @@ void Mode::land_run_normal_or_precland(bool pause_descent)
// The passed in location is expected to be NED and in m
void Mode::precland_retry_position(const Vector3f &retry_pos)
{
float target_yaw_rate = 0;
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){
AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
@ -787,6 +788,11 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
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};
@ -800,8 +806,14 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
const Vector3f thrust_vector{pos_control->get_thrust_vector()};
// roll, pitch from position controller, yaw heading from auto_heading()
attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.yaw());
// 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());
}
}
// Run precland statemachine. This function should be called from any mode that wants to do precision landing.