From f222b8bb59bb4814436600e6053b07ef2c3f8bf7 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Thu, 1 Dec 2022 18:24:51 -0300 Subject: [PATCH] Sub: fix missing _dz in get_pilot_desired_yaw_rate() --- ArduSub/control_auto.cpp | 4 ++-- ArduSub/control_circle.cpp | 2 +- ArduSub/control_guided.cpp | 6 +++--- ArduSub/control_surface.cpp | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 8a5c5effd3..59e344fc22 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -123,7 +123,7 @@ void Sub::auto_wp_run() float target_yaw_rate = 0; if (!failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100; + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()) * 100; if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -313,7 +313,7 @@ void Sub::auto_loiter_run() // accept pilot input of yaw float target_yaw_rate = 0; if (!failsafe.pilot_input) { - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100; + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()) * 100; } // set motors to full range diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index 767135258a..c61f103fc5 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -48,7 +48,7 @@ void Sub::circle_run() // process pilot inputs // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100; + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()) * 100; if (!is_zero(target_yaw_rate)) { circle_pilot_yaw_override = true; } diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 387f5dbf98..43296fd781 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -296,7 +296,7 @@ void Sub::guided_pos_control_run() float target_yaw_rate = 0; if (!failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100; + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()) * 100; if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -349,7 +349,7 @@ void Sub::guided_vel_control_run() float target_yaw_rate = 0; if (!failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100; + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()) * 100; if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -407,7 +407,7 @@ void Sub::guided_posvel_control_run() if (!failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100; + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()) * 100; if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index df11b1d5d8..d8de072809 100644 --- a/ArduSub/control_surface.cpp +++ b/ArduSub/control_surface.cpp @@ -43,7 +43,7 @@ void Sub::surface_run() get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100; + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()) * 100; // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);