From fd9423b2549167faab670113c5f2fa16c6a257d3 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 19 Feb 2024 23:47:27 +0000 Subject: [PATCH] Sub: `euler_rate_to_ang_vel` takes Quaternion attitude --- ArduSub/mode.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp index 29d314d72a..d445dd5c8c 100644 --- a/ArduSub/mode.cpp +++ b/ArduSub/mode.cpp @@ -249,7 +249,7 @@ void Mode::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int1 } // convert earth-frame level rates to body-frame level rates - attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level); + attitude_control->euler_rate_to_ang_vel(attitude_control->get_attitude_target_quat(), rate_ef_level, rate_bf_level); // combine earth frame rate corrections with rate requests if (g.acro_trainer == ACRO_TRAINER_LIMITED) {