diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 452b7e1ed5..04c91ca423 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -26,13 +26,12 @@ extern const AP_HAL::HAL& hal; - // init void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_type) { // make sure 6 output channels are mapped - for (uint8_t i=0; i<6; i++) { - add_motor_num(CH_1+i); + for (uint8_t i = 0; i < 6; i++) { + add_motor_num(CH_1 + i); } // set the motor_enabled flag so that the main ESC can be calibrated like other frame types @@ -40,10 +39,10 @@ void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_t motor_enabled[AP_MOTORS_MOT_6] = true; // setup actuator scaling - for (uint8_t i=0; i 1.0f) { limit.roll_pitch = true; _actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f); diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index c7d9230282..50d08cbd01 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -38,18 +38,18 @@ void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame } // set update rate to motors - a value in hertz -void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz ) +void AP_MotorsMatrix::set_update_rate(uint16_t speed_hz) { // record requested speed _speed_hz = speed_hz; uint16_t mask = 0; - for (uint8_t i=0; i rp_high && (!_thrust_boost || i != _motor_lost_index)) { rp_high = _thrust_rpyt_out[i]; } @@ -210,14 +209,14 @@ void AP_MotorsMatrix::output_armed_stabilizing() // include the lost motor scaled by _thrust_boost_ratio if (_thrust_boost && motor_enabled[_motor_lost_index]) { - // record highest roll+pitch command + // record highest roll + pitch command if (_thrust_rpyt_out[_motor_lost_index] > rp_high) { - rp_high = _thrust_boost_ratio*rp_high + (1.0f-_thrust_boost_ratio)*_thrust_rpyt_out[_motor_lost_index]; + rp_high = _thrust_boost_ratio * rp_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index]; } } // check for roll and pitch saturation - if (rp_high-rp_low > 1.0f || throttle_avg_max < -rp_low) { + if (rp_high - rp_low > 1.0f || throttle_avg_max < -rp_low) { // Full range is being used by roll and pitch. limit.roll_pitch = true; } @@ -228,8 +227,8 @@ void AP_MotorsMatrix::output_armed_stabilizing() // calculate the maximum yaw control that can be used // todo: make _yaw_headroom 0 to 1 yaw_allowed = (float)_yaw_headroom / 1000.0f; - yaw_allowed = _thrust_boost_ratio*0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed; - yaw_allowed = MAX(MIN(throttle_thrust_best_rpy+rp_low, 1.0f - (throttle_thrust_best_rpy + rp_high)), yaw_allowed); + yaw_allowed = _thrust_boost_ratio * 0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed; + yaw_allowed = MAX(MIN(throttle_thrust_best_rpy + rp_low, 1.0f - (throttle_thrust_best_rpy + rp_high)), yaw_allowed); if (fabsf(yaw_thrust) > yaw_allowed) { // not all commanded yaw can be used yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed); @@ -239,15 +238,15 @@ void AP_MotorsMatrix::output_armed_stabilizing() // add yaw control to thrust outputs float rpy_low = 1.0f; // lowest thrust value float rpy_high = -1.0f; // highest thrust value - for (i=0; i rpy_high && (!_thrust_boost || i != _motor_lost_index)) { rpy_high = _thrust_rpyt_out[i]; } @@ -255,15 +254,15 @@ void AP_MotorsMatrix::output_armed_stabilizing() } // include the lost motor scaled by _thrust_boost_ratio if (_thrust_boost) { - // record highest roll+pitch+yaw command + // record highest roll + pitch + yaw command if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index]) { - rpy_high = _thrust_boost_ratio*rpy_high + (1.0f-_thrust_boost_ratio)*_thrust_rpyt_out[_motor_lost_index]; + rpy_high = _thrust_boost_ratio * rpy_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index]; } } // calculate any scaling needed to make the combined thrust outputs fit within the output range - if (rpy_high-rpy_low > 1.0f) { - rpy_scale = 1.0f / (rpy_high-rpy_low); + if (rpy_high - rpy_low > 1.0f) { + rpy_scale = 1.0f / (rpy_high - rpy_low); } if (is_negative(rpy_low)) { rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low); @@ -295,7 +294,7 @@ void AP_MotorsMatrix::output_armed_stabilizing() } // add scaled roll, pitch, constrained yaw and throttle for each motor - for (i=0; i AP_MOTORS_MAX_NUM_MOTORS -1) { + if (output_channel > AP_MOTORS_MAX_NUM_MOTORS - 1) { return false; } @@ -407,10 +406,10 @@ bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm) void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order) { // ensure valid motor number is provided - if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { + if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) { // increment number of motors if this motor is being newly motor_enabled - if( !motor_enabled[motor_num] ) { + if (!motor_enabled[motor_num]) { motor_enabled[motor_num] = true; } @@ -448,7 +447,7 @@ void AP_MotorsMatrix::add_motor(int8_t motor_num, float roll_factor_in_degrees, void AP_MotorsMatrix::remove_motor(int8_t motor_num) { // ensure valid motor number is provided - if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { + if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) { // disable the motor, set all factors to zero motor_enabled[motor_num] = false; _roll_factor[motor_num] = 0; @@ -460,7 +459,7 @@ void AP_MotorsMatrix::remove_motor(int8_t motor_num) void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) { // remove existing motors - for (int8_t i=0; i 0) { float throttle = constrain_float(get_throttle() * _boost_scale, 0, 1); - SRV_Channels::set_output_scaled(SRV_Channel::k_boost_throttle, throttle*1000); + SRV_Channels::set_output_scaled(SRV_Channel::k_boost_throttle, throttle * 1000); } } - // sends minimum values out to the motors void AP_MotorsMulticopter::output_min() @@ -270,7 +269,7 @@ void AP_MotorsMulticopter::output_min() void AP_MotorsMulticopter::update_throttle_filter() { if (armed()) { - _throttle_filter.apply(_throttle_in, 1.0f/_loop_rate); + _throttle_filter.apply(_throttle_in, 1.0f / _loop_rate); // constrain filtered throttle if (_throttle_filter.get() < 0.0f) { _throttle_filter.reset(0.0f); @@ -305,18 +304,18 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle() float _batt_current = battery.current_amps(_batt_idx); // calculate the maximum current to prevent voltage sag below _batt_voltage_min - float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx)-_batt_voltage_min)/_batt_resistance); + float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx) - _batt_voltage_min) / _batt_resistance); - float batt_current_ratio = _batt_current/batt_current_max; + float batt_current_ratio = _batt_current / batt_current_max; - float loop_interval = 1.0f/_loop_rate; - _throttle_limit += (loop_interval/(loop_interval+_batt_current_time_constant))*(1.0f - batt_current_ratio); + float loop_interval = 1.0f / _loop_rate; + _throttle_limit += (loop_interval / (loop_interval + _batt_current_time_constant)) * (1.0f - batt_current_ratio); // throttle limit drops to 20% between hover and full throttle _throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f); // limit max throttle - return get_throttle_hover() + ((1.0-get_throttle_hover())*_throttle_limit); + return get_throttle_hover() + ((1.0 - get_throttle_hover()) * _throttle_limit); } // apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1 @@ -329,10 +328,10 @@ float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) co // zero expo means linear, avoid floating point exception for small values return thrust; } - if(!is_zero(_batt_voltage_filt.get())) { - throttle_ratio = ((thrust_curve_expo-1.0f) + safe_sqrt((1.0f-thrust_curve_expo)*(1.0f-thrust_curve_expo) + 4.0f*thrust_curve_expo*_lift_max*thrust))/(2.0f*thrust_curve_expo*_batt_voltage_filt.get()); + if (!is_zero(_batt_voltage_filt.get())) { + throttle_ratio = ((thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * _lift_max * thrust)) / (2.0f * thrust_curve_expo * _batt_voltage_filt.get()); } else { - throttle_ratio = ((thrust_curve_expo-1.0f) + safe_sqrt((1.0f-thrust_curve_expo)*(1.0f-thrust_curve_expo) + 4.0f*thrust_curve_expo*_lift_max*thrust))/(2.0f*thrust_curve_expo); + throttle_ratio = ((thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * _lift_max * thrust)) / (2.0f * thrust_curve_expo); } return constrain_float(throttle_ratio, 0.0f, 1.0f); @@ -344,7 +343,7 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage() // sanity check battery_voltage_min is not too small // if disabled or misconfigured exit immediately float _batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(_batt_idx); - if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f*_batt_voltage_min)) { + if ((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f * _batt_voltage_min)) { _batt_voltage_filt.reset(1.0f); _lift_max = 1.0f; return; @@ -356,11 +355,11 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage() _batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max); // filter at 0.5 Hz - float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate/_batt_voltage_max, 1.0f/_loop_rate); + float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate / _batt_voltage_max, 1.0f / _loop_rate); // calculate lift max float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f); - _lift_max = batt_voltage_filt*(1-thrust_curve_expo) + thrust_curve_expo*batt_voltage_filt*batt_voltage_filt; + _lift_max = batt_voltage_filt * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt * batt_voltage_filt; } float AP_MotorsMulticopter::get_compensation_gain() const @@ -375,7 +374,7 @@ float AP_MotorsMulticopter::get_compensation_gain() const #if AP_MOTORS_DENSITY_COMP == 1 // air density ratio is increasing in density / decreasing in altitude if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) { - ret *= 1.0f / constrain_float(_air_density_ratio,0.5f,1.25f); + ret *= 1.0f / constrain_float(_air_density_ratio, 0.5f, 1.25f); } #endif return ret; @@ -394,7 +393,7 @@ int16_t AP_MotorsMulticopter::output_to_pwm(float actuator) } } else { // in all other spool modes, covert to desired PWM - pwm_output = get_pwm_output_min() + (get_pwm_output_max()-get_pwm_output_min()) * actuator; + pwm_output = get_pwm_output_min() + (get_pwm_output_max() - get_pwm_output_min()) * actuator; } return pwm_output; @@ -404,7 +403,7 @@ int16_t AP_MotorsMulticopter::output_to_pwm(float actuator) float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in) { thrust_in = constrain_float(thrust_in, 0.0f, 1.0f); - return _spin_min + (_spin_max-_spin_min)*apply_thrust_curve_and_volt_scaling(thrust_in); + return _spin_min + (_spin_max - _spin_min) * apply_thrust_curve_and_volt_scaling(thrust_in); } // adds slew rate limiting to actuator output @@ -423,13 +422,13 @@ void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float // If MOT_SLEW_UP_TIME is set, calculate the highest allowed new output value, constrained 0.0~1.0 if (is_positive(_slew_up_time)) { - float output_delta_up_max = 1.0f/(constrain_float(_slew_up_time,0.0f,0.5f) * _loop_rate); + float output_delta_up_max = 1.0f / (constrain_float(_slew_up_time, 0.0f, 0.5f) * _loop_rate); output_slew_limit_up = constrain_float(actuator_output + output_delta_up_max, 0.0f, 1.0f); } // If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0 if (is_positive(_slew_dn_time)) { - float output_delta_dn_max = 1.0f/(constrain_float(_slew_dn_time,0.0f,0.5f) * _loop_rate); + float output_delta_dn_max = 1.0f / (constrain_float(_slew_dn_time, 0.0f, 0.5f) * _loop_rate); output_slew_limit_dn = constrain_float(actuator_output - output_delta_dn_max, 0.0f, 1.0f); } @@ -489,7 +488,7 @@ void AP_MotorsMulticopter::update_throttle_hover(float dt) { if (_throttle_hover_learn != HOVER_LEARN_DISABLED) { // we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial. - _throttle_hover = constrain_float(_throttle_hover + (dt/(dt+AP_MOTORS_THST_HOVER_TC))*(get_throttle()-_throttle_hover), AP_MOTORS_THST_HOVER_MIN, AP_MOTORS_THST_HOVER_MAX); + _throttle_hover = constrain_float(_throttle_hover + (dt / (dt + AP_MOTORS_THST_HOVER_TC)) * (get_throttle() - _throttle_hover), AP_MOTORS_THST_HOVER_MIN, AP_MOTORS_THST_HOVER_MAX); } } @@ -514,167 +513,167 @@ void AP_MotorsMulticopter::output_logic() } switch (_spool_state) { - case SpoolState::SHUT_DOWN: - // Motors should be stationary. - // Servos set to their trim values or in a test condition. + case SpoolState::SHUT_DOWN: + // Motors should be stationary. + // Servos set to their trim values or in a test condition. - // set limits flags - limit.roll_pitch = true; - limit.yaw = true; - limit.throttle_lower = true; - limit.throttle_upper = true; + // set limits flags + limit.roll_pitch = true; + limit.yaw = true; + limit.throttle_lower = true; + limit.throttle_upper = true; - // make sure the motors are spooling in the correct direction - if (_spool_desired != DesiredSpoolState::SHUT_DOWN) { - _spool_state = SpoolState::GROUND_IDLE; - break; - } - - // set and increment ramp variables - _spin_up_ratio = 0.0f; - _throttle_thrust_max = 0.0f; - - // initialise motor failure variables - _thrust_boost = false; - _thrust_boost_ratio = 0.0f; - break; - - case SpoolState::GROUND_IDLE: { - // Motors should be stationary or at ground idle. - // Servos should be moving to correct the current attitude. - - // set limits flags - limit.roll_pitch = true; - limit.yaw = true; - limit.throttle_lower = true; - limit.throttle_upper = true; - - // set and increment ramp variables - float spool_step = 1.0f/(_spool_up_time*_loop_rate); - switch (_spool_desired) { - case DesiredSpoolState::SHUT_DOWN: - _spin_up_ratio -= spool_step; - // constrain ramp value and update mode - if (_spin_up_ratio <= 0.0f) { - _spin_up_ratio = 0.0f; - _spool_state = SpoolState::SHUT_DOWN; - } - break; - case DesiredSpoolState::THROTTLE_UNLIMITED: - _spin_up_ratio += spool_step; - // constrain ramp value and update mode - if (_spin_up_ratio >= 1.0f) { - _spin_up_ratio = 1.0f; - _spool_state = SpoolState::SPOOLING_UP; - } - break; - case DesiredSpoolState::GROUND_IDLE: - float spin_up_armed_ratio = 0.0f; - if (_spin_min > 0.0f) { - spin_up_armed_ratio = _spin_arm / _spin_min; - } - _spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step); - break; - } - _throttle_thrust_max = 0.0f; - - // initialise motor failure variables - _thrust_boost = false; - _thrust_boost_ratio = 0.0f; + // make sure the motors are spooling in the correct direction + if (_spool_desired != DesiredSpoolState::SHUT_DOWN) { + _spool_state = SpoolState::GROUND_IDLE; break; } - case SpoolState::SPOOLING_UP: - // Maximum throttle should move from minimum to maximum. - // Servos should exhibit normal flight behavior. - // initialize limits flags - limit.roll_pitch = false; - limit.yaw = false; - limit.throttle_lower = false; - limit.throttle_upper = false; + // set and increment ramp variables + _spin_up_ratio = 0.0f; + _throttle_thrust_max = 0.0f; - // make sure the motors are spooling in the correct direction - if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED ){ - _spool_state = SpoolState::SPOOLING_DOWN; - break; - } + // initialise motor failure variables + _thrust_boost = false; + _thrust_boost_ratio = 0.0f; + break; - // set and increment ramp variables - _spin_up_ratio = 1.0f; - _throttle_thrust_max += 1.0f/(_spool_up_time*_loop_rate); + case SpoolState::GROUND_IDLE: { + // Motors should be stationary or at ground idle. + // Servos should be moving to correct the current attitude. + // set limits flags + limit.roll_pitch = true; + limit.yaw = true; + limit.throttle_lower = true; + limit.throttle_upper = true; + + // set and increment ramp variables + float spool_step = 1.0f / (_spool_up_time * _loop_rate); + switch (_spool_desired) { + case DesiredSpoolState::SHUT_DOWN: + _spin_up_ratio -= spool_step; // constrain ramp value and update mode - if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) { - _throttle_thrust_max = get_current_limit_max_throttle(); - _spool_state = SpoolState::THROTTLE_UNLIMITED; - } else if (_throttle_thrust_max < 0.0f) { - _throttle_thrust_max = 0.0f; - } - - // initialise motor failure variables - _thrust_boost = false; - _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0 / (_spool_up_time * _loop_rate)); - break; - - case SpoolState::THROTTLE_UNLIMITED: - // Throttle should exhibit normal flight behavior. - // Servos should exhibit normal flight behavior. - - // initialize limits flags - limit.roll_pitch = false; - limit.yaw = false; - limit.throttle_lower = false; - limit.throttle_upper = false; - - // make sure the motors are spooling in the correct direction - if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) { - _spool_state = SpoolState::SPOOLING_DOWN; - break; - } - - // set and increment ramp variables - _spin_up_ratio = 1.0f; - _throttle_thrust_max = get_current_limit_max_throttle(); - - if (_thrust_boost && !_thrust_balanced) { - _thrust_boost_ratio = MIN(1.0, _thrust_boost_ratio+1.0f/(_spool_up_time*_loop_rate)); - } else { - _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio-1.0f/(_spool_up_time*_loop_rate)); + if (_spin_up_ratio <= 0.0f) { + _spin_up_ratio = 0.0f; + _spool_state = SpoolState::SHUT_DOWN; } break; - - case SpoolState::SPOOLING_DOWN: - // Maximum throttle should move from maximum to minimum. - // Servos should exhibit normal flight behavior. - - // initialize limits flags - limit.roll_pitch = false; - limit.yaw = false; - limit.throttle_lower = false; - limit.throttle_upper = false; - - // make sure the motors are spooling in the correct direction - if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) { + case DesiredSpoolState::THROTTLE_UNLIMITED: + _spin_up_ratio += spool_step; + // constrain ramp value and update mode + if (_spin_up_ratio >= 1.0f) { + _spin_up_ratio = 1.0f; _spool_state = SpoolState::SPOOLING_UP; - break; } - - // set and increment ramp variables - _spin_up_ratio = 1.0f; - _throttle_thrust_max -= 1.0f/(_spool_up_time*_loop_rate); - - // constrain ramp value and update mode - if (_throttle_thrust_max <= 0.0f){ - _throttle_thrust_max = 0.0f; - } - if (_throttle_thrust_max >= get_current_limit_max_throttle()) { - _throttle_thrust_max = get_current_limit_max_throttle(); - } else if (is_zero(_throttle_thrust_max)) { - _spool_state = SpoolState::GROUND_IDLE; - } - - _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio-1.0f/(_spool_up_time*_loop_rate)); break; + case DesiredSpoolState::GROUND_IDLE: + float spin_up_armed_ratio = 0.0f; + if (_spin_min > 0.0f) { + spin_up_armed_ratio = _spin_arm / _spin_min; + } + _spin_up_ratio += constrain_float(spin_up_armed_ratio - _spin_up_ratio, -spool_step, spool_step); + break; + } + _throttle_thrust_max = 0.0f; + + // initialise motor failure variables + _thrust_boost = false; + _thrust_boost_ratio = 0.0f; + break; + } + case SpoolState::SPOOLING_UP: + // Maximum throttle should move from minimum to maximum. + // Servos should exhibit normal flight behavior. + + // initialize limits flags + limit.roll_pitch = false; + limit.yaw = false; + limit.throttle_lower = false; + limit.throttle_upper = false; + + // make sure the motors are spooling in the correct direction + if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) { + _spool_state = SpoolState::SPOOLING_DOWN; + break; + } + + // set and increment ramp variables + _spin_up_ratio = 1.0f; + _throttle_thrust_max += 1.0f / (_spool_up_time * _loop_rate); + + // constrain ramp value and update mode + if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) { + _throttle_thrust_max = get_current_limit_max_throttle(); + _spool_state = SpoolState::THROTTLE_UNLIMITED; + } else if (_throttle_thrust_max < 0.0f) { + _throttle_thrust_max = 0.0f; + } + + // initialise motor failure variables + _thrust_boost = false; + _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0 / (_spool_up_time * _loop_rate)); + break; + + case SpoolState::THROTTLE_UNLIMITED: + // Throttle should exhibit normal flight behavior. + // Servos should exhibit normal flight behavior. + + // initialize limits flags + limit.roll_pitch = false; + limit.yaw = false; + limit.throttle_lower = false; + limit.throttle_upper = false; + + // make sure the motors are spooling in the correct direction + if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) { + _spool_state = SpoolState::SPOOLING_DOWN; + break; + } + + // set and increment ramp variables + _spin_up_ratio = 1.0f; + _throttle_thrust_max = get_current_limit_max_throttle(); + + if (_thrust_boost && !_thrust_balanced) { + _thrust_boost_ratio = MIN(1.0, _thrust_boost_ratio + 1.0f / (_spool_up_time * _loop_rate)); + } else { + _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0f / (_spool_up_time * _loop_rate)); + } + break; + + case SpoolState::SPOOLING_DOWN: + // Maximum throttle should move from maximum to minimum. + // Servos should exhibit normal flight behavior. + + // initialize limits flags + limit.roll_pitch = false; + limit.yaw = false; + limit.throttle_lower = false; + limit.throttle_upper = false; + + // make sure the motors are spooling in the correct direction + if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) { + _spool_state = SpoolState::SPOOLING_UP; + break; + } + + // set and increment ramp variables + _spin_up_ratio = 1.0f; + _throttle_thrust_max -= 1.0f / (_spool_up_time * _loop_rate); + + // constrain ramp value and update mode + if (_throttle_thrust_max <= 0.0f) { + _throttle_thrust_max = 0.0f; + } + if (_throttle_thrust_max >= get_current_limit_max_throttle()) { + _throttle_thrust_max = get_current_limit_max_throttle(); + } else if (is_zero(_throttle_thrust_max)) { + _spool_state = SpoolState::GROUND_IDLE; + } + + _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0f / (_spool_up_time * _loop_rate)); + break; } } @@ -685,7 +684,7 @@ void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float th if (armed()) { uint16_t pwm_out = get_pwm_output_min() + constrain_float(throttle_input, 0.0f, 1.0f) * (get_pwm_output_max() - get_pwm_output_min()); // send the pilot's input directly to each enabled motor - for (uint16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + for (uint16_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { rc_write(i, pwm_out); } @@ -701,18 +700,17 @@ void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float th // the range 0 to 1 void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float rudder_dt) { - for (uint8_t i=0; i fabsf(actuator[i])) { actuator_max = fabsf(actuator[i]); } @@ -222,7 +221,7 @@ void AP_MotorsSingle::output_armed_stabilizing() // reduce roll, pitch and yaw to reduce the requested defection to maximum limit.roll_pitch = true; limit.yaw = true; - rp_scale = thrust_out_actuator/actuator_max; + rp_scale = thrust_out_actuator / actuator_max; } else { rp_scale = 1.0f; } @@ -231,8 +230,8 @@ void AP_MotorsSingle::output_armed_stabilizing() // static thrust is proportional to the airflow velocity squared // therefore the torque of the roll and pitch actuators should be approximately proportional to // the angle of attack multiplied by the static thrust. - for (uint8_t i=0; i _thrust_rear){ + rpy_low = MIN(_thrust_right, _thrust_left); + rpy_high = MAX(_thrust_right, _thrust_left); + if (rpy_low > _thrust_rear) { rpy_low = _thrust_rear; } // check to see if the rear motor will reach maximum thrust before the front two motors - if ((1.0f - rpy_high) > (pivot_thrust_max - _thrust_rear)){ + if ((1.0f - rpy_high) > (pivot_thrust_max - _thrust_rear)) { thrust_max = pivot_thrust_max; rpy_high = _thrust_rear; } @@ -210,41 +210,41 @@ void AP_MotorsTri::output_armed_stabilizing() // We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it // check everything fits - throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, throttle_avg_max); - if(is_zero(rpy_low)){ + throttle_thrust_best_rpy = MIN(0.5f * thrust_max - (rpy_low + rpy_high) / 2.0, throttle_avg_max); + if (is_zero(rpy_low)) { rpy_scale = 1.0f; } else { - rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f); + rpy_scale = constrain_float(-throttle_thrust_best_rpy / rpy_low, 0.0f, 1.0f); } // calculate how close the motors can come to the desired throttle thr_adj = throttle_thrust - throttle_thrust_best_rpy; - if(rpy_scale < 1.0f){ + if (rpy_scale < 1.0f) { // Full range is being used by roll, pitch, and yaw. limit.roll_pitch = true; - if (thr_adj > 0.0f){ + if (thr_adj > 0.0f) { limit.throttle_upper = true; } thr_adj = 0.0f; - }else{ - if(thr_adj < -(throttle_thrust_best_rpy+rpy_low)){ + } else { + if (thr_adj < -(throttle_thrust_best_rpy + rpy_low)) { // Throttle can't be reduced to desired value - thr_adj = -(throttle_thrust_best_rpy+rpy_low); - }else if(thr_adj > thrust_max - (throttle_thrust_best_rpy+rpy_high)){ + thr_adj = -(throttle_thrust_best_rpy + rpy_low); + } else if (thr_adj > thrust_max - (throttle_thrust_best_rpy + rpy_high)) { // Throttle can't be increased to desired value - thr_adj = thrust_max - (throttle_thrust_best_rpy+rpy_high); + thr_adj = thrust_max - (throttle_thrust_best_rpy + rpy_high); limit.throttle_upper = true; } } // add scaled roll, pitch, constrained yaw and throttle for each motor - _thrust_right = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_right; - _thrust_left = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_left; - _thrust_rear = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_rear; + _thrust_right = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_right; + _thrust_left = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_left; + _thrust_rear = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_rear; // scale pivot thrust to account for pivot angle // we should not need to check for divide by zero as _pivot_angle is constrained to the 5deg ~ 80 deg range - _thrust_rear = _thrust_rear/cosf(_pivot_angle); + _thrust_rear = _thrust_rear / cosf(_pivot_angle); // constrain all outputs to 0.0f to 1.0f // test code should be run with these lines commented out as they should not do anything @@ -296,10 +296,10 @@ int16_t AP_MotorsTri::calc_yaw_radio_output(float yaw_input, float yaw_input_max yaw_input = -yaw_input; } - if (yaw_input >= 0){ - ret = (_yaw_servo->get_trim() + (yaw_input/yaw_input_max * (_yaw_servo->get_output_max() - _yaw_servo->get_trim()))); + if (yaw_input >= 0) { + ret = (_yaw_servo->get_trim() + (yaw_input / yaw_input_max * (_yaw_servo->get_output_max() - _yaw_servo->get_trim()))); } else { - ret = (_yaw_servo->get_trim() + (yaw_input/yaw_input_max * (_yaw_servo->get_trim() - _yaw_servo->get_output_min()))); + ret = (_yaw_servo->get_trim() + (yaw_input / yaw_input_max * (_yaw_servo->get_trim() - _yaw_servo->get_output_min()))); } return ret; @@ -321,8 +321,8 @@ void AP_MotorsTri::thrust_compensation(void) // extract compensated thrust values _thrust_right = thrust[0]; - _thrust_left = thrust[1]; - _thrust_rear = thrust[3]; + _thrust_left = thrust[1]; + _thrust_rear = thrust[3]; } } @@ -347,7 +347,7 @@ float AP_MotorsTri::get_roll_factor(uint8_t i) case AP_MOTORS_MOT_1: ret = -1.0f; break; - // left motor + // left motor case AP_MOTORS_MOT_2: ret = 1.0f; break; diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 8e14dadd03..f6e5db8832 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -150,8 +150,8 @@ void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz) uint32_t AP_Motors::rc_map_mask(uint32_t mask) const { uint32_t mask2 = 0; - for (uint8_t i=0; i<32; i++) { - uint32_t bit = 1UL<= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { + if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) { uint8_t chan; SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num); SRV_Channels::set_aux_channel_default(function, motor_num); diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 9ae489b7be..42fe3f5942 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -65,7 +65,7 @@ public: AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); // singleton support - static AP_Motors *get_singleton(void) { return _singleton; } + static AP_Motors *get_singleton(void) { return _singleton; } // check initialisation succeeded bool initialised_ok() const { return _flags.initialised_ok; } @@ -85,7 +85,7 @@ public: void set_pitch(float pitch_in) { _pitch_in = pitch_in; }; // range -1 ~ +1 void set_yaw(float yaw_in) { _yaw_in = yaw_in; }; // range -1 ~ +1 void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1 - void set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max,0.0f,1.0f); }; // range 0 ~ 1 + void set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max, 0.0f, 1.0f); }; // range 0 ~ 1 void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); } void set_forward(float forward_in) { _forward_in = forward_in; }; // range -1 ~ +1 void set_lateral(float lateral_in) { _lateral_in = lateral_in; }; // range -1 ~ +1 @@ -94,8 +94,8 @@ public: float get_roll() const { return _roll_in; } float get_pitch() const { return _pitch_in; } float get_yaw() const { return _yaw_in; } - float get_throttle() const { return constrain_float(_throttle_filter.get(),0.0f,1.0f); } - float get_throttle_bidirectional() const { return constrain_float(2*(_throttle_filter.get()-0.5f),-1.0f,1.0f); } + float get_throttle() const { return constrain_float(_throttle_filter.get(), 0.0f, 1.0f); } + float get_throttle_bidirectional() const { return constrain_float(2 * (_throttle_filter.get() - 0.5f), -1.0f, 1.0f); } float get_forward() const { return _forward_in; } float get_lateral() const { return _lateral_in; } virtual float get_throttle_hover() const = 0; @@ -189,7 +189,7 @@ public: protected: // output functions that should be overloaded by child classes - virtual void output_armed_stabilizing()=0; + virtual void output_armed_stabilizing() = 0; virtual void rc_write(uint8_t chan, uint16_t pwm); virtual void rc_write_angle(uint8_t chan, int16_t angle_cd); virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz);