Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

MotorMatrix: output_test for individual motors 

Based on original work by Nils Hogberg
This commit is contained in:
Randy Mackay 2014-04-28 16:29:09 +09:00
parent 11d02ea5d2
commit d63d82ec17
2 changed files with 15 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -329,43 +329,23 @@ void AP_MotorsMatrix::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_MotorsMatrix::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_MotorsMatrix::output_test(uint8_t motor_seq, int16_t pwm)
{
uint8_t min_order, max_order;
uint8_t i,j;
// find min and max orders
min_order = _test_order[0];
max_order = _test_order[0];
for(i=1; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( _test_order[i] < min_order )
min_order = _test_order[i];
if( _test_order[i] > max_order )
max_order = _test_order[i];
// exit immediately if not armed
if (!_flags.armed) {
return;
}
// shut down all motors
output_min();
// first delay is longer
hal.scheduler->delay(4000);
// loop through all the possible orders spinning any motors that match that description
for( i=min_order; i<=max_order; i++ ) {
for( j=0; j<AP_MOTORS_MAX_NUM_MOTORS; j++ ) {
if( motor_enabled[j] && _test_order[j] == i ) {
// turn on this motor and wait 1/3rd of a second
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[j]), _rc_throttle.radio_min + _min_throttle);
hal.scheduler->delay(300);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[j]), _rc_throttle.radio_min);
hal.scheduler->delay(2000);
}
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i] && _test_order[i] == motor_seq) {
// turn on this motor
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), pwm);
}
}
// shut down all motors
output_min();
}
// add_motor

6
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -38,8 +38,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();