// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max float AP_MotorsMulticopter::get_current_limit_max_throttle() { // return maximum if current limiting is disabled if (_batt_current_max <= 0) { _throttle_limit = 1.0f; return 1.0f; } // remove throttle limit if disarmed if (!_flags.armed) { _throttle_limit = 1.0f; return 1.0f; } // calculate the maximum current to prevent voltage sag below _batt_voltage_min float batt_current_max = MIN(_batt_current_max, _batt_current + (_batt_voltage-_batt_voltage_min)/_batt_resistance); 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); // 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); }
// update_battery_resistance - calculate battery resistance when throttle is above hover_out void AP_MotorsMulticopter::update_battery_resistance() { // if disarmed reset resting voltage and current if (!_flags.armed) { _batt_voltage_resting = _batt_voltage; _batt_current_resting = _batt_current; _batt_timer = 0; } else if(_batt_voltage_resting < _batt_voltage && _batt_current_resting > _batt_current) { // update battery resistance when throttle is over hover throttle float batt_resistance = (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting); if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) { if (get_throttle() >= get_throttle_hover()) { // filter reaches 90% in 1/4 the test time _batt_resistance += 0.05f*(batt_resistance - _batt_resistance); _batt_timer += 1; } else { // initialize battery resistance to prevent change in resting voltage estimate _batt_resistance = batt_resistance; } } // make sure battery resistance value doesn't result in the predicted battery voltage exceeding the resting voltage if(batt_resistance < _batt_resistance){ _batt_resistance = batt_resistance; } } }