Pr lean axis angle

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

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);
+}

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);

libraries/AP_Math/tests/test_rc_in_to_roll_pitch.cpp

@@ -0,0 +1,102 @@
+#include <AP_gtest.h>
+
+#include <AP_Math/AP_Math.h>
+
+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<ARRAY_SIZE(roll_val_deg); i++) {
+        rc_input_to_roll_pitch(roll_in[i], pitch_in[i], angle_max_deg, angle_limit_deg, roll_out_deg, pitch_out_deg);
+        // printf("{%.3f, %.3f, %.3f, %.3f},\n", roll_in[i], pitch_in[i], roll_out_deg, pitch_out_deg);
+
+        EXPECT_TRUE(fabsf(roll_out_deg  - roll_val_deg[i])  < .02f);
+        EXPECT_TRUE(fabsf(pitch_out_deg - pitch_val_deg[i]) < .02f);
+    }
+}
+
+// Test some points on the axes
+TEST(RC2RPTest, Axes) {
+    // (0,-1), (0,1), (1,0), (-1,1)
+    float roll_in[]  = { 0,  0,  0, -1,  0.5,  0.0, 1};
+    float pitch_in[] = {-1,  1,  0,  0,  0.0,  0.5, 0};
+    float roll_val_deg[] = {0,
+                            0,
+                            0, 
+                            -angle_max_deg,
+                            angle_max_deg/2,
+                            0,
+                            angle_max_deg};
+    float pitch_val_deg[] = {-angle_max_deg,
+                             angle_max_deg, 
+                             0,
+                             0,
+                             0,
+                             angle_max_deg/2,
+                             0};
+
+    for (uint i=0; i<ARRAY_SIZE(roll_val_deg); i++) {
+        rc_input_to_roll_pitch(roll_in[i], pitch_in[i], angle_max_deg, angle_limit_deg, roll_out_deg, pitch_out_deg);
+        // printf("{%.3f, %.3f, %.3f, %.3f},\n", roll_in[i], pitch_in[i], roll_out_deg, pitch_out_deg);
+
+        EXPECT_TRUE(fabsf(roll_out_deg  - roll_val_deg[i])  < .001f);
+        EXPECT_TRUE(fabsf(pitch_out_deg - pitch_val_deg[i]) < .001f);
+    }
+
+}
+
+// Test some points on the circle at 60 degrees
+TEST(RC2RPTest, Circle) {
+
+    // values generated by VPython implementation; roll/pitch deltas are < .02 degrees w.r.t C++ code
+    float xy_rp[][4] = {
+        {1.000, 0.000, 80.000, -0.000},
+        {0.924, 0.383, 78.712, 27.480},
+        {0.707, 0.707, 75.981, 44.134},
+        {0.383, 0.924, 69.378, 60.460},
+        {0.000, 1.000, 0.000, 80.000},
+        {-0.383, 0.924, -69.378, 60.460},
+        {-0.707, 0.707, -75.981, 44.134},
+        {-0.924, 0.383, -78.712, 27.480},
+        {-1.000, 0.000, -80.000, 0.000},
+        {-0.924, -0.383, -78.712, -27.480},
+        {-0.707, -0.707, -75.981, -44.134},
+        {-0.383, -0.924, -69.378, -60.460},
+        {-0.000, -1.000, 0.000, -80.000},
+        {0.383, -0.924, 69.378, -60.460},
+        {0.707, -0.707, 75.981, -44.134},
+        {0.924, -0.383, 78.712, -27.480}
+    };
+
+#if false
+    for (uint row=0; row<ARRAY_SIZE(xy_rp); row++) {
+        float roll_in = xy_rp[row][0];
+        float pitch_in = xy_rp[row][1];
+        rc_input_to_roll_pitch(roll_in, pitch_in, angle_max_deg, angle_limit_deg,roll_out_deg, pitch_out_deg);
+        printf("{%.3f, %.3f, %.3f, %.3f},\n", roll_in, pitch_in, roll_out_deg, pitch_out_deg);
+    }
+#endif
+    for (uint row=0; row<ARRAY_SIZE(xy_rp); row++) {
+        float roll_in = xy_rp[row][0];
+        float pitch_in = xy_rp[row][1];
+        rc_input_to_roll_pitch(roll_in, pitch_in, angle_max_deg, angle_limit_deg,roll_out_deg, pitch_out_deg);
+
+        EXPECT_TRUE(fabsf(roll_out_deg  - xy_rp[row][2]) < .02f);
+        EXPECT_TRUE(fabsf(pitch_out_deg - xy_rp[row][3]) < .02f);
+    }
+}
+
+
+AP_GTEST_MAIN();