Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: test: fix for heli

This commit is contained in:
Iampete1 2023-05-02 01:00:57 +01:00 committed by Andrew Tridgell
parent 8653bf7349
commit bea98fa909
1 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
View File

@ -17,6 +17,7 @@
#include <AP_Motors/AP_Motors.h>
#include <RC_Channel/RC_Channel.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#include <stdio.h>
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
@ -31,12 +32,15 @@ void update_motors();
#define HELI_TEST 0 // set to 1 to test helicopters
#define NUM_OUTPUTS 4 // set to 6 for hexacopter, 8 for octacopter and heli
#if HAL_WITH_ESC_TELEM
AP_ESC_Telem esc_telem;
#endif
SRV_Channels srvs;
// uncomment the row below depending upon what frame you are using
//AP_MotorsTri motors(400);
AP_MotorsMatrix motors(400);
//AP_MotorsHeli_Single motors(rc7, rsc, h1, h2, h3, h4, 400);
//AP_MotorsHeli_Single motors(400);
//AP_MotorsSingle motors(400);
//AP_MotorsCoax motors(400);
@ -53,6 +57,9 @@ void setup()
motors.set_dt(1.0/400.0);
motors.set_update_rate(490);
#if HELI_TEST
motors.init(AP_Motors::MOTOR_FRAME_HELI, AP_Motors::MOTOR_FRAME_TYPE_PLUS);
#else
#if NUM_OUTPUTS == 8
motors.init(AP_Motors::MOTOR_FRAME_OCTA, AP_Motors::MOTOR_FRAME_TYPE_X);
#elif NUM_OUTPUTS == 6
@ -60,6 +67,7 @@ void setup()
#else
motors.init(AP_Motors::MOTOR_FRAME_QUAD, AP_Motors::MOTOR_FRAME_TYPE_X);
#endif
#endif // HELI_TEST == 0
char frame_and_type_string[30];
motors.get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string));
@ -76,6 +84,7 @@ void setup()
char * const *argv;
hal.util->commandline_arguments(argc, argv);
if (argc > 1) {
#if HELI_TEST == 0
for (uint8_t i = 2; i < argc; i++) {
const char* arg = argv[i];
@ -102,6 +111,7 @@ void setup()
}
}
#endif
if (strcmp(argv[1],"t") == 0) {
motor_order_test();
@ -165,9 +175,11 @@ void motor_order_test()
// stability_test
void stability_test()
{
#if HELI_TEST == 0
hal.console->printf("Throttle average max: %0.4f\n", motors.get_throttle_avg_max());
hal.console->printf("Yaw headroom: %i\n", motors.get_yaw_headroom());
hal.console->printf("Thrust boost: %s\n", thrust_boost?"True":"False");
#endif
const float throttle_tests[] = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0};
const uint8_t throttle_tests_num = ARRAY_SIZE(throttle_tests);
@ -183,9 +195,11 @@ void stability_test()
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
hal.console->printf("Mot%i,",i+1);
}
#if HELI_TEST == 0
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
hal.console->printf("Mot%i_norm,",i+1);
}
#endif
hal.console->printf("LimR,LimP,LimY,LimThD,LimThU\n");
// run stability test
@ -203,14 +217,17 @@ void stability_test()
motors.set_throttle(throttle_in);
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
update_motors();
SRV_Channels::calc_pwm();
// display input and output
hal.console->printf("%0.2f,%0.2f,%0.2f,%0.2f,", roll_in, pitch_in, yaw_in, throttle_in);
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
hal.console->printf("%d,",(int)hal.rcout->read(i));
}
#if HELI_TEST == 0
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
hal.console->printf("%0.4f,", motors.get_thrust_rpyt_out(i));
}
#endif
hal.console->printf("%d,%d,%d,%d,%d\n",
(int)motors.limit.roll,
(int)motors.limit.pitch,