Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SingleCopter: use refs for all RC_Channels

Browse Source
This commit is contained in:
Randy Mackay 2014-02-10 13:22:54 +09:00 committed by Andrew Tridgell
parent 18d3907928
commit 01d30271d8
2 changed files with 50 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -99,14 +99,14 @@ void AP_MotorsSingle::Init()
motor_enabled[AP_MOTORS_MOT_7] = true;
// we set four servos to angle
_servo1->set_type(RC_CHANNEL_TYPE_ANGLE);
_servo2->set_type(RC_CHANNEL_TYPE_ANGLE);
_servo3->set_type(RC_CHANNEL_TYPE_ANGLE);
_servo4->set_type(RC_CHANNEL_TYPE_ANGLE);
_servo1->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo2->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo3->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo4->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo1.set_type(RC_CHANNEL_TYPE_ANGLE);
_servo2.set_type(RC_CHANNEL_TYPE_ANGLE);
_servo3.set_type(RC_CHANNEL_TYPE_ANGLE);
_servo4.set_type(RC_CHANNEL_TYPE_ANGLE);
_servo1.set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo2.set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo3.set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo4.set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
// disable CH7 from being used as an aux output (i.e. for camera gimbal, etc)
RC_Channel_aux::disable_aux_channel(CH_7);
@ -144,27 +144,27 @@ void AP_MotorsSingle::enable()
void AP_MotorsSingle::output_min()
{
// send minimum value to each motor
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], _servo3->radio_trim);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_trim);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], _rc_throttle->radio_min);
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], _servo3.radio_trim);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4.radio_trim);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], _rc_throttle.radio_min);
}
// output_armed - sends commands to the motors
void AP_MotorsSingle::output_armed()
{
int16_t out_min = _rc_throttle->radio_min + _min_throttle;
int16_t out_min = _rc_throttle.radio_min + _min_throttle;
int16_t motor_out; // main motor output
// Throttle is 0 to 1000 only
_rc_throttle->servo_out = constrain_int16(_rc_throttle->servo_out, 0, _max_throttle);
_rc_throttle.servo_out = constrain_int16(_rc_throttle.servo_out, 0, _max_throttle);
// capture desired throttle from receiver
_rc_throttle->calc_pwm();
_rc_throttle.calc_pwm();
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle->servo_out == 0) {
if(_rc_throttle.servo_out == 0) {
// range check spin_when_armed
if (_spin_when_armed < 0) {
_spin_when_armed = 0;
@ -173,10 +173,10 @@ void AP_MotorsSingle::output_armed()
_spin_when_armed = _min_throttle;
}
motor_out = _rc_throttle->radio_min + _spin_when_armed;
motor_out = _rc_throttle.radio_min + _spin_when_armed;
}else{
//motor
motor_out = _rc_throttle->radio_out;
motor_out = _rc_throttle.radio_out;
// adjust for throttle curve
if( _throttle_curve_enabled ) {
@ -188,24 +188,24 @@ void AP_MotorsSingle::output_armed()
}
// front servo
_servo1->servo_out = _rev_roll*_rc_roll->servo_out + _rev_yaw*_rc_yaw->servo_out;
_servo1.servo_out = _rev_roll*_rc_roll.servo_out + _rev_yaw*_rc_yaw.servo_out;
// right servo
_servo2->servo_out = _rev_pitch*_rc_pitch->servo_out + _rev_yaw*_rc_yaw->servo_out;
_servo2.servo_out = _rev_pitch*_rc_pitch.servo_out + _rev_yaw*_rc_yaw.servo_out;
// rear servo
_servo3->servo_out = -_rev_roll*_rc_roll->servo_out + _rev_yaw*_rc_yaw->servo_out;
_servo3.servo_out = -_rev_roll*_rc_roll.servo_out + _rev_yaw*_rc_yaw.servo_out;
// left servo
_servo4->servo_out = -_rev_pitch*_rc_pitch->servo_out + _rev_yaw*_rc_yaw->servo_out;
_servo4.servo_out = -_rev_pitch*_rc_pitch.servo_out + _rev_yaw*_rc_yaw.servo_out;
_servo1->calc_pwm();
_servo2->calc_pwm();
_servo3->calc_pwm();
_servo4->calc_pwm();
_servo1.calc_pwm();
_servo2.calc_pwm();
_servo3.calc_pwm();
_servo4.calc_pwm();
// send output to each motor
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], _servo3->radio_out);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_out);
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], _servo3.radio_out);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4.radio_out);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], motor_out);
}
@ -224,41 +224,41 @@ void AP_MotorsSingle::output_test()
// 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.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.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.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.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.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.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.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.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.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.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.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.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.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);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4.radio_trim);
// Send minimum values to all motors
output_min();

10
libraries/AP_Motors/AP_MotorsSingle.h
View File

@ -25,7 +25,7 @@ class AP_MotorsSingle : public AP_Motors {
public:
/// Constructor
AP_MotorsSingle( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, RC_Channel* servo1, RC_Channel* servo2, RC_Channel* servo3, RC_Channel* servo4, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsSingle( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, RC_Channel& servo1, RC_Channel& servo2, RC_Channel& servo3, RC_Channel& servo4, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
_servo1(servo1),
_servo2(servo2),
@ -62,10 +62,10 @@ protected:
AP_Int8 _rev_pitch; // REV pitch feedback
AP_Int8 _rev_yaw; // REV yaw feedback
AP_Int16 _servo_speed; // servo speed
RC_Channel* _servo1;
RC_Channel* _servo2;
RC_Channel* _servo3;
RC_Channel* _servo4;
RC_Channel& _servo1;
RC_Channel& _servo2;
RC_Channel& _servo3;
RC_Channel& _servo4;
};
#endif // AP_MOTORSSINGLE