diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 23849ed3139e9..eed383dff03ad 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -405,33 +405,17 @@ 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); + //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; + 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); - // 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; - - // 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; - } - - // 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. - - // 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; + // Convert to centi-degrees + 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 diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index 03adce568b6a3..e52f59f5be241 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -508,18 +508,70 @@ 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))); } + +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_in_unit, float pitch_in_unit, + float angle_max_deg, float angle_limit_deg, + float &roll_out_deg, float &pitch_out_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; + + // tilt direction + float theta = atan2f(sinf(pitch_rad), -sinf(roll_rad)); + + // tilt angle + float tilt = sqrtf(pitch_rad*pitch_rad + roll_rad*roll_rad); + + roll_out_deg = 0; + pitch_out_deg = 0; + if (!is_zero(tilt)) { + // limit tilt + tilt = constrain_float(tilt, 0, radians(angle_limit_deg)); + + // get axis for axis-angle rotation + float rx = -cosf(theta); + float ry = sinf(theta); + + // 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 ecec883200c71..a05814d3afbe1 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -146,12 +146,17 @@ 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_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 new file mode 100644 index 0000000000000..0dc4093e80732 --- /dev/null +++ b/libraries/AP_Math/tests/test_rc_in_to_roll_pitch.cpp @@ -0,0 +1,102 @@ +#include + +#include + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +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 +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 = 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