Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

TradHeli: output_test for individual motors

Browse Source 
Based on original work by Nils Hogberg
This commit is contained in:
Randy Mackay 2014-04-28 16:29:49 +09:00
parent 8f74f5b3b0
commit 3610cfe24c
2 changed files with 39 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -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 // output_test - spin a motor at the pwm value specified
void AP_MotorsHeli::output_test() // 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; // exit immediately if not armed
// Send minimum values to all motors if (!_flags.armed) {
output_min(); return;
// 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);
} }
// servo 2 // output to motors and servos
for( i=0; i<5; i++ ) { switch (motor_seq) {
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_trim + 100); case 1:
hal.scheduler->delay(300); // swash servo 1
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_trim - 100); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm);
hal.scheduler->delay(300); break;
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_trim + 0); case 2:
hal.scheduler->delay(300); // 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 // allow_arming - returns true if main rotor is spinning and it is ok to arm

6
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -138,8 +138,10 @@ public:
// output_min - sends minimum values out to the motors // output_min - sends minimum values out to the motors
void output_min(); void output_min();
// output_test - wiggle servos in order to show connections are correct // output_test - spin a motor at the pwm value specified
void output_test(); // 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 // heli specific methods