From b5efc52428538a6f444cfec4c74142ffcd6e7eba Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 2 Apr 2022 18:13:06 +1030 Subject: [PATCH 1/6] Copter: Clarify get_pilot_desired_lean_angles and fix limit --- ArduCopter/mode.cpp | 37 ++++++++++++++++--------------------- 1 file changed, 16 insertions(+), 21 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 23849ed3139e9..5c5c71a63ae9d 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -405,33 +405,28 @@ void Mode::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd pitch_out_cd = 0.0; return; } - // fetch roll and pitch stick positions - float thrust_angle_x_cd = - channel_pitch->get_control_in(); - float thrust_angle_y_cd = channel_roll->get_control_in(); - // limit max lean angle - angle_limit_cd = constrain_float(angle_limit_cd, 1000.0f, angle_max_cd); + float rc_2_rad = radians(angle_max_cd * 0.01) / (float)ROLL_PITCH_YAW_INPUT_MAX; - // scale roll and pitch inputs to +- angle_max - float scaler = angle_max_cd/(float)ROLL_PITCH_YAW_INPUT_MAX; - thrust_angle_x_cd *= scaler; - thrust_angle_y_cd *= scaler; + // fetch roll and pitch stick positions and convert them to normalised horizontal thrust + Vector2f thrust; + thrust.x = - tanf(rc_2_rad * channel_pitch->get_control_in()); + thrust.y = tanf(rc_2_rad * channel_roll->get_control_in()); - // convert square mapping to circular mapping with maximum magnitude of angle_limit - float total_in = norm(thrust_angle_x_cd, thrust_angle_y_cd); - if (total_in > angle_limit_cd) { - float ratio = angle_limit_cd / total_in; - thrust_angle_x_cd *= ratio; - thrust_angle_y_cd *= ratio; - } + // calculate the horizontal thrust limit based on the angle limit + angle_limit_cd = constrain_float(angle_limit_cd, 1000.0f, angle_max_cd); + float thrust_limit = tanf(radians(angle_limit_cd * 0.01)); - // thrust_angle_x and thrust_angle_y represents a level body frame thrust vector in the - // direction of [thrust_angle_x, thrust_angle_y] and a magnitude - // tan(mag([thrust_angle_x, thrust_angle_y])) * 9.81 * aircraft mass. + // apply horizontal thrust limit + thrust.limit_length(thrust_limit); // Conversion from angular thrust vector to euler angles. - roll_out_cd = (18000/M_PI) * atanf(cosf(thrust_angle_x_cd*(M_PI/18000))*tanf(thrust_angle_y_cd*(M_PI/18000))); - pitch_out_cd = - thrust_angle_x_cd; + float pitch_rad = - atanf(thrust.x); + float roll_rad = atanf(cosf(pitch_rad) * thrust.y); + + // Convert to centi-degrees + roll_out_cd = degrees(roll_rad) * 100.0; + pitch_out_cd = degrees(pitch_rad) * 100.0; } // transform pilot's roll or pitch input into a desired velocity From b60e3bb6c26e6f8fe161547c434f3097af4b587d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 12 Apr 2022 09:31:34 +0930 Subject: [PATCH 2/6] AP_Math: Move rc_input_to_roll_pitch conversion --- libraries/AP_Math/control.cpp | 39 +++++++++++++++++++++++++++++------ libraries/AP_Math/control.h | 15 ++++++++------ 2 files changed, 42 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index 03adce568b6a3..6072281d65560 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -508,18 +508,45 @@ float input_expo(float input, float expo) return input; } -/* - convert a maximum lean angle in degrees to an accel limit in m/s/s - */ +// angle_to_accel converts a maximum lean angle in degrees to an accel limit in m/s/s float angle_to_accel(float angle_deg) { return GRAVITY_MSS * tanf(radians(angle_deg)); } -/* - convert a maximum accel in m/s/s to a lean angle in degrees - */ +// accel_to_angle converts a maximum accel in m/s/s to a lean angle in degrees float accel_to_angle(float accel) { return degrees(atanf((accel/GRAVITY_MSS))); } + +// rc_input_to_roll_pitch - transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command +// roll_in_unit and pitch_in_unit - are normalised roll and pitch stick input +// angle_max_deg - maximum lean angle from the z axis +// angle_limit_deg - provides the ability to reduce the maximum output lean angle to less than angle_max_deg +// returns roll and pitch angle in degrees +void rc_input_to_roll_pitch(float &roll_out_deg, float &pitch_out_deg, float roll_in_unit, float pitch_in_unit, float angle_max_deg, float angle_limit_deg) +{ + angle_max_deg = MIN(angle_max_deg, 85.0); + float rc_2_rad = radians(angle_max_deg); + + // fetch roll and pitch stick positions and convert them to normalised horizontal thrust + Vector2f thrust; + thrust.x = - tanf(rc_2_rad * pitch_in_unit); + thrust.y = tanf(rc_2_rad * roll_in_unit); + + // calculate the horizontal thrust limit based on the angle limit + angle_limit_deg = constrain_float(angle_limit_deg, 10.0f, angle_max_deg); + float thrust_limit = tanf(radians(angle_limit_deg)); + + // apply horizontal thrust limit + thrust.limit_length(thrust_limit); + + // Conversion from angular thrust vector to euler angles. + float pitch_rad = - atanf(thrust.x); + float roll_rad = atanf(cosf(pitch_rad) * thrust.y); + + // Convert to centi-degrees + roll_out_deg = degrees(roll_rad); + pitch_out_deg = degrees(pitch_rad); +} diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h index ecec883200c71..397c73257e825 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -146,12 +146,15 @@ float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float m // The expo should be less than 1.0 but limited to be less than 0.95. float input_expo(float input, float expo); -/* - convert a maximum lean angle in degrees to an accel limit in m/s/s - */ +// angle_to_accel converts a maximum lean angle in degrees to an accel limit in m/s/s float angle_to_accel(float angle_deg); -/* - convert a maximum accel in m/s/s to a lean angle in degrees - */ +// accel_to_angle converts a maximum accel in m/s/s to a lean angle in degrees float accel_to_angle(float accel); + +// rc_input_to_roll_pitch - transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command +// roll_in_unit and pitch_in_unit - are normalised roll and pitch stick inputs +// angle_max_deg - maximum lean angle from the z axis +// angle_limit_deg - provides the ability to reduce the maximum output lean angle to less than angle_max_deg +// returns roll and pitch euler angles in degrees +void rc_input_to_roll_pitch(float &roll_out_deg, float &pitch_out_deg, float roll_in_unit, float pitch_in_unit, float angle_max_deg, float angle_limit_deg); From a3c0ff9235c4ab7bb8c059df4f023a0ab897d3de Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 12 Apr 2022 09:31:50 +0930 Subject: [PATCH 3/6] Copter: AP_Math: Move rc_input_to_roll_pitch conversion to AP_Math --- ArduCopter/mode.cpp | 25 ++++++------------------- 1 file changed, 6 insertions(+), 19 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 5c5c71a63ae9d..beb5d801f34c8 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -406,27 +406,14 @@ void Mode::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd return; } - float rc_2_rad = radians(angle_max_cd * 0.01) / (float)ROLL_PITCH_YAW_INPUT_MAX; - - // fetch roll and pitch stick positions and convert them to normalised horizontal thrust - Vector2f thrust; - thrust.x = - tanf(rc_2_rad * channel_pitch->get_control_in()); - thrust.y = tanf(rc_2_rad * channel_roll->get_control_in()); - - // calculate the horizontal thrust limit based on the angle limit - angle_limit_cd = constrain_float(angle_limit_cd, 1000.0f, angle_max_cd); - float thrust_limit = tanf(radians(angle_limit_cd * 0.01)); - - // apply horizontal thrust limit - thrust.limit_length(thrust_limit); - - // Conversion from angular thrust vector to euler angles. - float pitch_rad = - atanf(thrust.x); - float roll_rad = atanf(cosf(pitch_rad) * thrust.y); + //transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command + float roll_out_deg; + float pitch_out_deg; + rc_input_to_roll_pitch(roll_out_deg, pitch_out_deg, channel_roll->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), channel_pitch->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), angle_max_cd * 0.01, angle_limit_cd * 0.01); // Convert to centi-degrees - roll_out_cd = degrees(roll_rad) * 100.0; - pitch_out_cd = degrees(pitch_rad) * 100.0; + roll_out_cd = roll_out_deg * 100.0; + pitch_out_cd = pitch_out_deg * 100.0; } // transform pilot's roll or pitch input into a desired velocity From 12ef3a6f798f1db22f59f27b224f95222e9f6c41 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 9 Apr 2022 21:57:32 -0600 Subject: [PATCH 4/6] AP_Math: add unit test for rc_input_to_roll_pitch --- .../tests/test_rc_in_to_roll_pitch.cpp | 91 +++++++++++++++++++ 1 file changed, 91 insertions(+) create mode 100644 libraries/AP_Math/tests/test_rc_in_to_roll_pitch.cpp diff --git a/libraries/AP_Math/tests/test_rc_in_to_roll_pitch.cpp b/libraries/AP_Math/tests/test_rc_in_to_roll_pitch.cpp new file mode 100644 index 0000000000000..7c8a87e3f19a7 --- /dev/null +++ b/libraries/AP_Math/tests/test_rc_in_to_roll_pitch.cpp @@ -0,0 +1,91 @@ +#include + +#include + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +const float angle_max_deg = 60; +const float angle_limit_deg = 60; +float roll_out_deg, pitch_out_deg; + +// Test the corners of the input range +TEST(RC2RPTest, Corners) { + // (-1,-1), (-1,1), (1,-1), (1,1) + float roll_in[] = {-1, -1, 1, 1}; + float pitch_in[] = {-1, 1, -1, 1}; + float rc = 37.761f; // roll at 60 deg max/limit + float pc = 50.768f; // pitch at 60 deg max/limit + float roll_val_deg[] = {-rc, -rc, rc, rc}; + float pitch_val_deg[] = {-pc, pc, -pc, pc}; + + for (uint i=0; i Date: Tue, 19 Apr 2022 16:40:51 -0600 Subject: [PATCH 5/6] Copter: change arg order for rc_input_to_roll_pitch --- ArduCopter/mode.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index beb5d801f34c8..eed383dff03ad 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -409,7 +409,9 @@ void Mode::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd //transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command float roll_out_deg; float pitch_out_deg; - rc_input_to_roll_pitch(roll_out_deg, pitch_out_deg, channel_roll->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), channel_pitch->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), angle_max_cd * 0.01, angle_limit_cd * 0.01); + float roll_in = channel_roll->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX); + float pitch_in = channel_pitch->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX); + rc_input_to_roll_pitch(roll_in, pitch_in, angle_max_cd * 0.01, angle_limit_cd * 0.01, roll_out_deg, pitch_out_deg); // Convert to centi-degrees roll_out_cd = roll_out_deg * 100.0; From 9fe150ceee60588e88af0b230c36e6ed73af105d Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 19 Apr 2022 16:43:45 -0600 Subject: [PATCH 6/6] AP_Math: use non-Euler stick mapping in rc_input_to_roll_pitch and update unit test --- libraries/AP_Math/control.cpp | 57 +++++++++++----- libraries/AP_Math/control.h | 4 +- .../tests/test_rc_in_to_roll_pitch.cpp | 67 +++++++++++-------- 3 files changed, 83 insertions(+), 45 deletions(-) diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index 6072281d65560..e52f59f5be241 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -520,33 +520,58 @@ float accel_to_angle(float accel) return degrees(atanf((accel/GRAVITY_MSS))); } +void axisAngle2rollPitch(Vector3f k, float angle, float &roll_rad, float & pitch_rad) { + float a = cosf(angle); + float sa = sinf(angle); + float zva = k.z * (1 - a); + + // 3rd column of corresponding rotation matrix + float r31 = k.x * zva - k.y * sa; + float r32 = k.y * zva + k.x * sa; + float r33 = k.z * zva + a; + + pitch_rad = -asinf(r31); + roll_rad = atan2f(r32, r33); +} + // rc_input_to_roll_pitch - transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command // roll_in_unit and pitch_in_unit - are normalised roll and pitch stick input // angle_max_deg - maximum lean angle from the z axis // angle_limit_deg - provides the ability to reduce the maximum output lean angle to less than angle_max_deg // returns roll and pitch angle in degrees -void rc_input_to_roll_pitch(float &roll_out_deg, float &pitch_out_deg, float roll_in_unit, float pitch_in_unit, float angle_max_deg, float angle_limit_deg) +void rc_input_to_roll_pitch(float roll_in_unit, float pitch_in_unit, + float angle_max_deg, float angle_limit_deg, + float &roll_out_deg, float &pitch_out_deg) { - angle_max_deg = MIN(angle_max_deg, 85.0); - float rc_2_rad = radians(angle_max_deg); + float angle_max = radians(MIN(angle_max_deg, 89.999)); + + float roll_rad = roll_in_unit * angle_max; + float pitch_rad = pitch_in_unit * angle_max; - // fetch roll and pitch stick positions and convert them to normalised horizontal thrust - Vector2f thrust; - thrust.x = - tanf(rc_2_rad * pitch_in_unit); - thrust.y = tanf(rc_2_rad * roll_in_unit); + // tilt direction + float theta = atan2f(sinf(pitch_rad), -sinf(roll_rad)); - // calculate the horizontal thrust limit based on the angle limit - angle_limit_deg = constrain_float(angle_limit_deg, 10.0f, angle_max_deg); - float thrust_limit = tanf(radians(angle_limit_deg)); + // tilt angle + float tilt = sqrtf(pitch_rad*pitch_rad + roll_rad*roll_rad); - // apply horizontal thrust limit - thrust.limit_length(thrust_limit); + roll_out_deg = 0; + pitch_out_deg = 0; + if (!is_zero(tilt)) { + // limit tilt + tilt = constrain_float(tilt, 0, radians(angle_limit_deg)); - // Conversion from angular thrust vector to euler angles. - float pitch_rad = - atanf(thrust.x); - float roll_rad = atanf(cosf(pitch_rad) * thrust.y); + // get axis for axis-angle rotation + float rx = -cosf(theta); + float ry = sinf(theta); - // Convert to centi-degrees + // axis-angle rotation: tilt * (rx, ry, 0).normalized() + Vector3f k = Vector3f(rx, ry, 0); + k.normalize(); + + // convert axis-angle to Euler roll, pitch + axisAngle2rollPitch(k, tilt, roll_rad, pitch_rad); + } + // Convert to degrees roll_out_deg = degrees(roll_rad); pitch_out_deg = degrees(pitch_rad); } diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h index 397c73257e825..a05814d3afbe1 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -157,4 +157,6 @@ float accel_to_angle(float accel); // angle_max_deg - maximum lean angle from the z axis // angle_limit_deg - provides the ability to reduce the maximum output lean angle to less than angle_max_deg // returns roll and pitch euler angles in degrees -void rc_input_to_roll_pitch(float &roll_out_deg, float &pitch_out_deg, float roll_in_unit, float pitch_in_unit, float angle_max_deg, float angle_limit_deg); +void rc_input_to_roll_pitch(float roll_in_unit, float pitch_in_unit, float angle_max_deg, float angle_limit_deg, float &roll_out_deg, float &pitch_out_deg); + +void axisAngle2rollPitch(Vector3f k, float angle, float &roll_rad, float & pitch_rad); diff --git a/libraries/AP_Math/tests/test_rc_in_to_roll_pitch.cpp b/libraries/AP_Math/tests/test_rc_in_to_roll_pitch.cpp index 7c8a87e3f19a7..0dc4093e80732 100644 --- a/libraries/AP_Math/tests/test_rc_in_to_roll_pitch.cpp +++ b/libraries/AP_Math/tests/test_rc_in_to_roll_pitch.cpp @@ -4,8 +4,8 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); -const float angle_max_deg = 60; -const float angle_limit_deg = 60; +const float angle_max_deg = 80; +const float angle_limit_deg = angle_max_deg; float roll_out_deg, pitch_out_deg; // Test the corners of the input range @@ -13,16 +13,17 @@ TEST(RC2RPTest, Corners) { // (-1,-1), (-1,1), (1,-1), (1,1) float roll_in[] = {-1, -1, 1, 1}; float pitch_in[] = {-1, 1, -1, 1}; - float rc = 37.761f; // roll at 60 deg max/limit - float pc = 50.768f; // pitch at 60 deg max/limit + float rc = 75.998f; // roll at max/limit + float pc = 44.136f; // pitch at max/limit float roll_val_deg[] = {-rc, -rc, rc, rc}; float pitch_val_deg[] = {-pc, pc, -pc, pc}; for (uint i=0; i