From 8f74f5b3b0abe6c27dac66bfdff33756371d2255 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 28 Apr 2014 16:29:30 +0900 Subject: [PATCH] CoaxCopter: output_test for individual motors Based on original work by Nils Hogberg --- libraries/AP_Motors/AP_MotorsCoax.cpp | 65 +++++++++++++-------------- libraries/AP_Motors/AP_MotorsCoax.h | 6 ++- 2 files changed, 34 insertions(+), 37 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 249e79f18e..a44c75c1fd 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -206,41 +206,36 @@ void AP_MotorsCoax::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_MotorsCoax::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_MotorsCoax::output_test(uint8_t motor_seq, int16_t pwm) { - // Send minimum values to all motors - output_min(); - - // spin motor 1 - hal.scheduler->delay(1000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _rc_throttle.radio_min + _min_throttle); - hal.scheduler->delay(1000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _rc_throttle.radio_min); - hal.scheduler->delay(2000); - - // spin motor 2 - hal.scheduler->delay(1000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min + _min_throttle); - hal.scheduler->delay(1000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min); - hal.scheduler->delay(2000); - - // flap servo 1 - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_min); - hal.scheduler->delay(1000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_max); - hal.scheduler->delay(1000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_trim); - hal.scheduler->delay(2000); - - // flap servo 2 - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_min); - hal.scheduler->delay(1000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_max); - hal.scheduler->delay(1000); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_trim); + // 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: + // flap servo 1 + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm); + break; + case 2: + // flap servo 2 + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), pwm); + break; + case 3: + // motor 1 + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), pwm); + break; + case 4: + // motor 2 + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm); + break; + default: + // do nothing + break; + } } diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index ef73cc3441..20f76db7d1 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -42,8 +42,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();