AP_Motors: Example: allow setting of COL2YAW and autorotation flag

ArduPilot · Dec 9, 2023 · bc17e70 · bc17e70
1 parent 6d51238
commit bc17e70
Showing 1 changed file with 30 additions and 3 deletions.
diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
@@ -38,6 +38,7 @@ SRV_Channels srvs;
 AP_BattMonitor _battmonitor{0, nullptr, nullptr};
 
 AP_Motors *motors;
+AP_MotorsHeli *motors_heli;
 AP_MotorsMatrix *motors_matrix;
 
 bool thrust_boost = false;
@@ -175,7 +176,8 @@ void setup()
                         break;
 
                     case AP_Motors::MOTOR_FRAME_HELI:
-                        motors = new AP_MotorsHeli_Single(400);
+                        motors_heli = new AP_MotorsHeli_Single(400);
+                        motors = motors_heli;
                         // Mot 1-3: Swash plate 1 to 3
                         // Mot 4: Tail rotor
                         // Mot 5: 4th servo in H4-90 swash
@@ -186,13 +188,15 @@ void setup()
                         break;
 
                     case AP_Motors::MOTOR_FRAME_HELI_DUAL:
-                        motors = new AP_MotorsHeli_Dual(400);
+                        motors_heli = new AP_MotorsHeli_Dual(400);
+                        motors = motors_heli;
                         // Mot 1-3 swashplate 1, mot 4-6 swashplate 2, mot 7 and 8 for 4th servos on H4-90 swash plates front and back, respectively
                         num_outputs = 8;
                         break;
 
                     case AP_Motors::MOTOR_FRAME_HELI_QUAD:
-                        motors = new AP_MotorsHeli_Quad(400);
+                        motors_heli = new AP_MotorsHeli_Quad(400);
+                        motors = motors_heli;
                         num_outputs = 4; // Only 4 collective servos
                         break;
 
@@ -215,6 +219,29 @@ void setup()
                     exit(1);
                 }
 
+            } else if (strcmp(cmd,"COL2YAW") == 0) {
+                if (frame_class != AP_Motors::MOTOR_FRAME_HELI) {
+                    ::printf("COL2YAW only supported by single heli frame type (%i), got %i\n", AP_Motors::MOTOR_FRAME_HELI, frame_class);
+                    exit(1);
+                }
+
+                // Union allows pointers to be aligned despite different sizes
+                // avoids "increases required alignment of target type" error when casting from char* to AP_Int16*
+                union {
+                    char *char_ptr;
+                    AP_Float *ap_float;
+                } collective_yaw_scale;
+
+                collective_yaw_scale.char_ptr = (char*)motors + AP_MotorsHeli_Single::var_info[7].offset;
+                collective_yaw_scale.ap_float->set(value);
+
+            } else if (strcmp(cmd,"autorotation") == 0) {
+                if (motors_heli == nullptr) {
+                    ::printf("autorotation only supported by heli frame types, got %i\n", frame_class);
+                    exit(1);
+                }
+                motors_heli->set_in_autorotation(!is_zero(value));
+
             } else {
                 ::printf("Expected \"frame_class\", \"yaw_headroom\" or \"throttle_avg_max\"\n");
                 exit(1);