From 9304fa0f52408089829a9dda901380614354d668 Mon Sep 17 00:00:00 2001 From: GH6947 Date: Tue, 24 May 2022 16:24:05 +0300 Subject: [PATCH] Sub: roll pitch comments In ArduSub modes Auto & Guided, direct (not translated to forward/lateral) Roll and Pitch values come from the RC channels, not from wp controller. The existing comments are leftovers from ArduCopter --- ArduSub/control_auto.cpp | 6 +++--- ArduSub/control_guided.cpp | 12 ++++++------ 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 4ee719f7c6..4860cbc15c 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -162,10 +162,10 @@ void Sub::auto_wp_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { - // roll & pitch from waypoint controller, yaw rate from pilot + // roll & pitch & yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } else { - // roll, pitch from waypoint controller, yaw heading from auto_heading() + // roll, pitch from pilot, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true); } } @@ -339,7 +339,7 @@ void Sub::auto_loiter_run() float target_roll, target_pitch; get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); - // roll & pitch from waypoint controller, yaw rate from pilot + // roll & pitch & yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index bcab8732d9..0a444d9a08 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -315,10 +315,10 @@ void Sub::guided_pos_control_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { - // roll & pitch from waypoint controller, yaw rate from pilot + // roll & pitch & yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { - // roll, pitch from waypoint controller, yaw heading from auto_heading() + // roll, pitch from pilot, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); } } @@ -371,10 +371,10 @@ void Sub::guided_vel_control_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { - // roll & pitch from waypoint controller, yaw rate from pilot + // roll & pitch & yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { - // roll, pitch from waypoint controller, yaw heading from auto_heading() + // roll, pitch from pilot, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); } } @@ -437,10 +437,10 @@ void Sub::guided_posvel_control_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { - // roll & pitch from waypoint controller, yaw rate from pilot + // roll & pitch & yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { - // roll, pitch from waypoint controller, yaw heading from auto_heading() + // roll, pitch from pilot, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); } }