diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index d5a563c613..2c6f40afa6 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -62,7 +62,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 speed_hz ) : +AP_Motors::AP_Motors( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz ) : _rc_roll(rc_roll), _rc_pitch(rc_pitch), _rc_throttle(rc_throttle), @@ -107,14 +107,14 @@ void AP_Motors::armed(bool 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) { - _min_throttle = (float)min_throttle * (_rc_throttle->radio_max - _rc_throttle->radio_min) / 1000.0f; + _min_throttle = (float)min_throttle * (_rc_throttle.radio_max - _rc_throttle.radio_min) / 1000.0f; } // set_mid_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 void AP_Motors::set_mid_throttle(uint16_t mid_throttle) { - _hover_out = _rc_throttle->radio_min + (float)(_rc_throttle->radio_max - _rc_throttle->radio_min) * mid_throttle / 1000.0f; + _hover_out = _rc_throttle.radio_min + (float)(_rc_throttle.radio_max - _rc_throttle.radio_min) * mid_throttle / 1000.0f; } // throttle_pass_through - passes pilot's throttle input directly to all motors - dangerous but used for initialising ESCs @@ -124,7 +124,7 @@ void AP_Motors::throttle_pass_through() // send the pilot's input directly to each enabled motor for (int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { - hal.rcout->write(_motor_to_channel_map[i], _rc_throttle->radio_in); + hal.rcout->write(_motor_to_channel_map[i], _rc_throttle.radio_in); } } } @@ -148,8 +148,8 @@ void AP_Motors::output() // returns true if set up successfully bool AP_Motors::setup_throttle_curve() { - int16_t min_pwm = _rc_throttle->radio_min; - int16_t max_pwm = _rc_throttle->radio_max; + int16_t min_pwm = _rc_throttle.radio_min; + int16_t max_pwm = _rc_throttle.radio_max; int16_t mid_throttle_pwm = (max_pwm + min_pwm) / 2; int16_t mid_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_mid/100.0f); int16_t max_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_max/100.0f); @@ -189,7 +189,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(_rc_throttle.servo_out, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE); } // update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag @@ -213,7 +213,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 >= _rc_throttle.servo_out) { _max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE; _flags.slow_start = false; } diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 79217bf218..4babe5cdf9 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -68,7 +68,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 speed_hz = AP_MOTORS_SPEED_DEFAULT); + AP_Motors( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); // init virtual void Init(); @@ -107,6 +107,12 @@ public: int16_t throttle_min() const { return _min_throttle;} 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(int16_t throttle_in) { _rc_throttle.servo_out = throttle_in; }; // range 0 ~ 1000 + // output - sends commands to the motors void output(); @@ -166,7 +172,10 @@ protected: AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min // internal variables - RC_Channel* _rc_roll, *_rc_pitch, *_rc_throttle, *_rc_yaw; // input in from users + 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 uint8_t _motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS]; // mapping of motor number (as received from upper APM code) to RC channel output - used to account for differences between APM1 and APM2 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)