diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 9abec10f656532..e1cc6f2ce4d1da 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) \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index e50ebfd68bb69d..818a7c675daa5e 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -65,6 +65,7 @@ class AP_MotorsTri : public AP_MotorsMulticopter { 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 f42427230811f8..f73612a11947a3 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++) {