From b8181b6b90fb2a5b5fbfaede335ec250d59e7cf3 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 14 May 2015 21:00:46 -0400 Subject: [PATCH] AP_Motors: RCMAP fix Remove all RC Input channels passed as reference into AP_Motors. All input handling self-contained inside AP_Motors. Rework Tricopter to use internal servo calcs. --- libraries/AP_Motors/AP_MotorsCoax.cpp | 51 +++---- libraries/AP_Motors/AP_MotorsCoax.h | 4 +- libraries/AP_Motors/AP_MotorsHeli.cpp | 39 ++++-- libraries/AP_Motors/AP_MotorsHeli.h | 18 ++- libraries/AP_Motors/AP_MotorsHexa.h | 5 +- libraries/AP_Motors/AP_MotorsMatrix.cpp | 75 ++++++----- libraries/AP_Motors/AP_MotorsMatrix.h | 4 +- libraries/AP_Motors/AP_MotorsOcta.h | 5 +- libraries/AP_Motors/AP_MotorsOctaQuad.h | 5 +- libraries/AP_Motors/AP_MotorsQuad.h | 5 +- libraries/AP_Motors/AP_MotorsSingle.cpp | 58 ++++---- libraries/AP_Motors/AP_MotorsSingle.h | 4 +- libraries/AP_Motors/AP_MotorsTri.cpp | 171 ++++++++++++++++-------- libraries/AP_Motors/AP_MotorsTri.h | 21 ++- libraries/AP_Motors/AP_MotorsY6.h | 5 +- libraries/AP_Motors/AP_Motors_Class.cpp | 36 ++--- libraries/AP_Motors/AP_Motors_Class.h | 59 ++++---- 17 files changed, 334 insertions(+), 231 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index be7ac4ddf7..cd45bc201e 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -119,13 +119,14 @@ void AP_MotorsCoax::output_min() // send minimum value to each motor hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_trim); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_trim); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _rc_throttle.radio_min); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _throttle_radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _throttle_radio_min); } void AP_MotorsCoax::output_armed_not_stabilizing() { - int16_t out_min = _rc_throttle.radio_min + _min_throttle; + int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 + int16_t out_min = _throttle_radio_min + _min_throttle; int16_t motor_out; int16_t min_thr = rel_pwm_to_thr_range(_spin_when_armed_ramped); @@ -136,18 +137,18 @@ void AP_MotorsCoax::output_armed_not_stabilizing() limit.throttle_lower = false; limit.throttle_upper = false; - if (_rc_throttle.servo_out <= min_thr) { - _rc_throttle.servo_out = min_thr; + if (_throttle_control_input <= min_thr) { + _throttle_control_input = min_thr; limit.throttle_lower = true; } - if (_rc_throttle.servo_out >= _max_throttle) { - _rc_throttle.servo_out = _max_throttle; + if (_throttle_control_input >= _max_throttle) { + _throttle_control_input = _max_throttle; limit.throttle_upper = true; } - _rc_throttle.calc_pwm(); + throttle_radio_output = calc_throttle_radio_output(); - motor_out = _rc_throttle.radio_out; + motor_out = throttle_radio_output; _servo1.servo_out = 0; _servo1.calc_pwm(); @@ -156,7 +157,7 @@ void AP_MotorsCoax::output_armed_not_stabilizing() _servo2.calc_pwm(); if (motor_out >= out_min) { - motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _rc_throttle.radio_max); + motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _throttle_radio_max); } hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_out); @@ -169,7 +170,9 @@ void AP_MotorsCoax::output_armed_not_stabilizing() // TODO pull code that is common to output_armed_not_stabilizing into helper functions void AP_MotorsCoax::output_armed_stabilizing() { - int16_t out_min = _rc_throttle.radio_min + _min_throttle; + int16_t yaw_pwm; // yaw pwm value, initially calculated by calc_yaw_pwm() but may be modified after, +/- 400 + int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 + int16_t out_min = _throttle_radio_min + _min_throttle; int16_t motor_out[4]; // initialize limits flags @@ -178,36 +181,36 @@ void AP_MotorsCoax::output_armed_stabilizing() limit.throttle_lower = false; limit.throttle_upper = false; - if (_rc_throttle.servo_out <= _min_throttle) { - _rc_throttle.servo_out = _min_throttle; + if (_throttle_control_input <= _min_throttle) { + _throttle_control_input = _min_throttle; limit.throttle_lower = true; } - if (_rc_throttle.servo_out >= _max_throttle) { - _rc_throttle.servo_out = _max_throttle; + if (_throttle_control_input >= _max_throttle) { + _throttle_control_input = _max_throttle; limit.throttle_upper = true; } - // capture desired throttle and yaw from receiver - _rc_throttle.calc_pwm(); - _rc_yaw.calc_pwm(); + // calculate throttle and yaw PWM + throttle_radio_output = calc_throttle_radio_output(); + yaw_pwm = calc_yaw_pwm(); // motors - motor_out[AP_MOTORS_MOT_3] = _rev_yaw*_rc_yaw.pwm_out + _rc_throttle.radio_out; - motor_out[AP_MOTORS_MOT_4] = -_rev_yaw*_rc_yaw.pwm_out +_rc_throttle.radio_out; + motor_out[AP_MOTORS_MOT_3] = _rev_yaw*yaw_pwm + throttle_radio_output; + motor_out[AP_MOTORS_MOT_4] = -_rev_yaw*yaw_pwm + throttle_radio_output; // TODO: set limits.roll_pitch and limits.yaw // front - _servo1.servo_out = _rev_roll*_rc_roll.servo_out; + _servo1.servo_out = _rev_roll*_roll_control_input; // right - _servo2.servo_out = _rev_pitch*_rc_pitch.servo_out; + _servo2.servo_out = _rev_pitch*_pitch_control_input; _servo1.calc_pwm(); _servo2.calc_pwm(); // adjust for thrust curve and voltage scaling - motor_out[AP_MOTORS_MOT_3] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_3], out_min, _rc_throttle.radio_max); - motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, _rc_throttle.radio_max); + motor_out[AP_MOTORS_MOT_3] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_3], out_min, _throttle_radio_max); + motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, _throttle_radio_max); // ensure motors don't drop below a minimum value and stop motor_out[AP_MOTORS_MOT_3] = max(motor_out[AP_MOTORS_MOT_3], out_min); diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index 58799c0c1b..047718f37f 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -25,8 +25,8 @@ class AP_MotorsCoax : public AP_Motors { public: /// Constructor - AP_MotorsCoax( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, RC_Channel& servo1, RC_Channel& servo2, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz), + AP_MotorsCoax(RC_Channel& servo1, RC_Channel& servo2, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_Motors(loop_rate, speed_hz), _servo1(servo1), _servo2(servo2) { diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 9e42c0c948..6c8a161226 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -403,18 +403,13 @@ void AP_MotorsHeli::output_armed_stabilizing() { // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash if (_servo_manual == 1) { - _rc_roll.servo_out = _rc_roll.control_in; - _rc_pitch.servo_out = _rc_pitch.control_in; - _rc_throttle.servo_out = _rc_throttle.control_in; - _rc_yaw.servo_out = _rc_yaw.control_in; + _roll_control_input = _roll_radio_passthrough; + _pitch_control_input = _pitch_radio_passthrough; + _throttle_control_input = _throttle_radio_passthrough; + _yaw_control_input = _yaw_radio_passthrough; } - _rc_roll.calc_pwm(); - _rc_pitch.calc_pwm(); - _rc_throttle.calc_pwm(); - _rc_yaw.calc_pwm(); - - move_swash(_rc_roll.servo_out, _rc_pitch.servo_out, _rc_throttle.servo_out, _rc_yaw.servo_out); + move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); // update rotor and direct drive esc speeds rsc_control(); @@ -456,7 +451,7 @@ void AP_MotorsHeli::reset_swash() // set roll, pitch and throttle scaling _roll_scaler = 1.0f; _pitch_scaler = 1.0f; - _collective_scalar = ((float)(_rc_throttle.radio_max - _rc_throttle.radio_min))/1000.0f; + _collective_scalar = ((float)(_throttle_radio_max - _throttle_radio_min))/1000.0f; _collective_scalar_manual = 1.0f; // we must be in set-up mode so mark swash as uninitialised @@ -568,7 +563,7 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll } // To-Do: This equation seems to be wrong. It probably restricts swash movement so that swash setup doesn't work right. // _collective_scalar should probably not be used or set to 1? - coll_out_scaled = coll_in * _collective_scalar + _rc_throttle.radio_min - 1000; + coll_out_scaled = coll_in * _collective_scalar + _throttle_radio_min - 1000; }else{ // regular flight mode // check if we need to reinitialise the swash @@ -846,5 +841,23 @@ void AP_MotorsHeli::update_throttle_filter() _throttle_filter.apply(_throttle_in, 1.0f/_loop_rate); // prevent _rc_throttle.servo_out from wrapping at int16 max or min - _rc_throttle.servo_out = constrain_float(_throttle_filter.get(),-32000,32000); + _throttle_control_input = constrain_float(_throttle_filter.get(),-32000,32000); +} + +// set_radio_passthrough used to pass radio inputs directly to outputs +void AP_MotorsHeli::set_radio_passthrough(int16_t radio_roll_input, int16_t radio_pitch_input, int16_t radio_throttle_input, int16_t radio_yaw_input) +{ + _roll_radio_passthrough = radio_roll_input; + _pitch_radio_passthrough = radio_pitch_input; + _throttle_radio_passthrough = radio_throttle_input; + _yaw_radio_passthrough = radio_yaw_input; +} + +// reset_radio_passthrough used to reset all radio inputs to center +void AP_MotorsHeli::reset_radio_passthrough() +{ + _roll_radio_passthrough = 0; + _pitch_radio_passthrough = 0; + _throttle_radio_passthrough = 500; + _yaw_radio_passthrough = 0; } diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index f94f92d63b..366381f4e7 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -89,11 +89,7 @@ class AP_MotorsHeli : public AP_Motors { public: /// Constructor - AP_MotorsHeli( RC_Channel& rc_roll, - RC_Channel& rc_pitch, - RC_Channel& rc_throttle, - RC_Channel& rc_yaw, - RC_Channel& servo_aux, + AP_MotorsHeli( RC_Channel& servo_aux, RC_Channel& servo_rotor, RC_Channel& swash_servo_1, RC_Channel& swash_servo_2, @@ -101,7 +97,7 @@ public: RC_Channel& yaw_servo, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : - AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz), + AP_Motors(loop_rate, speed_hz), _servo_aux(servo_aux), _servo_rsc(servo_rotor), _servo_1(swash_servo_1), @@ -207,6 +203,12 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict virtual uint16_t get_motor_mask(); + // set_radio_passthrough used to pass radio inputs directly to outputs + void set_radio_passthrough(int16_t radio_roll_input, int16_t radio_pitch_input, int16_t radio_throttle_input, int16_t radio_yaw_input); + + // reset_radio_passthrough used to reset all radio inputs to center + void reset_radio_passthrough(); + // output - sends commands to the motors void output(); @@ -310,6 +312,10 @@ private: int16_t _tail_direct_drive_out; // current ramped speed of output on ch7 when using direct drive variable pitch tail type float _dt; // main loop time int16_t _delta_phase_angle; // phase angle dynamic compensation + int16_t _roll_radio_passthrough; // roll control PWM direct from radio, used for manual control + int16_t _pitch_radio_passthrough; // pitch control PWM direct from radio, used for manual control + int16_t _throttle_radio_passthrough;// throttle control PWM direct from radio, used for manual control + int16_t _yaw_radio_passthrough; // yaw control PWM direct from radio, used for manual control }; #endif // AP_MOTORSHELI diff --git a/libraries/AP_Motors/AP_MotorsHexa.h b/libraries/AP_Motors/AP_MotorsHexa.h index 8afe215606..707d969f94 100644 --- a/libraries/AP_Motors/AP_MotorsHexa.h +++ b/libraries/AP_Motors/AP_MotorsHexa.h @@ -16,8 +16,9 @@ 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 loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) { - }; + AP_MotorsHexa(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMatrix(loop_rate, speed_hz) + { }; // setup_motors - configures the motors for a hexa virtual void setup_motors(); diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 13d5cd7c29..12e722c6ad 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(pgm_read_byte(&_motor_to_channel_map[i]), _rc_throttle.radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), _throttle_radio_min); } } } @@ -121,9 +121,10 @@ uint16_t AP_MotorsMatrix::get_motor_mask() void AP_MotorsMatrix::output_armed_not_stabilizing() { int8_t i; - int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent 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 throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 + int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent to the motors + int16_t out_min_pwm = _throttle_radio_min + _min_throttle; // minimum pwm value we can send to the motors + int16_t out_max_pwm = _throttle_radio_max; // maximum pwm value we can send to the motors // initialize limits flags limit.roll_pitch = true; @@ -133,26 +134,26 @@ void AP_MotorsMatrix::output_armed_not_stabilizing() int16_t min_thr = rel_pwm_to_thr_range(_spin_when_armed_ramped); - if (_rc_throttle.servo_out <= min_thr) { - _rc_throttle.servo_out = min_thr; + if (_throttle_control_input <= min_thr) { + _throttle_control_input = min_thr; limit.throttle_lower = true; } - if (_rc_throttle.servo_out >= _hover_out) { - _rc_throttle.servo_out = _hover_out; + if (_throttle_control_input >= _hover_out) { + _throttle_control_input = _hover_out; limit.throttle_upper = true; } - _rc_throttle.calc_pwm(); + throttle_radio_output = calc_throttle_radio_output(); // set output throttle for (i=0; i= out_min_pwm) { + if(throttle_radio_output >= out_min_pwm) { // apply thrust curve and voltage scaling for (i=0; i= _max_throttle) { - _rc_throttle.servo_out = _max_throttle; + if (_throttle_control_input >= _max_throttle) { + _throttle_control_input = _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(); + roll_pwm = calc_roll_pwm(); + pitch_pwm = calc_pitch_pwm(); + yaw_pwm = calc_yaw_pwm(); + throttle_radio_output = calc_throttle_radio_output(); // calculate roll and pitch for each motor // set rpy_low and rpy_high to the lowest and highest values of the motors for (i=0; i= 0) { + if (yaw_pwm >= 0) { // if yawing right - if (yaw_allowed > _rc_yaw.pwm_out * get_compensation_gain()) { - yaw_allowed = _rc_yaw.pwm_out * get_compensation_gain(); // 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 > yaw_pwm * get_compensation_gain()) { + yaw_allowed = yaw_pwm * get_compensation_gain(); // 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 * get_compensation_gain()) { - yaw_allowed = _rc_yaw.pwm_out * get_compensation_gain(); // 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 < yaw_pwm * get_compensation_gain()) { + yaw_allowed = yaw_pwm * get_compensation_gain(); // 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; } @@ -284,7 +287,7 @@ void AP_MotorsMatrix::output_armed_stabilizing() } // check everything fits - thr_adj = _rc_throttle.radio_out - out_best_thr_pwm; + thr_adj = throttle_radio_output - out_best_thr_pwm; // calculate upper and lower limits of thr_adj int16_t thr_adj_max = max(out_max_pwm-(out_best_thr_pwm+rpy_high),0); diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index fed3b489bb..2aefb4845e 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -19,8 +19,8 @@ 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 loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) + AP_MotorsMatrix(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_Motors(loop_rate, speed_hz) {}; // init diff --git a/libraries/AP_Motors/AP_MotorsOcta.h b/libraries/AP_Motors/AP_MotorsOcta.h index d3b7899488..75df137d9c 100644 --- a/libraries/AP_Motors/AP_MotorsOcta.h +++ b/libraries/AP_Motors/AP_MotorsOcta.h @@ -16,8 +16,9 @@ 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 loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) { - }; + AP_MotorsOcta(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMatrix(loop_rate, speed_hz) + { }; // setup_motors - configures the motors for an octa virtual void setup_motors(); diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.h b/libraries/AP_Motors/AP_MotorsOctaQuad.h index 9e8dc4b7c8..1b05a9a204 100644 --- a/libraries/AP_Motors/AP_MotorsOctaQuad.h +++ b/libraries/AP_Motors/AP_MotorsOctaQuad.h @@ -16,8 +16,9 @@ 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 loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) { - }; + AP_MotorsOctaQuad(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMatrix(loop_rate, speed_hz) + { }; // setup_motors - configures the motors for a quad virtual void setup_motors(); diff --git a/libraries/AP_Motors/AP_MotorsQuad.h b/libraries/AP_Motors/AP_MotorsQuad.h index 82327d1a1e..2d01b4cedf 100644 --- a/libraries/AP_Motors/AP_MotorsQuad.h +++ b/libraries/AP_Motors/AP_MotorsQuad.h @@ -16,8 +16,9 @@ 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 loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) { - }; + AP_MotorsQuad(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMatrix(loop_rate, speed_hz) + { }; // setup_motors - configures the motors for a quad virtual void setup_motors(); diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 8e344ecad3..f26b4143c3 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -126,7 +126,7 @@ void AP_MotorsSingle::output_min() hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_trim); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo3.radio_trim); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo4.radio_trim); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), _rc_throttle.radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), _throttle_radio_min); } // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) @@ -139,9 +139,8 @@ uint16_t AP_MotorsSingle::get_motor_mask() void AP_MotorsSingle::output_armed_not_stabilizing() { - int16_t out_min = _rc_throttle.radio_min + _min_throttle; - int16_t motor_out; - + int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 + int16_t out_min = _throttle_radio_min + _min_throttle; int16_t min_thr = rel_pwm_to_thr_range(_spin_when_armed_ramped); // initialize limits flags @@ -150,18 +149,16 @@ void AP_MotorsSingle::output_armed_not_stabilizing() limit.throttle_lower = false; limit.throttle_upper = false; - if (_rc_throttle.servo_out <= min_thr) { - _rc_throttle.servo_out = min_thr; + if (_throttle_control_input <= min_thr) { + _throttle_control_input = min_thr; limit.throttle_lower = true; } - if (_rc_throttle.servo_out >= _max_throttle) { - _rc_throttle.servo_out = _max_throttle; + if (_throttle_control_input >= _max_throttle) { + _throttle_control_input = _max_throttle; limit.throttle_upper = true; } - _rc_throttle.calc_pwm(); - - motor_out = _rc_throttle.radio_out; + throttle_radio_output = calc_throttle_radio_output(); // front servo _servo1.servo_out = 0; @@ -177,23 +174,23 @@ void AP_MotorsSingle::output_armed_not_stabilizing() _servo3.calc_pwm(); _servo4.calc_pwm(); - if (motor_out >= out_min) { - motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _rc_throttle.radio_max); + if (throttle_radio_output >= out_min) { + throttle_radio_output = apply_thrust_curve_and_volt_scaling(throttle_radio_output, out_min, _throttle_radio_max); } hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_out); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_out); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo3.radio_out); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo4.radio_out); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), motor_out); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), throttle_radio_output); } // sends commands to the motors // TODO pull code that is common to output_armed_not_stabilizing into helper functions void AP_MotorsSingle::output_armed_stabilizing() { - int16_t out_min = _rc_throttle.radio_min + _min_throttle; - int16_t motor_out; // main motor output + int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 + int16_t out_min = _throttle_radio_min + _min_throttle; // initialize limits flags limit.roll_pitch = false; @@ -202,37 +199,34 @@ void AP_MotorsSingle::output_armed_stabilizing() limit.throttle_upper = false; // Throttle is 0 to 1000 only - if (_rc_throttle.servo_out <= _min_throttle) { - _rc_throttle.servo_out = _min_throttle; + if (_throttle_control_input <= _min_throttle) { + _throttle_control_input = _min_throttle; limit.throttle_lower = true; } - if (_rc_throttle.servo_out >= _max_throttle) { - _rc_throttle.servo_out = _max_throttle; + if (_throttle_control_input >= _max_throttle) { + _throttle_control_input = _max_throttle; limit.throttle_upper = true; } - // capture desired throttle from receiver - _rc_throttle.calc_pwm(); - - //motor - motor_out = _rc_throttle.radio_out; + // calculate throttle PWM + throttle_radio_output = calc_throttle_radio_output(); // adjust for thrust curve and voltage scaling - motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _rc_throttle.radio_max); + throttle_radio_output = apply_thrust_curve_and_volt_scaling(throttle_radio_output, out_min, _throttle_radio_max); // ensure motor doesn't drop below a minimum value and stop - motor_out = max(motor_out, out_min); + throttle_radio_output = max(throttle_radio_output, out_min); // TODO: set limits.roll_pitch and limits.yaw // front servo - _servo1.servo_out = _rev_roll*_rc_roll.servo_out + _rev_yaw*_rc_yaw.servo_out; + _servo1.servo_out = _rev_roll*_roll_control_input + _rev_yaw*_yaw_control_input; // right servo - _servo2.servo_out = _rev_pitch*_rc_pitch.servo_out + _rev_yaw*_rc_yaw.servo_out; + _servo2.servo_out = _rev_pitch*_pitch_control_input + _rev_yaw*_yaw_control_input; // rear servo - _servo3.servo_out = -_rev_roll*_rc_roll.servo_out + _rev_yaw*_rc_yaw.servo_out; + _servo3.servo_out = -_rev_roll*_roll_control_input + _rev_yaw*_yaw_control_input; // left servo - _servo4.servo_out = -_rev_pitch*_rc_pitch.servo_out + _rev_yaw*_rc_yaw.servo_out; + _servo4.servo_out = -_rev_pitch*_pitch_control_input + _rev_yaw*_yaw_control_input; _servo1.calc_pwm(); _servo2.calc_pwm(); @@ -244,7 +238,7 @@ void AP_MotorsSingle::output_armed_stabilizing() hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_out); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo3.radio_out); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo4.radio_out); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), motor_out); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), throttle_radio_output); } // output_disarmed - sends commands to the motors diff --git a/libraries/AP_Motors/AP_MotorsSingle.h b/libraries/AP_Motors/AP_MotorsSingle.h index f8f07ed979..a86cda349f 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.h +++ b/libraries/AP_Motors/AP_MotorsSingle.h @@ -25,8 +25,8 @@ 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 loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz), + AP_MotorsSingle(RC_Channel& servo1, RC_Channel& servo2, RC_Channel& servo3, RC_Channel& servo4, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_Motors(loop_rate, speed_hz), _servo1(servo1), _servo2(servo2), _servo3(servo3), diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 26855f84b2..bfcdc86506 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -25,6 +25,48 @@ extern const AP_HAL::HAL& hal; +const AP_Param::GroupInfo AP_MotorsTri::var_info[] PROGMEM = { + // variables from parent vehicle + AP_NESTEDGROUPINFO(AP_Motors, 0), + + // @Param: YAW_SV_REV + // @DisplayName: Yaw Servo Reverse + // @Description: Yaw servo signal reversing + // @Range: 0, 1 + // @User: Standard + AP_GROUPINFO("YAW_SV_REV", 1, AP_MotorsTri, _yaw_servo_reverse, 0), + + // @Param: YAW_SV_TRIM + // @DisplayName: Yaw Servo Trim/Center + // @Description: Trim or center position of yaw servo + // @Range: 1250 1750 + // @Units: PWM + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("YAW_SV_TRIM", 2, AP_MotorsTri, _yaw_servo_trim, 1500), + + // @Param: YAW_SV_MIN + // @DisplayName: Yaw Servo Min Position + // @Description: Minimum angle limit of yaw servo + // @Range: 1000 1400 + // @Units: PWM + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("YAW_SV_MIN", 3, AP_MotorsTri, _yaw_servo_min, 1250), + + // @Param: YAW_SV_MAX + // @DisplayName: Yaw Servo Max Position + // @Description: Maximum angle limit of yaw servo + // @Range: 1600 2000 + // @Units: PWM + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("YAW_SV_MAX", 4, AP_MotorsTri, _yaw_servo_max, 1750), + + + AP_GROUPEND +}; + // init void AP_MotorsTri::Init() { @@ -74,17 +116,17 @@ void AP_MotorsTri::output_min() limit.throttle_lower = true; // send minimum value to each motor - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _rc_throttle.radio_min); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _rc_throttle.radio_min); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min); - hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_trim); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _throttle_radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _throttle_radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _throttle_radio_min); + hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim); } // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict uint16_t AP_MotorsTri::get_motor_mask() { - // tri copter uses channels 1,2,4 and 7 + // tri copter uses channels 1,2,3 and 4 return (1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1])) | (1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2])) | (1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4])) | @@ -93,8 +135,9 @@ uint16_t AP_MotorsTri::get_motor_mask() void AP_MotorsTri::output_armed_not_stabilizing() { - int16_t out_min = _rc_throttle.radio_min + _min_throttle; - int16_t out_max = _rc_throttle.radio_max; + int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 + int16_t out_min = _throttle_radio_min + _min_throttle; + int16_t out_max = _throttle_radio_max; int16_t motor_out[AP_MOTORS_MOT_4+1]; // initialize limits flags @@ -105,26 +148,23 @@ void AP_MotorsTri::output_armed_not_stabilizing() int16_t min_thr = rel_pwm_to_thr_range(_spin_when_armed_ramped); - if (_rc_throttle.servo_out <= min_thr) { - _rc_throttle.servo_out = min_thr; + if (_throttle_control_input <= min_thr) { + _throttle_control_input = min_thr; limit.throttle_lower = true; } - if (_rc_throttle.servo_out >= _hover_out) { - _rc_throttle.servo_out = _hover_out; + if (_throttle_control_input >= _hover_out) { + _throttle_control_input = _hover_out; limit.throttle_upper = true; } - _rc_yaw.servo_out=0; - _rc_yaw.calc_pwm(); + throttle_radio_output = calc_throttle_radio_output(); - _rc_throttle.calc_pwm(); + motor_out[AP_MOTORS_MOT_1] = throttle_radio_output; + motor_out[AP_MOTORS_MOT_2] = throttle_radio_output; + motor_out[AP_MOTORS_MOT_4] = throttle_radio_output; - motor_out[AP_MOTORS_MOT_1] = _rc_throttle.radio_out; - motor_out[AP_MOTORS_MOT_2] = _rc_throttle.radio_out; - motor_out[AP_MOTORS_MOT_4] = _rc_throttle.radio_out; - - if(_rc_throttle.radio_out >= out_min) { + if(throttle_radio_output >= out_min) { // adjust for thrust curve and voltage scaling motor_out[AP_MOTORS_MOT_1] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_1], out_min, out_max); motor_out[AP_MOTORS_MOT_2] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_2], out_min, out_max); @@ -136,19 +176,20 @@ void AP_MotorsTri::output_armed_not_stabilizing() hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), motor_out[AP_MOTORS_MOT_2]); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), motor_out[AP_MOTORS_MOT_4]); - if( _rc_tail.get_reverse() == true ) { - hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_trim - (_rc_yaw.radio_out - _rc_yaw.radio_trim)); - }else{ - hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_out); - } + // send centering signal to yaw servo + hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim); } // sends commands to the motors // TODO pull code that is common to output_armed_not_stabilizing into helper functions void AP_MotorsTri::output_armed_stabilizing() { - int16_t out_min = _rc_throttle.radio_min + _min_throttle; - int16_t out_max = _rc_throttle.radio_max; + int16_t roll_pwm; // roll pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400 + int16_t pitch_pwm; // pitch pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400 + int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 + int16_t yaw_radio_output; // final yaw pwm value sent to motors, typically ~1100-1900 + int16_t out_min = _throttle_radio_min + _min_throttle; + int16_t out_max = _throttle_radio_max; int16_t motor_out[AP_MOTORS_MOT_4+1]; // initialize limits flags @@ -158,30 +199,29 @@ void AP_MotorsTri::output_armed_stabilizing() limit.throttle_upper = false; // Throttle is 0 to 1000 only - if (_rc_throttle.servo_out <= 0) { - _rc_throttle.servo_out = 0; + if (_throttle_control_input <= 0) { + _throttle_control_input = 0; limit.throttle_lower = true; } - if (_rc_throttle.servo_out >= _max_throttle) { - _rc_throttle.servo_out = _max_throttle; + if (_throttle_control_input >= _max_throttle) { + _throttle_control_input = _max_throttle; limit.throttle_upper = true; } // tricopters limit throttle to 80% // To-Do: implement improved stability patch and remove this limit - if (_rc_throttle.servo_out > 800) { - _rc_throttle.servo_out = 800; + if (_throttle_control_input > 800) { + _throttle_control_input = 800; 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(); + roll_pwm = calc_roll_pwm(); + pitch_pwm = calc_pitch_pwm(); + throttle_radio_output = calc_throttle_radio_output(); + yaw_radio_output = calc_yaw_radio_output(); // if we are not sending a throttle output, we cut the motors - if(_rc_throttle.servo_out == 0) { + if( is_zero(_throttle_control_input) ) { // range check spin_when_armed if (_spin_when_armed_ramped < 0) { _spin_when_armed_ramped = 0; @@ -189,29 +229,29 @@ void AP_MotorsTri::output_armed_stabilizing() if (_spin_when_armed_ramped > _min_throttle) { _spin_when_armed_ramped = _min_throttle; } - motor_out[AP_MOTORS_MOT_1] = _rc_throttle.radio_min + _spin_when_armed_ramped; - motor_out[AP_MOTORS_MOT_2] = _rc_throttle.radio_min + _spin_when_armed_ramped; - motor_out[AP_MOTORS_MOT_4] = _rc_throttle.radio_min + _spin_when_armed_ramped; + motor_out[AP_MOTORS_MOT_1] = _throttle_radio_min + _spin_when_armed_ramped; + motor_out[AP_MOTORS_MOT_2] = _throttle_radio_min + _spin_when_armed_ramped; + motor_out[AP_MOTORS_MOT_4] = _throttle_radio_min + _spin_when_armed_ramped; }else{ - int16_t roll_out = (float)_rc_roll.pwm_out * 0.866f; - int16_t pitch_out = _rc_pitch.pwm_out / 2; + int16_t roll_out = (float)(roll_pwm * 0.866f); + int16_t pitch_out = pitch_pwm / 2; // check if throttle is below limit - if (_rc_throttle.servo_out <= _min_throttle) { + if (_throttle_control_input <= _min_throttle) { limit.throttle_lower = true; - _rc_throttle.servo_out = _min_throttle; - _rc_throttle.calc_pwm(); // recalculate radio.out + _throttle_control_input = _min_throttle; + throttle_radio_output = calc_throttle_radio_output(); } // TODO: set limits.roll_pitch and limits.yaw //left front - motor_out[AP_MOTORS_MOT_2] = _rc_throttle.radio_out + roll_out + pitch_out; + motor_out[AP_MOTORS_MOT_2] = throttle_radio_output + roll_out + pitch_out; //right front - motor_out[AP_MOTORS_MOT_1] = _rc_throttle.radio_out - roll_out + pitch_out; + motor_out[AP_MOTORS_MOT_1] = throttle_radio_output - roll_out + pitch_out; // rear - motor_out[AP_MOTORS_MOT_4] = _rc_throttle.radio_out - _rc_pitch.pwm_out; + motor_out[AP_MOTORS_MOT_4] = throttle_radio_output - pitch_pwm; // Tridge's stability patch if(motor_out[AP_MOTORS_MOT_1] > out_max) { @@ -248,15 +288,8 @@ void AP_MotorsTri::output_armed_stabilizing() hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), motor_out[AP_MOTORS_MOT_2]); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), motor_out[AP_MOTORS_MOT_4]); - // also send out to tail command (we rely on any auto pilot to have updated the rc_yaw->radio_out to the correct value) - // note we do not save the radio_out to the motor_out array so it may not appear in the ch7out in the status screen of the mission planner - // note: we use _rc_tail's (aka channel 7's) REV parameter to control whether the servo is reversed or not but this is a bit nonsensical. - // a separate servo object (including min, max settings etc) would be better or at least a separate parameter to specify the direction of the tail servo - if( _rc_tail.get_reverse() == true ) { - hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_trim - (_rc_yaw.radio_out - _rc_yaw.radio_trim)); - }else{ - hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_out); - } + // send out to yaw command to tail servo + hal.rcout->write(AP_MOTORS_CH_TRI_YAW, yaw_radio_output); } // output_disarmed - sends commands to the motors @@ -299,3 +332,25 @@ void AP_MotorsTri::output_test(uint8_t motor_seq, int16_t pwm) break; } } + +// calc_yaw_radio_output - calculate final radio output for yaw channel +int16_t AP_MotorsTri::calc_yaw_radio_output() +{ + int16_t ret; + + if (_yaw_servo_reverse){ + if (_yaw_control_input >= 0){ + ret = (_yaw_servo_trim - (_yaw_control_input/4500 * (_yaw_servo_trim - _yaw_servo_min))); + } else { + ret = (_yaw_servo_trim - (_yaw_control_input/4500 * (_yaw_servo_max - _yaw_servo_trim))); + } + } else { + if (_yaw_control_input >= 0){ + ret = ((_yaw_control_input/4500 * (_yaw_servo_max - _yaw_servo_trim)) + _yaw_servo_trim); + } else { + ret = ((_yaw_control_input/4500 * (_yaw_servo_trim - _yaw_servo_min)) + _yaw_servo_trim); + } + } + + return ret; +} diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 9ec9a0cedd..f66984a21b 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -12,16 +12,17 @@ #include "AP_Motors.h" // tail servo uses channel 7 -#define AP_MOTORS_CH_TRI_YAW CH_7 +#define AP_MOTORS_CH_TRI_YAW CH_3 /// @class AP_MotorsTri class AP_MotorsTri : public AP_Motors { public: /// Constructor - AP_MotorsTri(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, RC_Channel& rc_tail, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz), - _rc_tail(rc_tail) { + AP_MotorsTri(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_Motors(loop_rate, speed_hz) + { + AP_Param::setup_object_defaults(this, var_info); }; // init @@ -45,13 +46,23 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict virtual uint16_t get_motor_mask(); + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + protected: // output - sends commands to the motors void output_armed_stabilizing(); void output_armed_not_stabilizing(); void output_disarmed(); - RC_Channel& _rc_tail; // REV parameter used from this channel to determine direction of tail servo movement + // calc_yaw_radio_output - calculate final radio output for yaw channel + int16_t calc_yaw_radio_output(); // calculate radio output for yaw servo, typically in range of 1100-1900 + + // parameters + AP_Int8 _yaw_servo_reverse; // Yaw servo signal reversing + AP_Int16 _yaw_servo_trim; // Trim or center position of yaw servo + AP_Int16 _yaw_servo_min; // Minimum angle limit of yaw servo + AP_Int16 _yaw_servo_max; // Maximum angle limit of yaw servo }; #endif // AP_MOTORSTRI diff --git a/libraries/AP_Motors/AP_MotorsY6.h b/libraries/AP_Motors/AP_MotorsY6.h index 792e1ba163..88d86c9e32 100644 --- a/libraries/AP_Motors/AP_MotorsY6.h +++ b/libraries/AP_Motors/AP_MotorsY6.h @@ -18,8 +18,9 @@ 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 loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) { - }; + AP_MotorsY6(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMatrix(loop_rate, speed_hz) + { }; // setup_motors - configures the motors for a Y6 virtual void setup_motors(); diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index d1f880cec5..23de0a37b4 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -104,11 +104,7 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = { }; // Constructor -AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz) : - _rc_roll(rc_roll), - _rc_pitch(rc_pitch), - _rc_throttle(rc_throttle), - _rc_yaw(rc_yaw), +AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) : _loop_rate(loop_rate), _speed_hz(speed_hz), _min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE), @@ -152,16 +148,20 @@ void AP_Motors::armed(bool arm) AP_Notify::flags.armed = arm; }; -// set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle) -void AP_Motors::set_min_throttle(uint16_t min_throttle) +// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle) +// also sets throttle channel minimum and maximum pwm +void AP_Motors::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max) { - _min_throttle = (float)min_throttle * (_rc_throttle.radio_max - _rc_throttle.radio_min) / 1000.0f; + _throttle_radio_min = radio_min; + _throttle_radio_max = radio_max; + _throttle_pwm_scalar = (_throttle_radio_max - _throttle_radio_min) / 1000.0f; + _min_throttle = (float)min_throttle * _throttle_pwm_scalar; } // get_hover_throttle_as_pwm - converts hover throttle to pwm (i.e. range 1000 ~ 2000) int16_t AP_Motors::get_hover_throttle_as_pwm() const { - return (_rc_throttle.radio_min + (float)(_rc_throttle.radio_max - _rc_throttle.radio_min) * _hover_out / 1000.0f); + return (_throttle_radio_min + (float)(_throttle_radio_max - _throttle_radio_min) * _hover_out / 1000.0f); } // throttle_pass_through - passes provided pwm directly to all motors - dangerous but used for initialising ESCs @@ -217,7 +217,7 @@ void AP_Motors::slow_start(bool true_false) _flags.slow_start = true; // initialise maximum throttle to current throttle - _max_throttle = constrain_int16(_rc_throttle.servo_out, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE); + _max_throttle = constrain_int16(_throttle_control_input, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE); } // update the throttle input filter @@ -229,8 +229,8 @@ void AP_Motors::update_throttle_filter() _throttle_filter.reset(0.0f); } - // prevent _rc_throttle.servo_out from wrapping at int16 max or min - _rc_throttle.servo_out = constrain_float(_throttle_filter.get(),-32000,32000); + // prevent _throttle_control_input from wrapping at int16 max or min + _throttle_control_input = constrain_float(_throttle_filter.get(),-32000,32000); } // update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag @@ -251,7 +251,7 @@ void AP_Motors::update_max_throttle() _max_throttle += AP_MOTOR_SLOW_START_INCREMENT; // turn off slow start if we've reached max throttle - if (_max_throttle >= _rc_throttle.servo_out) { + if (_max_throttle >= _throttle_control_input) { _max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE; _flags.slow_start = false; } @@ -273,7 +273,7 @@ void AP_Motors::current_limit_max_throttle() } // remove throttle limit if throttle is at zero or disarmed - if(_rc_throttle.servo_out <= 0 || !_flags.armed) { + if(_throttle_control_input <= 0 || !_flags.armed) { _throttle_limit = 1.0f; } @@ -342,14 +342,14 @@ void AP_Motors::update_lift_max_from_batt_voltage() void AP_Motors::update_battery_resistance() { // if motors are stopped, reset resting voltage and current - if (_rc_throttle.servo_out <= 0 || !_flags.armed) { + if (_throttle_control_input <= 0 || !_flags.armed) { _batt_voltage_resting = _batt_voltage; _batt_current_resting = _batt_current; _batt_timer = 0; } else { // update battery resistance when throttle is over hover throttle if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) { - if (_rc_throttle.servo_out >= _hover_out) { + if (_throttle_control_input >= _hover_out) { // filter reaches 90% in 1/4 the test time _batt_resistance += 0.05f*(( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting) ) - _batt_resistance); _batt_timer += 1; @@ -393,10 +393,10 @@ float AP_Motors::get_compensation_gain() const float AP_Motors::rel_pwm_to_thr_range(float pwm) const { - return 1000.0f*pwm/(_rc_throttle.radio_max-_rc_throttle.radio_min); + return pwm/_throttle_pwm_scalar; } float AP_Motors::thr_range_to_rel_pwm(float thr) const { - return (_rc_throttle.radio_max-_rc_throttle.radio_min)*thr/1000.0f; + return _throttle_pwm_scalar*thr; } diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 0f343244cd..83055ea05c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -89,7 +89,7 @@ class AP_Motors { public: // Constructor - AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); + AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); // init virtual void Init() {} @@ -113,8 +113,9 @@ public: // get motor interlock status. true means motors run, false motors don't run bool get_interlock() const { return _flags.interlock; }; - // set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle) - void set_min_throttle(uint16_t min_throttle); + // set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle) + // also sets throttle channel minimum and maximum pwm + void set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max); // set_hover_throttle - sets the mid throttle which is close to the hover throttle of the copter // this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control @@ -127,17 +128,17 @@ public: int16_t throttle_max() const { return _max_throttle;} // set_roll, set_pitch, set_yaw, set_throttle - void set_roll(int16_t roll_in) { _rc_roll.servo_out = roll_in; }; // range -4500 ~ 4500 - void set_pitch(int16_t pitch_in) { _rc_pitch.servo_out = pitch_in; }; // range -4500 ~ 4500 - void set_yaw(int16_t yaw_in) { _rc_yaw.servo_out = yaw_in; }; // range -4500 ~ 4500 - void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1000 + void set_roll(int16_t roll_in) { _roll_control_input = roll_in; }; // range -4500 ~ 4500 + void set_pitch(int16_t pitch_in) { _pitch_control_input = pitch_in; }; // range -4500 ~ 4500 + void set_yaw(int16_t yaw_in) { _yaw_control_input = yaw_in; }; // range -4500 ~ 4500 + void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1000 void set_stabilizing(bool stabilizing) { _flags.stabilizing = stabilizing; } // accessors for roll, pitch, yaw and throttle inputs to motors - int16_t get_roll() const { return _rc_roll.servo_out; } - int16_t get_pitch() const { return _rc_pitch.servo_out; } - int16_t get_yaw() const { return _rc_yaw.servo_out; } - int16_t get_throttle_out() const { return _rc_throttle.servo_out; } + float get_roll() const { return _roll_control_input; } + float get_pitch() const { return _pitch_control_input; } + float get_yaw() const { return _yaw_control_input; } + float get_throttle() const { return _throttle_control_input; } void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); } @@ -249,6 +250,15 @@ protected: float rel_pwm_to_thr_range(float pwm) const; float thr_range_to_rel_pwm(float thr) const; + // convert RPY and Throttle servo ranges from legacy controller scheme back into PWM values + // RPY channels typically +/-45 degrees servo travel between +/-400 PWM + // Throttle channel typically 0-1000 range converts to 1100-1900 PWM for final output signal to motors + // ToDo: this should all be handled as floats +/- 1.0 instead of PWM and fake angle ranges + int16_t calc_roll_pwm() { return (_roll_control_input / 11.25);} + int16_t calc_pitch_pwm() { return (_pitch_control_input / 11.25);} + int16_t calc_yaw_pwm() { return (_yaw_control_input / 11.25);} + int16_t calc_throttle_radio_output() { return (_throttle_control_input * _throttle_pwm_scalar) + _throttle_radio_min;} + // flag bitmask struct AP_Motors_flags { uint8_t armed : 1; // 0 if disarmed, 1 if armed @@ -274,18 +284,21 @@ protected: AP_Float _thr_mix_min; // current over which maximum throttle is limited // internal variables - RC_Channel& _rc_roll; // roll input in from users is held in servo_out - RC_Channel& _rc_pitch; // pitch input in from users is held in servo_out - RC_Channel& _rc_throttle; // throttle input in from users is held in servo_out - RC_Channel& _rc_yaw; // yaw input in from users is held in servo_out - uint16_t _loop_rate; // rate at which output() function is called (normally 400hz) - uint16_t _speed_hz; // speed in hz to send updates to motors - int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying) - int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start) - int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000) - int16_t _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero - float _throttle_thr_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 - float _throttle_thr_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds + float _roll_control_input; // desired roll control from attitude controllers, +/- 4500 + float _pitch_control_input; // desired pitch control from attitude controller, +/- 4500 + float _throttle_control_input; // desired throttle (thrust) control from attitude controller, 0-1000 + float _yaw_control_input; // desired yaw control from attitude controller, +/- 4500 + float _throttle_pwm_scalar; // scalar used to convert throttle channel pwm range into 0-1000 range, ~0.8 - 1.0 + uint16_t _loop_rate; // rate at which output() function is called (normally 400hz) + uint16_t _speed_hz; // speed in hz to send updates to motors + int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying) + int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start) + int16_t _throttle_radio_min; // minimum radio channel pwm + int16_t _throttle_radio_max; // maximum radio channel pwm + int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000) + int16_t _spin_when_armed_ramped; // equal to _spin_when_armed parameter but slowly ramped up from zero + float _throttle_thr_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 + float _throttle_thr_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds // battery voltage compensation variables float _batt_voltage; // latest battery voltage reading