From 2be99d7a92f43f7cd395db64236d46747c68629b Mon Sep 17 00:00:00 2001 From: Randy Mackay <rmackay9@yahoo.com> Date: Mon, 28 Apr 2014 16:30:26 +0900 Subject: [PATCH] TriCopter: output_test for individual motors Based on original work by Nils Hogberg --- libraries/AP_Motors/AP_MotorsTri.cpp | 51 ++++++++++++++++------------ libraries/AP_Motors/AP_MotorsTri.h | 6 ++-- 2 files changed, 34 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index a29b7ba4c..14163bee8 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 679a0ef51..c7522d675 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(); -- GitLab