// output_disarmed - sends commands to the motors void AP_MotorsHeli::output_test() { int16_t i; // Send minimum values to all motors output_min(); // servo 1 & 3 for( i=0; i<5; i++ ) { hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_1->radio_trim - 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim - 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_1->radio_trim + 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 0); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_1->radio_trim - 0); hal.scheduler->delay(300); } // servo 2 & 6 for( i=0; i<5; i++ ) { hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_6], _servo_1->radio_trim - 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim - 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_6], _servo_1->radio_trim + 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 0); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_6], _servo_1->radio_trim - 0); hal.scheduler->delay(300); } // servo 3 // for( i=0; i<5; i++ ) { // hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 100); // hal.scheduler->delay(300); // hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim - 100); // hal.scheduler->delay(300); // hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 0); // hal.scheduler->delay(300); // } // external gyro if( ext_gyro_enabled ) { hal.rcout->write(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain); } // servo 4 for( i=0; i<5; i++ ) { hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim - 100); hal.scheduler->delay(300); hal.rcout->write(_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(); }
// output_test - wiggle servos in order to show connections are correct void AP_MotorsHeli::output_test() { int16_t i; // Send minimum values to all motors output_min(); // servo 1 for( i=0; i<5; i++ ) { hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim - 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 0); hal.scheduler->delay(300); } // servo 2 for( i=0; i<5; i++ ) { hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim - 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 0); hal.scheduler->delay(300); } // servo 3 for( i=0; i<5; i++ ) { hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim - 100); hal.scheduler->delay(300); hal.rcout->write(_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(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 100); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim - 100); hal.scheduler->delay(300); hal.rcout->write(_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(); }
// output_test - spin each motor for a moment to allow the user to confirm the motor order and spin direction void AP_MotorsSingle::output_test() { // Send minimum values to all motors output_min(); // spin main motor hal.scheduler->delay(1000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], _rc_throttle->radio_min + _min_throttle); hal.scheduler->delay(1000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], _rc_throttle->radio_min); hal.scheduler->delay(2000); // flap servo 1 hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_min); hal.scheduler->delay(1000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_max); hal.scheduler->delay(1000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_trim); hal.scheduler->delay(2000); // flap servo 2 hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_min); hal.scheduler->delay(1000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_max); hal.scheduler->delay(1000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_trim); hal.scheduler->delay(2000); // flap servo 3 hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_min); hal.scheduler->delay(1000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_max); hal.scheduler->delay(1000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_trim); hal.scheduler->delay(2000); // flap servo 4 hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_min); hal.scheduler->delay(1000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_max); hal.scheduler->delay(1000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_trim); // Send minimum values to all motors output_min(); }
// output_disarmed - sends commands to the motors void AP_MotorsTri::output_disarmed() { // fill the motor_out[] array for HIL use for (unsigned char i = AP_MOTORS_MOT_1; i < AP_MOTORS_MOT_4; i++) { motor_out[i] = _rc_throttle->radio_min; } // Send minimum values to all motors output_min(); }
// output_disarmed - sends commands to the motors void AP_MotorsMatrix::output_disarmed() { if(_rc_throttle->control_in > 0) { // we have pushed up the throttle // remove safety for auto pilot _auto_armed = true; } // Send minimum values to all motors 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() { 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]; } // 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(_motor_to_channel_map[j], _rc_throttle->radio_min + _min_throttle); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[j], _rc_throttle->radio_min); hal.scheduler->delay(2000); } } } // shut down all motors 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() { // 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); // Send minimum values to all motors output_min(); }
// output_disarmed - sends commands to the motors void AP_MotorsTri::output_test() { // Send minimum values to all motors output_min(); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min); hal.scheduler->delay(4000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + _min_throttle); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min); hal.scheduler->delay(2000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min + _min_throttle); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min); hal.scheduler->delay(2000); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + _min_throttle); hal.scheduler->delay(300); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]); }
// output_disarmed - sends commands to the motors void AP_MotorsMatrix::output_disarmed() { // Send minimum values to all motors output_min(); }