From 7e46685986e69291376f3800c03969890e31f6a6 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 9 Dec 2019 13:12:12 -0300 Subject: [PATCH] Sub: move trims to vehicle frame --- ArduSub/joystick.cpp | 30 ++++++++++++++++++++++++++---- 1 file changed, 26 insertions(+), 4 deletions(-) diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 68641edff0..f446418c12 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -141,6 +141,16 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held) { + // Used for trimming level in vehicle frame + Quaternion attitudeTarget; + attitudeTarget.from_euler( + radians(last_roll * 0.01f), + radians(last_pitch * 0.01f), + radians(last_yaw * 0.01f) + ); + Vector3f localPitch = Vector3f(0, 1, 0); + Vector3f localRoll = Vector3f(1, 0, 0); + // Act based on the function assigned to this button switch (get_button(button)->function(shift)) { case JSButton::button_function_t::k_arm_toggle: @@ -331,16 +341,28 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held) } break; case JSButton::button_function_t::k_trim_roll_inc: - rollTrim = constrain_float(rollTrim+10,-200,200); + attitudeTarget.rotate(localRoll * radians(1)); + last_roll = degrees(attitudeTarget.get_euler_roll()) * 100; + last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100; + last_yaw = degrees(attitudeTarget.get_euler_yaw()) * 100; break; case JSButton::button_function_t::k_trim_roll_dec: - rollTrim = constrain_float(rollTrim-10,-200,200); + attitudeTarget.rotate(localRoll * radians(-1)); + last_roll = degrees(attitudeTarget.get_euler_roll()) * 100; + last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100; + last_yaw = degrees(attitudeTarget.get_euler_yaw()) * 100; break; case JSButton::button_function_t::k_trim_pitch_inc: - pitchTrim = constrain_float(pitchTrim+10,-200,200); + attitudeTarget.rotate(localPitch * radians(1)); + last_roll = degrees(attitudeTarget.get_euler_roll()) * 100; + last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100; + last_yaw = degrees(attitudeTarget.get_euler_yaw()) * 100; break; case JSButton::button_function_t::k_trim_pitch_dec: - pitchTrim = constrain_float(pitchTrim-10,-200,200); + attitudeTarget.rotate(localPitch * radians(-1)); + last_roll = degrees(attitudeTarget.get_euler_roll()) * 100; + last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100; + last_yaw = degrees(attitudeTarget.get_euler_yaw()) * 100; break; case JSButton::button_function_t::k_input_hold_set: if(!motors.armed()) {