Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 17:18:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: move trims to vehicle frame

Browse Source
This commit is contained in:
Willian Galvani 2019-12-09 13:12:12 -03:00 committed by Jacob Walser
parent 9b1e0f0fa3
commit 7e46685986
1 changed files with 26 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduSub/joystick.cpp
View File

@ -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()) {