mirror of https://github.com/ArduPilot/ardupilot
AP_Motors_test: add tri frames to json output
This commit is contained in:
parent
143c975fb1
commit
dc3f2c9724
|
@ -360,6 +360,30 @@ float AP_MotorsTri::get_roll_factor(uint8_t i)
|
||||||
return ret;
|
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
|
// Run arming checks
|
||||||
bool AP_MotorsTri::arming_checks(size_t buflen, char *buffer) const
|
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
|
// run base class checks
|
||||||
return AP_MotorsMulticopter::arming_checks(buflen, buffer);
|
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)
|
||||||
|
|
|
@ -48,9 +48,15 @@ public:
|
||||||
// using copter motors for forward flight
|
// using copter motors for forward flight
|
||||||
float get_roll_factor(uint8_t i) override;
|
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
|
// Run arming checks
|
||||||
bool arming_checks(size_t buflen, char *buffer) const override;
|
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:
|
protected:
|
||||||
// output - sends commands to the motors
|
// output - sends commands to the motors
|
||||||
void output_armed_stabilizing() override;
|
void output_armed_stabilizing() override;
|
||||||
|
|
|
@ -25,14 +25,16 @@ void loop();
|
||||||
void motor_order_test();
|
void motor_order_test();
|
||||||
void stability_test();
|
void stability_test();
|
||||||
void update_motors();
|
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
|
// Instantiate a few classes that will be needed so that the singletons can be called from the motors lib
|
||||||
#if HAL_WITH_ESC_TELEM
|
#if HAL_WITH_ESC_TELEM
|
||||||
AP_ESC_Telem esc_telem;
|
AP_ESC_Telem esc_telem;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define VERSION "AP_Motors library test ver 1.1"
|
#define VERSION "AP_Motors library test ver 1.2"
|
||||||
|
|
||||||
SRV_Channels srvs;
|
SRV_Channels srvs;
|
||||||
AP_BattMonitor _battmonitor{0, nullptr, nullptr};
|
AP_BattMonitor _battmonitor{0, nullptr, nullptr};
|
||||||
|
@ -40,6 +42,7 @@ AP_BattMonitor _battmonitor{0, nullptr, nullptr};
|
||||||
AP_Motors *motors;
|
AP_Motors *motors;
|
||||||
AP_MotorsHeli *motors_heli;
|
AP_MotorsHeli *motors_heli;
|
||||||
AP_MotorsMatrix *motors_matrix;
|
AP_MotorsMatrix *motors_matrix;
|
||||||
|
AP_MotorsTri *motors_tri;
|
||||||
|
|
||||||
bool thrust_boost = false;
|
bool thrust_boost = false;
|
||||||
|
|
||||||
|
@ -271,9 +274,12 @@ void setup()
|
||||||
|
|
||||||
} else if (strcmp(argv[1],"p") == 0) {
|
} else if (strcmp(argv[1],"p") == 0) {
|
||||||
if (motors_matrix == nullptr) {
|
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 {
|
} else {
|
||||||
::printf("Expected first argument: 't', 's' or 'p'\n");
|
::printf("Expected first argument: 't', 's' or 'p'\n");
|
||||||
|
@ -336,23 +342,89 @@ void loop()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool first_layout = true;
|
||||||
|
|
||||||
// print motor layout for all frame types in json format
|
// 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("{\n");
|
||||||
hal.console->printf("\t\"Version\": \"%s\",\n", VERSION);
|
hal.console->printf("\t\"Version\": \"%s\",\n", VERSION);
|
||||||
hal.console->printf("\t\"layouts\": [\n");
|
hal.console->printf("\t\"layouts\": [\n");
|
||||||
|
|
||||||
bool first_layout = true;
|
first_layout = true;
|
||||||
char frame_and_type_string[30];
|
|
||||||
|
|
||||||
for (uint8_t frame_class=0; frame_class <= AP_Motors::MOTOR_FRAME_DECA; frame_class++) {
|
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++) {
|
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 ||
|
if (frame_class == AP_Motors::MOTOR_FRAME_TRI) {
|
||||||
frame_type == AP_Motors::MOTOR_FRAME_TYPE_ATAIL) {
|
print_motor_tri(frame_class, frame_type);
|
||||||
// Skip the none planar motors types
|
} else {
|
||||||
continue;
|
print_motor_matrix(frame_class, frame_type);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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_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);
|
motors_matrix->init((AP_Motors::motor_frame_class)frame_class, (AP_Motors::motor_frame_type)frame_type);
|
||||||
if (motors_matrix->initialised_ok()) {
|
if (motors_matrix->initialised_ok()) {
|
||||||
if (!first_layout) {
|
if (!first_layout) {
|
||||||
|
@ -398,6 +470,7 @@ void print_all_motor_matrix()
|
||||||
hal.console->printf("\t\t\t\t\t\"Roll\": %0.4f,\n", roll);
|
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\t\"Pitch\": %0.4f\n", pitch);
|
||||||
hal.console->printf("\t\t\t\t}");
|
hal.console->printf("\t\t\t\t}");
|
||||||
|
while (hal.console->tx_pending()) { ; }
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
hal.console->printf("\n");
|
hal.console->printf("\n");
|
||||||
|
@ -406,12 +479,6 @@ void print_all_motor_matrix()
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
hal.console->printf("\n");
|
|
||||||
hal.console->printf("\t]\n");
|
|
||||||
hal.console->printf("}\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
// stability_test
|
// stability_test
|
||||||
void motor_order_test()
|
void motor_order_test()
|
||||||
|
|
Loading…
Reference in New Issue