From 7e8f9c72fb9d0354df697ab8bfa8978c5ea8cead Mon Sep 17 00:00:00 2001
From: Bob Long <robertlong13@gmail.com>
Date: Mon, 11 Dec 2023 11:47:09 +1100
Subject: [PATCH] AP_MotorsTri: reverse-frame cleanup and fixes

- fix motor test order for reverse frame
- add frame type string for reverse frame
- fix initialization of _pitch_reversed flag
---
 libraries/AP_Motors/AP_MotorsTri.cpp          | 73 ++++++++++++++-----
 libraries/AP_Motors/AP_MotorsTri.h            |  1 +
 .../AP_Motors_test/AP_Motors_test.cpp         |  2 +-
 3 files changed, 55 insertions(+), 21 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp
index cb2299a1f1..e387694c20 100644
--- a/libraries/AP_Motors/AP_MotorsTri.cpp
+++ b/libraries/AP_Motors/AP_MotorsTri.cpp
@@ -48,9 +48,7 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty
     SRV_Channels::set_angle(SRV_Channels::get_motor_function(AP_MOTORS_CH_TRI_YAW), _yaw_servo_angle_max_deg*100);
 
     // check for reverse tricopter
-    if (frame_type == MOTOR_FRAME_TYPE_PLUSREV) {
-        _pitch_reversed = true;
-    }
+    _pitch_reversed = frame_type == MOTOR_FRAME_TYPE_PLUSREV;
 
     _mav_type = MAV_TYPE_TRICOPTER;
 
@@ -62,11 +60,7 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty
 void AP_MotorsTri::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
 {
     // check for reverse tricopter
-    if (frame_type == MOTOR_FRAME_TYPE_PLUSREV) {
-        _pitch_reversed = true;
-    } else {
-        _pitch_reversed = false;
-    }
+    _pitch_reversed = frame_type == MOTOR_FRAME_TYPE_PLUSREV;
 
     set_initialised_ok((frame_class == MOTOR_FRAME_TRI) && SRV_Channels::function_assigned(SRV_Channel::k_motor7));
 }
@@ -286,7 +280,8 @@ void AP_MotorsTri::output_armed_stabilizing()
 void AP_MotorsTri::_output_test_seq(uint8_t motor_seq, int16_t pwm)
 {
     // output to motors and servos
-    switch (motor_seq) {
+    if (!_pitch_reversed) {
+        switch (motor_seq) {
         case 1:
             // front right motor
             rc_write(AP_MOTORS_MOT_1, pwm);
@@ -306,6 +301,29 @@ void AP_MotorsTri::_output_test_seq(uint8_t motor_seq, int16_t pwm)
         default:
             // do nothing
             break;
+        }
+    } else {
+        switch (motor_seq) {
+        case 1:
+            // front motor
+            rc_write(AP_MOTORS_MOT_4, pwm);
+            break;
+        case 2:
+            // front servo
+            rc_write(AP_MOTORS_CH_TRI_YAW, pwm);
+            break;
+        case 3:
+            // back right motor
+            rc_write(AP_MOTORS_MOT_1, pwm);
+            break;
+        case 4:
+            // back left motor
+            rc_write(AP_MOTORS_MOT_2, pwm);
+            break;
+        default:
+            // do nothing
+            break;
+        }
     }
 }
 
@@ -404,17 +422,32 @@ bool AP_MotorsTri::arming_checks(size_t buflen, char *buffer) const
 // Get the testing order for the motors
 uint8_t AP_MotorsTri::get_motor_test_order(uint8_t i)
 {
-    switch (i) {
-    case AP_MOTORS_MOT_1: // front right motor
-        return 1;
-    case AP_MOTORS_MOT_4: // back motor
-        return 2;
-    case AP_MOTORS_CH_TRI_YAW: // back servo
-        return 3;
-    case AP_MOTORS_MOT_2: // front left motor
-        return 4;
-    default:
-        return 0;
+    if (!_pitch_reversed) {
+        switch (i) {
+        case AP_MOTORS_MOT_1: // front right motor
+            return 1;
+        case AP_MOTORS_MOT_4: // back motor
+            return 2;
+        case AP_MOTORS_CH_TRI_YAW: // back servo
+            return 3;
+        case AP_MOTORS_MOT_2: // front left motor
+            return 4;
+        default:
+            return 0;
+        }
+    } else {
+        switch (i) {
+        case AP_MOTORS_MOT_4: // front motor
+            return 1;
+        case AP_MOTORS_CH_TRI_YAW: // front servo
+            return 2;
+        case AP_MOTORS_MOT_1: // back right motor
+            return 3;
+        case AP_MOTORS_MOT_2: // back left motor
+            return 4;
+        default:
+            return 0;
+        }
     }
 }
 #endif // APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h
index eed4a615f1..3c034ccc1c 100644
--- a/libraries/AP_Motors/AP_MotorsTri.h
+++ b/libraries/AP_Motors/AP_MotorsTri.h
@@ -65,6 +65,7 @@ protected:
     void                thrust_compensation(void) override;
 
     const char* _get_frame_string() const override { return "TRI"; }
+    const char*  get_type_string() const override { return _pitch_reversed ? "pitch-reversed" : ""; }
 
     // output_test_seq - spin a motor at the pwm value specified
     //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
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
index 039984f64f..7a3d0ee67d 100644
--- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
+++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
@@ -391,7 +391,7 @@ void print_motor_tri(uint8_t frame_class, uint8_t frame_type)
         hal.console->printf("\t\t\t\"Class\": %i,\n", frame_class);
         hal.console->printf("\t\t\t\"ClassName\": \"%s\",\n", frame_string+2);
         hal.console->printf("\t\t\t\"Type\": %i,\n", frame_type);
-        hal.console->printf("\t\t\t\"TypeName\": \"%s\",\n", (type_string != nullptr) ? type_string + 1 : "?");
+        hal.console->printf("\t\t\t\"TypeName\": \"%s\",\n", (type_string != nullptr) ? type_string + 1 : "default");
         hal.console->printf("\t\t\t\"motors\": [\n");
         bool first_motor = true;
         for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {