From 3610cfe24c9bb7bea5f5b03fb5c092cb7237548e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 28 Apr 2014 16:29:49 +0900 Subject: [PATCH] TradHeli: output_test for individual motors Based on original work by Nils Hogberg --- libraries/AP_Motors/AP_MotorsHeli.cpp | 85 +++++++++++---------------- libraries/AP_Motors/AP_MotorsHeli.h | 6 +- 2 files changed, 39 insertions(+), 52 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 2f4178119b..e0d67a208b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -274,60 +274,45 @@ void AP_MotorsHeli::output_min() } -// output_test - spin each motor for a moment to allow the user to confirm the motor order and spin direction -void AP_MotorsHeli::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_MotorsHeli::output_test(uint8_t motor_seq, int16_t pwm) { - int16_t i; - // Send minimum values to all motors - output_min(); - - // servo 1 - for( i=0; i<5; i++ ) { - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_trim + 100); - hal.scheduler->delay(300); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_trim - 100); - hal.scheduler->delay(300); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_trim + 0); - hal.scheduler->delay(300); + // exit immediately if not armed + if (!_flags.armed) { + return; } - // servo 2 - for( i=0; i<5; i++ ) { - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_trim + 100); - hal.scheduler->delay(300); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_trim - 100); - hal.scheduler->delay(300); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_trim + 0); - hal.scheduler->delay(300); + // output to motors and servos + switch (motor_seq) { + case 1: + // swash servo 1 + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm); + break; + case 2: + // swash servo 2 + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), pwm); + break; + case 3: + // swash servo 3 + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), pwm); + break; + case 4: + // external gyro & tail servo + if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { + write_aux(_ext_gyro_gain); + } + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm); + break; + case 5: + // main rotor + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_HELI_RSC]), pwm); + break; + default: + // do nothing + break; } - - // servo 3 - for( i=0; i<5; i++ ) { - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_trim + 100); - hal.scheduler->delay(300); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_trim - 100); - hal.scheduler->delay(300); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_trim + 0); - hal.scheduler->delay(300); - } - - // external gyro - if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { - write_aux(_ext_gyro_gain); - } - - // servo 4 - for( i=0; i<5; i++ ) { - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_trim + 100); - hal.scheduler->delay(300); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_trim - 100); - hal.scheduler->delay(300); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_trim + 0); - hal.scheduler->delay(300); - } - - // Send minimum values to all motors - output_min(); } // allow_arming - returns true if main rotor is spinning and it is ok to arm diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 46932b3831..0cd5d255e2 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -138,8 +138,10 @@ public: // output_min - sends minimum values out to the motors void output_min(); - // output_test - wiggle servos in order to show connections are correct - 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); // // heli specific methods