From e688b52d284b8563fd7b05167c5ccc6cd0d9ff0e Mon Sep 17 00:00:00 2001 From: Bob Long Date: Thu, 30 Nov 2023 23:56:16 +1100 Subject: [PATCH] AP_Motors_test.cpp: add tri frames to json output --- libraries/AP_Motors/AP_MotorsTri.cpp | 40 ++++ libraries/AP_Motors/AP_MotorsTri.h | 7 + .../AP_Motors_test/AP_Motors_test.cpp | 192 ++++++++++++------ 3 files changed, 177 insertions(+), 62 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 7b868d3aaf4bf..2c9b776232a24 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -360,6 +360,25 @@ float AP_MotorsTri::get_roll_factor(uint8_t i) return ret; } +float AP_MotorsTri::get_pitch_factor(uint8_t i) +{ + float ret = 0.0f; + + switch (i) { + // front motors + case AP_MOTORS_MOT_1: + case AP_MOTORS_MOT_2: + ret = 0.5f; + break; + // rear motor + case AP_MOTORS_MOT_4: + ret = -1.0f; + break; + } + + return ret; +} + // Run arming checks bool AP_MotorsTri::arming_checks(size_t buflen, char *buffer) const { @@ -374,3 +393,24 @@ bool AP_MotorsTri::arming_checks(size_t buflen, char *buffer) const // run base class checks return AP_MotorsMulticopter::arming_checks(buflen, buffer); } + +// Get the testing order for the motors +uint8_t AP_MotorsTri::get_motor_test_order(uint8_t i) +{ + switch (i) { + // front right motor + case AP_MOTORS_MOT_1: + return 1; + // front left motor + case AP_MOTORS_MOT_2: + return 2; + // tilt servo + case AP_MOTORS_CH_TRI_YAW: + return 3; + // rear motor + case AP_MOTORS_MOT_4: + return 4; + default: + return 0; + } +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 430e5c9b706a2..2d2dc4283edd3 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 roll factor of any motor, this is used for AP_Motors_test + float get_pitch_factor(uint8_t i) override; + // 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; @@ -59,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 "default"; } // 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 d65c940cd86f8..96eddceacdb92 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,20 +25,23 @@ 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}; AP_Motors *motors; AP_MotorsMatrix *motors_matrix; +AP_MotorsTri *motors_tri; bool thrust_boost = false; @@ -215,9 +218,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"); @@ -280,74 +286,24 @@ 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); } - 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}"); - } - } - hal.console->printf("\n"); - hal.console->printf("\t\t\t]\n"); - hal.console->printf("\t\t}"); - + else { + print_motor_matrix(frame_class, frame_type); } } } @@ -357,6 +313,118 @@ void print_all_motor_matrix() 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(i); + testing_order = motors_tri->get_motor_test_order(i); + if (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\": \"?\",\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}"); + + } +} + +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}"); + + } +} + // stability_test void motor_order_test() {