From 5318cf88439ed7f79361551b7fb9fe7ebc1d4e1c Mon Sep 17 00:00:00 2001 From: Bob Long Date: Thu, 30 Nov 2023 23:57:29 +1100 Subject: [PATCH 1/2] AP_Motors_test: add tri frames to json output --- libraries/AP_Motors/AP_MotorsTri.cpp | 44 ++++ libraries/AP_Motors/AP_MotorsTri.h | 6 + .../AP_Motors_test/AP_Motors_test.cpp | 191 ++++++++++++------ 3 files changed, 179 insertions(+), 62 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index ab566944aeb96..cb2299a1f1604 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -360,6 +360,30 @@ float AP_MotorsTri::get_roll_factor(uint8_t i) return ret; } +// This function is currently only used by AP_Motors_test +#if APM_BUILD_TYPE(APM_BUILD_UNKNOWN) +float AP_MotorsTri::get_pitch_factor_json(uint8_t i) +{ + float ret = 0.0f; + + switch (i) { + case AP_MOTORS_MOT_1: // front motors + case AP_MOTORS_MOT_2: + ret = 0.5f; + break; + case AP_MOTORS_MOT_4: // rear motor + ret = -1.0f; + break; + } + + if (_pitch_reversed) { + ret *= -1.0f; + } + + return ret; +} +#endif // APM_BUILD_TYPE(APM_BUILD_UNKNOWN) + // Run arming checks bool AP_MotorsTri::arming_checks(size_t buflen, char *buffer) const { @@ -374,3 +398,23 @@ bool AP_MotorsTri::arming_checks(size_t buflen, char *buffer) const // run base class checks return AP_MotorsMulticopter::arming_checks(buflen, buffer); } + +// This function is currently only used by AP_Motors_test +#if APM_BUILD_TYPE(APM_BUILD_UNKNOWN) +// 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; + } +} +#endif // APM_BUILD_TYPE(APM_BUILD_UNKNOWN) diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 430e5c9b706a2..eed4a615f1590 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -48,9 +48,15 @@ class AP_MotorsTri : public AP_MotorsMulticopter { // using copter motors for forward flight float get_roll_factor(uint8_t i) override; + // return the pitch factor of any motor, this is used for AP_Motors_test + float get_pitch_factor_json(uint8_t i); + // Run arming checks bool arming_checks(size_t buflen, char *buffer) const override; + // Get the testing order for the motors, this is used for AP_Motors_test + uint8_t get_motor_test_order(uint8_t i); + protected: // output - sends commands to the motors void output_armed_stabilizing() override; 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 491bb0fc60bff..039984f64f601 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 @@ -25,14 +25,16 @@ void loop(); void motor_order_test(); void stability_test(); void update_motors(); -void print_all_motor_matrix(); +void print_all_motors(); +void print_motor_matrix(uint8_t frame_class, uint8_t frame_type); +void print_motor_tri(uint8_t frame_class, uint8_t frame_type); // Instantiate a few classes that will be needed so that the singletons can be called from the motors lib #if HAL_WITH_ESC_TELEM AP_ESC_Telem esc_telem; #endif -#define VERSION "AP_Motors library test ver 1.1" +#define VERSION "AP_Motors library test ver 1.2" SRV_Channels srvs; AP_BattMonitor _battmonitor{0, nullptr, nullptr}; @@ -40,6 +42,7 @@ AP_BattMonitor _battmonitor{0, nullptr, nullptr}; AP_Motors *motors; AP_MotorsHeli *motors_heli; AP_MotorsMatrix *motors_matrix; +AP_MotorsTri *motors_tri; bool thrust_boost = false; @@ -271,9 +274,12 @@ void setup() } else if (strcmp(argv[1],"p") == 0) { if (motors_matrix == nullptr) { - ::printf("Print only supports motors matrix\n"); + motors_matrix = new AP_MotorsMatrix(400); } - print_all_motor_matrix(); + if (motors_tri == nullptr) { + motors_tri = new AP_MotorsTri(400); + } + print_all_motors(); } else { ::printf("Expected first argument: 't', 's' or 'p'\n"); @@ -336,81 +342,142 @@ void loop() } } +bool first_layout = true; + // print motor layout for all frame types in json format -void print_all_motor_matrix() +void print_all_motors() { hal.console->printf("{\n"); hal.console->printf("\t\"Version\": \"%s\",\n", VERSION); hal.console->printf("\t\"layouts\": [\n"); - bool first_layout = true; - char frame_and_type_string[30]; + first_layout = true; for (uint8_t frame_class=0; frame_class <= AP_Motors::MOTOR_FRAME_DECA; frame_class++) { for (uint8_t frame_type=0; frame_type < AP_Motors::MOTOR_FRAME_TYPE_Y4; frame_type++) { - if (frame_type == AP_Motors::MOTOR_FRAME_TYPE_VTAIL || - frame_type == AP_Motors::MOTOR_FRAME_TYPE_ATAIL) { - // Skip the none planar motors types - continue; + if (frame_class == AP_Motors::MOTOR_FRAME_TRI) { + print_motor_tri(frame_class, frame_type); + } else { + print_motor_matrix(frame_class, frame_type); } - motors_matrix->init((AP_Motors::motor_frame_class)frame_class, (AP_Motors::motor_frame_type)frame_type); - if (motors_matrix->initialised_ok()) { - if (!first_layout) { + } + } + + hal.console->printf("\n"); + hal.console->printf("\t]\n"); + hal.console->printf("}\n"); +} + +void print_motor_tri(uint8_t frame_class, uint8_t frame_type) +{ + char frame_and_type_string[30]; + motors_tri->init((AP_Motors::motor_frame_class)frame_class, (AP_Motors::motor_frame_type)frame_type); + if (motors_tri->initialised_ok()) { + if (!first_layout) { + hal.console->printf(",\n"); + } + first_layout = false; + + // Grab full frame string and strip "Frame: " and split + // This is the long way round, motors does have direct getters, but there protected + motors_tri->get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string)); + char *frame_string = strchr(frame_and_type_string, ':'); + char *type_string = strchr(frame_and_type_string, '/'); + if (type_string != nullptr) { + *type_string = 0; + } + + hal.console->printf("\t\t{\n"); + 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\"motors\": [\n"); + bool first_motor = true; + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + float roll, pitch; + uint8_t testing_order; + roll = motors_tri->get_roll_factor(i); + pitch = motors_tri->get_pitch_factor_json(i); + testing_order = motors_tri->get_motor_test_order(i); + if (testing_order) { + if (!first_motor) { hal.console->printf(",\n"); } - first_layout = false; - - // Grab full frame string and strip "Frame: " and split - // This is the long way round, motors does have direct getters, but there protected - motors_matrix->get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string)); - char *frame_string = strchr(frame_and_type_string, ':'); - char *type_string = strchr(frame_and_type_string, '/'); - if (type_string != nullptr) { - *type_string = 0; - } + first_motor = false; + hal.console->printf("\t\t\t\t{\n"); + hal.console->printf("\t\t\t\t\t\"Number\": %i,\n", i+1); + hal.console->printf("\t\t\t\t\t\"TestOrder\": %i,\n", testing_order); + hal.console->printf("\t\t\t\t\t\"Rotation\": \"?\",\n"); + hal.console->printf("\t\t\t\t\t\"Roll\": %0.4f,\n", roll); + hal.console->printf("\t\t\t\t\t\"Pitch\": %0.4f\n", pitch); + hal.console->printf("\t\t\t\t}"); + while (hal.console->tx_pending()) { ; } + } + } + hal.console->printf("\n"); + hal.console->printf("\t\t\t]\n"); + hal.console->printf("\t\t}"); - hal.console->printf("\t\t{\n"); - 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\"motors\": [\n"); - bool first_motor = true; - for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { - float roll, pitch, yaw, throttle; - uint8_t testing_order; - if (motors_matrix->get_factors(i, roll, pitch, yaw, throttle, testing_order)) { - if (!first_motor) { - hal.console->printf(",\n"); - } - first_motor = false; - hal.console->printf("\t\t\t\t{\n"); - hal.console->printf("\t\t\t\t\t\"Number\": %i,\n", i+1); - hal.console->printf("\t\t\t\t\t\"TestOrder\": %i,\n", testing_order); - hal.console->printf("\t\t\t\t\t\"Rotation\": "); - if (is_positive(yaw)) { - hal.console->printf("\"CCW\",\n"); - } else if (is_negative(yaw)) { - hal.console->printf("\"CW\",\n"); - } else { - hal.console->printf("\"?\",\n"); - } - hal.console->printf("\t\t\t\t\t\"Roll\": %0.4f,\n", roll); - hal.console->printf("\t\t\t\t\t\"Pitch\": %0.4f\n", pitch); - hal.console->printf("\t\t\t\t}"); - } - } - hal.console->printf("\n"); - hal.console->printf("\t\t\t]\n"); - hal.console->printf("\t\t}"); + } +} + +void print_motor_matrix(uint8_t frame_class, uint8_t frame_type) +{ + char frame_and_type_string[30]; + motors_matrix->init((AP_Motors::motor_frame_class)frame_class, (AP_Motors::motor_frame_type)frame_type); + if (motors_matrix->initialised_ok()) { + if (!first_layout) { + hal.console->printf(",\n"); + } + first_layout = false; + + // Grab full frame string and strip "Frame: " and split + // This is the long way round, motors does have direct getters, but there protected + motors_matrix->get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string)); + char *frame_string = strchr(frame_and_type_string, ':'); + char *type_string = strchr(frame_and_type_string, '/'); + if (type_string != nullptr) { + *type_string = 0; + } + hal.console->printf("\t\t{\n"); + 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\"motors\": [\n"); + bool first_motor = true; + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + float roll, pitch, yaw, throttle; + uint8_t testing_order; + if (motors_matrix->get_factors(i, roll, pitch, yaw, throttle, testing_order)) { + if (!first_motor) { + hal.console->printf(",\n"); + } + first_motor = false; + hal.console->printf("\t\t\t\t{\n"); + hal.console->printf("\t\t\t\t\t\"Number\": %i,\n", i+1); + hal.console->printf("\t\t\t\t\t\"TestOrder\": %i,\n", testing_order); + hal.console->printf("\t\t\t\t\t\"Rotation\": "); + if (is_positive(yaw)) { + hal.console->printf("\"CCW\",\n"); + } else if (is_negative(yaw)) { + hal.console->printf("\"CW\",\n"); + } else { + hal.console->printf("\"?\",\n"); + } + hal.console->printf("\t\t\t\t\t\"Roll\": %0.4f,\n", roll); + hal.console->printf("\t\t\t\t\t\"Pitch\": %0.4f\n", pitch); + hal.console->printf("\t\t\t\t}"); + while (hal.console->tx_pending()) { ; } } } - } + hal.console->printf("\n"); + hal.console->printf("\t\t\t]\n"); + hal.console->printf("\t\t}"); - hal.console->printf("\n"); - hal.console->printf("\t]\n"); - hal.console->printf("}\n"); + } } // stability_test From e7a6bc2afe069d330b44f5d439007f917c914e50 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 11 Dec 2023 11:47:09 +1100 Subject: [PATCH 2/2] 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 cb2299a1f1604..e387694c2010c 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 eed4a615f1590..3c034ccc1ceeb 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 039984f64f601..7a3d0ee67d632 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++) {