From cf1d6854b9c1f70518122c6a57f2898b4445cd15 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 10 Feb 2014 13:22:18 +0900 Subject: [PATCH] AP_MotorsMatrix: use ref for roll, pitch, yaw, thr channels --- libraries/AP_Motors/AP_MotorsHexa.h | 2 +- libraries/AP_Motors/AP_MotorsMatrix.cpp | 50 ++++++++++++------------- libraries/AP_Motors/AP_MotorsMatrix.h | 2 +- libraries/AP_Motors/AP_MotorsOcta.h | 2 +- libraries/AP_Motors/AP_MotorsOctaQuad.h | 2 +- libraries/AP_Motors/AP_MotorsQuad.h | 2 +- libraries/AP_Motors/AP_MotorsY6.h | 2 +- 7 files changed, 31 insertions(+), 31 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHexa.h b/libraries/AP_Motors/AP_MotorsHexa.h index abb255b2d5..2c51911a2b 100644 --- a/libraries/AP_Motors/AP_MotorsHexa.h +++ b/libraries/AP_Motors/AP_MotorsHexa.h @@ -16,7 +16,7 @@ class AP_MotorsHexa : public AP_MotorsMatrix { public: /// Constructor - AP_MotorsHexa( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { + AP_MotorsHexa( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { }; // setup_motors - configures the motors for a quad diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 6a79d34cd7..2140b0f4e8 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -100,7 +100,7 @@ void AP_MotorsMatrix::output_min() // fill the motor_out[] array for HIL use and send minimum value to each motor for( i=0; iwrite(_motor_to_channel_map[i], _rc_throttle->radio_min); + hal.rcout->write(_motor_to_channel_map[i], _rc_throttle.radio_min); } } } @@ -110,8 +110,8 @@ void AP_MotorsMatrix::output_min() void AP_MotorsMatrix::output_armed() { int8_t i; - int16_t out_min_pwm = _rc_throttle->radio_min + _min_throttle; // minimum pwm value we can send to the motors - int16_t out_max_pwm = _rc_throttle->radio_max; // maximum pwm value we can send to the motors + int16_t out_min_pwm = _rc_throttle.radio_min + _min_throttle; // minimum pwm value we can send to the motors + int16_t out_max_pwm = _rc_throttle.radio_max; // maximum pwm value we can send to the motors int16_t out_mid_pwm = (out_min_pwm+out_max_pwm)/2; // mid pwm value we can send to the motors int16_t out_best_thr_pwm; // the is the best throttle we can come up which provides good control without climbing float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits @@ -132,23 +132,23 @@ void AP_MotorsMatrix::output_armed() // Throttle is 0 to 1000 only // To-Do: we should not really be limiting this here because we don't "own" this _rc_throttle object - if (_rc_throttle->servo_out < 0) { - _rc_throttle->servo_out = 0; + if (_rc_throttle.servo_out < 0) { + _rc_throttle.servo_out = 0; limit.throttle_lower = true; } - if (_rc_throttle->servo_out > _max_throttle) { - _rc_throttle->servo_out = _max_throttle; + if (_rc_throttle.servo_out > _max_throttle) { + _rc_throttle.servo_out = _max_throttle; limit.throttle_upper = true; } // capture desired roll, pitch, yaw and throttle from receiver - _rc_roll->calc_pwm(); - _rc_pitch->calc_pwm(); - _rc_throttle->calc_pwm(); - _rc_yaw->calc_pwm(); + _rc_roll.calc_pwm(); + _rc_pitch.calc_pwm(); + _rc_throttle.calc_pwm(); + _rc_yaw.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_ramped < 0) { _spin_when_armed_ramped = 0; @@ -159,7 +159,7 @@ void AP_MotorsMatrix::output_armed() for (i=0; iradio_min + _spin_when_armed_ramped; + motor_out[i] = _rc_throttle.radio_min + _spin_when_armed_ramped; } } @@ -171,7 +171,7 @@ void AP_MotorsMatrix::output_armed() } else { // check if throttle is below limit - if (_rc_throttle->radio_out <= out_min_pwm) { // perhaps being at min throttle itself is not a problem, only being under is + if (_rc_throttle.radio_out <= out_min_pwm) { // perhaps being at min throttle itself is not a problem, only being under is limit.throttle_lower = true; } @@ -179,8 +179,8 @@ void AP_MotorsMatrix::output_armed() // set rpy_low and rpy_high to the lowest and highest values of the motors for (i=0; ipwm_out * _roll_factor[i] + - _rc_pitch->pwm_out * _pitch_factor[i]; + rpy_out[i] = _rc_roll.pwm_out * _roll_factor[i] + + _rc_pitch.pwm_out * _pitch_factor[i]; // record lowest roll pitch command if (rpy_out[i] < rpy_low) { @@ -203,25 +203,25 @@ void AP_MotorsMatrix::output_armed() // We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control) // We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favour reducing throttle instead of better yaw control because the pilot has commanded it int16_t motor_mid = (rpy_low+rpy_high)/2; - out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle->radio_out, (_rc_throttle->radio_out+_hover_out)/2)); + out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, (_rc_throttle.radio_out+_hover_out)/2)); // calculate amount of yaw we can fit into the throttle range // this is always equal to or less than the requested yaw from the pilot or rate controller yaw_allowed = min(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2; yaw_allowed = max(yaw_allowed, AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM); - if (_rc_yaw->pwm_out >= 0) { + if (_rc_yaw.pwm_out >= 0) { // if yawing right - if (yaw_allowed > _rc_yaw->pwm_out) { - yaw_allowed = _rc_yaw->pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output + if (yaw_allowed > _rc_yaw.pwm_out) { + yaw_allowed = _rc_yaw.pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output }else{ limit.yaw = true; } }else{ // if yawing left yaw_allowed = -yaw_allowed; - if( yaw_allowed < _rc_yaw->pwm_out ) { - yaw_allowed = _rc_yaw->pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output + if( yaw_allowed < _rc_yaw.pwm_out ) { + yaw_allowed = _rc_yaw.pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output }else{ limit.yaw = true; } @@ -247,7 +247,7 @@ void AP_MotorsMatrix::output_armed() } // check everything fits - thr_adj = _rc_throttle->radio_out - out_best_thr_pwm; + thr_adj = _rc_throttle.radio_out - out_best_thr_pwm; // calc upper and lower limits of thr_adj int16_t thr_adj_max = out_max_pwm-(out_best_thr_pwm+rpy_high); @@ -356,9 +356,9 @@ void AP_MotorsMatrix::output_test() for( j=0; jwrite(_motor_to_channel_map[j], _rc_throttle->radio_min + _min_throttle); + 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.rcout->write(_motor_to_channel_map[j], _rc_throttle.radio_min); hal.scheduler->delay(2000); } } diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index 02ff2618bc..946e7bba50 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -21,7 +21,7 @@ class AP_MotorsMatrix : public AP_Motors { public: /// Constructor - AP_MotorsMatrix( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMatrix( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {}; diff --git a/libraries/AP_Motors/AP_MotorsOcta.h b/libraries/AP_Motors/AP_MotorsOcta.h index ec1c586510..0124e428b3 100644 --- a/libraries/AP_Motors/AP_MotorsOcta.h +++ b/libraries/AP_Motors/AP_MotorsOcta.h @@ -16,7 +16,7 @@ class AP_MotorsOcta : public AP_MotorsMatrix { public: /// Constructor - AP_MotorsOcta( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { + AP_MotorsOcta( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { }; // setup_motors - configures the motors for a quad diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.h b/libraries/AP_Motors/AP_MotorsOctaQuad.h index 1b0f44babb..e00dca743e 100644 --- a/libraries/AP_Motors/AP_MotorsOctaQuad.h +++ b/libraries/AP_Motors/AP_MotorsOctaQuad.h @@ -16,7 +16,7 @@ class AP_MotorsOctaQuad : public AP_MotorsMatrix { public: /// Constructor - AP_MotorsOctaQuad( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { + AP_MotorsOctaQuad( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { }; // setup_motors - configures the motors for a quad diff --git a/libraries/AP_Motors/AP_MotorsQuad.h b/libraries/AP_Motors/AP_MotorsQuad.h index 4a19fb297d..cd32ac5dfc 100644 --- a/libraries/AP_Motors/AP_MotorsQuad.h +++ b/libraries/AP_Motors/AP_MotorsQuad.h @@ -16,7 +16,7 @@ class AP_MotorsQuad : public AP_MotorsMatrix { public: /// Constructor - AP_MotorsQuad( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { + AP_MotorsQuad( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { }; // setup_motors - configures the motors for a quad diff --git a/libraries/AP_Motors/AP_MotorsY6.h b/libraries/AP_Motors/AP_MotorsY6.h index d7d0e9ab9b..a761edf6b1 100644 --- a/libraries/AP_Motors/AP_MotorsY6.h +++ b/libraries/AP_Motors/AP_MotorsY6.h @@ -18,7 +18,7 @@ class AP_MotorsY6 : public AP_MotorsMatrix { public: /// Constructor - AP_MotorsY6( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { + AP_MotorsY6( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { }; // setup_motors - configures the motors for a quad