aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: SmartRTL add pilot yaw control

Browse Source
This commit is contained in:
Sarthak Bhagat 2018-04-30 02:18:43 +05:30 committed by Randy Mackay
parent 767f81265b
commit 0bd21b0b3f
1 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduCopter/mode_smart_rtl.cpp
View File

@ -75,6 +75,15 @@ void Copter::ModeSmartRTL::wait_cleanup_run()
void Copter::ModeSmartRTL::path_follow_run() void Copter::ModeSmartRTL::path_follow_run()
{ {
float target_yaw_rate = 0.0f;
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// if we are close to current target point, switch the next point to be our target. // if we are close to current target point, switch the next point to be our target.
if (wp_nav->reached_wp_destination()) { if (wp_nav->reached_wp_destination()) {
Vector3f next_point; Vector3f next_point;
@ -103,7 +112,7 @@ void Copter::ModeSmartRTL::path_follow_run()
// call attitude controller // call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) { if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
} else { } else {
// roll, pitch from waypoint controller, yaw heading from auto_heading() // roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true); attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);