AP_Motors: cope with H vs X frame in HeliQuad

Kiwa21 · Oct 2, 2017 · e1f8e7b · e1f8e7b
1 parent e2f7105
commit e1f8e7b
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 3 deletions.
diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp
@@ -181,6 +181,9 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
 // init
 void AP_MotorsHeli::init(motor_frame_class frame_class, motor_frame_type frame_type)
 {
+    // remember frame type
+    _frame_type = frame_type;
+
     // set update rate
     set_update_rate(_speed_hz);
 

diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h
@@ -213,4 +213,6 @@ class AP_MotorsHeli : public AP_Motors {
     // internal variables
     float           _collective_mid_pct = 0.0f;      // collective mid parameter value converted to 0 ~ 1 range
     uint8_t         _servo_test_cycle_counter = 0;   // number of test cycles left to run after bootup
+
+    motor_frame_type _frame_type;
 };
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
@@ -165,13 +165,18 @@ void AP_MotorsHeli_Quad::calculate_roll_pitch_collective_factors()
     // assume X quad layout, with motors at 45, 135, 225 and 315 degrees
     // order FrontRight, RearLeft, FrontLeft, RearLeft
     const float angles[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { 45, 225, 315, 135 };
-    const bool clockwise[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { false, false, true, true };
+    const bool x_clockwise[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { false, false, true, true };
     const float cos45 = cosf(radians(45));
 
     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
+        bool clockwise = x_clockwise[i];
+        if (_frame_type == MOTOR_FRAME_TYPE_H) {
+            // reverse yaw for H frame
+            clockwise = !clockwise;
+        }
         _rollFactor[CH_1+i]       = -0.5*sinf(radians(angles[i]))/cos45;
         _pitchFactor[CH_1+i]      =  0.5*cosf(radians(angles[i]))/cos45;
-        _yawFactor[CH_1+i]        = clockwise[i]?-0.5:0.5;
+        _yawFactor[CH_1+i]        = clockwise?-0.5:0.5;
         _collectiveFactor[CH_1+i] = 1;
     }
 }

diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.h b/libraries/AP_Motors/AP_MotorsHeli_Quad.h
@@ -30,7 +30,6 @@ class AP_MotorsHeli_Quad : public AP_MotorsHeli {
         AP_Param::setup_object_defaults(this, var_info);
     };
 
-
     // set_update_rate - set update rate to motors
     void set_update_rate( uint16_t speed_hz ) override;
-Original file line number
+Diff line change
@@ Expand Up / @@ -30,7 +30,6 @@ class AP_MotorsHeli_Quad : public AP_MotorsHeli { @@
             AP_Param::setup_object_defaults(this, var_info);
         };
         // set_update_rate - set update rate to motors
         void set_update_rate( uint16_t speed_hz ) override;
@@ Expand Down @@