Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Coax: servos to ch1, ch2, motors to ch3, ch4

Browse Source
This commit is contained in:
Dneault 2014-02-07 11:57:11 +09:00 committed by Randy Mackay
parent 5d929351ca
commit 5213ec4aa6
1 changed files with 35 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -92,12 +92,12 @@ void AP_MotorsCoax::Init()
// call parent Init function to set-up throttle curve
AP_Motors::Init();
// set update rate for the 2 motors (but not the 2 flaps (i.e. servos) on channels 3 and 4)
// set update rate for the 2 motors (but not the servo on channel 1&2)
set_update_rate(_speed_hz);
// set the motor_enabled flag so that the ESCs can be calibrated like other frame types
motor_enabled[AP_MOTORS_MOT_1] = true;
motor_enabled[AP_MOTORS_MOT_2] = true;
motor_enabled[AP_MOTORS_MOT_3] = true;
motor_enabled[AP_MOTORS_MOT_4] = true;
// set ranges for fin servos
_servo1->set_type(RC_CHANNEL_TYPE_ANGLE);
@ -114,14 +114,14 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz )
// set update rate for the two motors
uint32_t mask2 =
1U << _motor_to_channel_map[AP_MOTORS_MOT_1] |
1U << _motor_to_channel_map[AP_MOTORS_MOT_2] ;
1U << _motor_to_channel_map[AP_MOTORS_MOT_3] |
1U << _motor_to_channel_map[AP_MOTORS_MOT_4] ;
hal.rcout->set_freq(mask2, _speed_hz);
// set update rate for the two servos
uint32_t mask =
1U << _motor_to_channel_map[AP_MOTORS_MOT_3] |
1U << _motor_to_channel_map[AP_MOTORS_MOT_4] ;
1U << _motor_to_channel_map[AP_MOTORS_MOT_1] |
1U << _motor_to_channel_map[AP_MOTORS_MOT_2] ;
hal.rcout->set_freq(mask, _servo_speed);
}
@ -139,16 +139,16 @@ void AP_MotorsCoax::enable()
void AP_MotorsCoax::output_min()
{
// fill the motor_out[] array for HIL use
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min;
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min;
motor_out[AP_MOTORS_MOT_3] = _servo1->radio_trim;
motor_out[AP_MOTORS_MOT_4] = _servo2->radio_trim;
motor_out[AP_MOTORS_MOT_1] = _servo1->radio_trim;
motor_out[AP_MOTORS_MOT_2] = _servo2->radio_trim;
motor_out[AP_MOTORS_MOT_3] = _rc_throttle->radio_min;
motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min;
// send minimum value to each motor
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo1->radio_trim);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo2->radio_trim);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_trim);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_trim);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _rc_throttle->radio_min);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min);
}
// output_armed - sends commands to the motors
@ -171,13 +171,13 @@ void AP_MotorsCoax::output_armed()
if (_spin_when_armed > _min_throttle) {
_spin_when_armed = _min_throttle;
}
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min + _spin_when_armed;
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min + _spin_when_armed;
motor_out[AP_MOTORS_MOT_3] = _rc_throttle->radio_min + _spin_when_armed;
motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min + _spin_when_armed;
}else{
// motors
motor_out[AP_MOTORS_MOT_1] = _rev_yaw*_rc_yaw->servo_out + _rc_throttle->radio_out;
motor_out[AP_MOTORS_MOT_2] = -_rev_yaw*_rc_yaw->servo_out +_rc_throttle->radio_out;
motor_out[AP_MOTORS_MOT_3] = _rev_yaw*_rc_yaw->servo_out + _rc_throttle->radio_out;
motor_out[AP_MOTORS_MOT_4] = -_rev_yaw*_rc_yaw->servo_out +_rc_throttle->radio_out;
// front
_servo1->servo_out = _rev_roll*_rc_roll->servo_out;
// right
@ -185,23 +185,20 @@ void AP_MotorsCoax::output_armed()
_servo1->calc_pwm();
_servo2->calc_pwm();
motor_out[AP_MOTORS_MOT_3] = _servo1->radio_out;
motor_out[AP_MOTORS_MOT_4] = _servo2->radio_out;
// adjust for throttle curve
if( _throttle_curve_enabled ) {
motor_out[AP_MOTORS_MOT_1] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_1]);
motor_out[AP_MOTORS_MOT_2] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_2]);
motor_out[AP_MOTORS_MOT_3] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_3]);
motor_out[AP_MOTORS_MOT_4] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_4]);
}
// ensure motors don't drop below a minimum value and stop
motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], out_min);
motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2], out_min);
motor_out[AP_MOTORS_MOT_3] = max(motor_out[AP_MOTORS_MOT_3], out_min);
motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4], out_min);
}
// send output to each motor
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_1], _servo1->radio_out);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_out);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], motor_out[AP_MOTORS_MOT_3]);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
}
@ -221,32 +218,32 @@ void AP_MotorsCoax::output_test()
// spin motor 1
hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + _min_throttle);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _rc_throttle->radio_min + _min_throttle);
hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
hal.rcout->write(_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(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + _min_throttle);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min + _min_throttle);
hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min);
hal.scheduler->delay(2000);
// flap servo 1
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo1->radio_min);
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_3], _servo1->radio_max);
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_3], _servo1->radio_trim);
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_4], _servo2->radio_min);
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_4], _servo2->radio_max);
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_4], _servo2->radio_trim);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_trim);
// Send minimum values to all motors
output_min();