zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

mission-4.1.18
Dneault 11 years ago committed by Randy Mackay
parent
5d929351ca
commit
5213ec4aa6
1 changed files with 35 additions and 38 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 73
      libraries/AP_Motors/AP_MotorsCoax.cpp

73
libraries/AP_Motors/AP_MotorsCoax.cpp
Unescape View File

@ -92,12 +92,12 @@ void AP_MotorsCoax::Init() @@ -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 ) @@ -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() @@ -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() @@ -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() @@ -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() @@ -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();

Write Preview
Loading…
Cancel
Save