diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index a29b7ba4cc..14163bee80 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -185,27 +185,36 @@ void AP_MotorsTri::output_disarmed() output_min(); } -// output_test - spin each motor for a moment to allow the user to confirm the motor order and spin direction -void AP_MotorsTri::output_test() +// output_test - 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 +// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 +void AP_MotorsTri::output_test(uint8_t motor_seq, int16_t pwm) { - // Send minimum values to all motors - output_min(); - - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _rc_throttle.radio_min); - hal.scheduler->delay(4000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _rc_throttle.radio_min + _min_throttle); - hal.scheduler->delay(300); - - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _rc_throttle.radio_min); - hal.scheduler->delay(2000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min + _min_throttle); - hal.scheduler->delay(300); - - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min); - hal.scheduler->delay(2000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _rc_throttle.radio_min + _min_throttle); - hal.scheduler->delay(300); + // exit immediately if not armed + if (!_flags.armed) { + return; + } - // Send minimum values to all motors - output_min(); + // output to motors and servos + switch (motor_seq) { + case 1: + // front right motor + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm); + break; + case 2: + // back motor + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm); + break; + case 3: + // back servo + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), pwm); + break; + case 4: + // front left motor + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), pwm); + break; + default: + // do nothing + break; + } } diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 679a0ef51d..c7522d6751 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -33,8 +33,10 @@ public: // enable - starts allowing signals to be sent to motors virtual void enable(); - // output_test - spin each motor for a moment to allow the user to confirm the motor order and spin direction - virtual void output_test(); + // output_test - 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 + // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 + virtual void output_test(uint8_t motor_seq, int16_t pwm); // output_min - sends minimum values out to the motors virtual void output_min();