diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 6b97fe9945..a5d558d696 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -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(); diff --git a/libraries/AP_Motors/AP_MotorsSingle.h b/libraries/AP_Motors/AP_MotorsSingle.h index c882d5365d..21693f084d 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.h +++ b/libraries/AP_Motors/AP_MotorsSingle.h @@ -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