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
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

6
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -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