Skip to content

Commit

Permalink
AP_Motors_test.cpp: add tri frames to json output
Browse files Browse the repository at this point in the history
  • Loading branch information
@robertlong13
robertlong13 committed Nov 30, 2023
1 parent ce824b7 commit e688b52
Show file tree
Hide file tree
Showing 3 changed files with 177 additions and 62 deletions.
40 changes: 40 additions & 0 deletions libraries/AP_Motors/AP_MotorsTri.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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;
}
}
7 changes: 7 additions & 0 deletions libraries/AP_Motors/AP_MotorsTri.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand Down
192 changes: 130 additions & 62 deletions libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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);
}
}
}
Expand All @@ -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()
{
Expand Down

0 comments on commit e688b52

Please sign in to comment.