77 lines
1.5 KiB
C++
77 lines
1.5 KiB
C++
/*
|
||
* TorqueEstimation.cpp
|
||
*
|
||
* Created on: 29 дек. 2020 г.
|
||
* Author: titov
|
||
*/
|
||
|
||
#include "LoadEstimation.hh"
|
||
|
||
bool processing::LoadEstimation::Setting::isValid() {
|
||
|
||
const Setting non_valid;
|
||
|
||
if( Jp <= 0.0f )
|
||
Jp = non_valid.Jp;
|
||
|
||
if( L1 <= 0.0f )
|
||
L1 = non_valid.L1;
|
||
|
||
if( L2 <= 0.0f )
|
||
L2 = non_valid.L2;
|
||
|
||
return Jp != non_valid.Jp
|
||
and L1 != non_valid.L1
|
||
and L2 != non_valid.L2;
|
||
|
||
}
|
||
|
||
processing::LoadEstimation::Setting::Setting() : Jp(0.0f), L1(0.0f), L2(0.0f) {}
|
||
|
||
void processing::LoadEstimation::configure( const Setting & setting ) {
|
||
|
||
_L1Base = setting.L1;
|
||
_L2Base = setting.L2;
|
||
_Base_J = 1.0f / setting.Jp;
|
||
|
||
_L1Ts = _L1Base * _Ts;
|
||
_L2Ts = _L2Base * _Ts;
|
||
_Ts_J = _Base_J * _Ts;
|
||
|
||
}
|
||
|
||
void processing::LoadEstimation::process() {
|
||
|
||
const float error = ( speed - est_speed );
|
||
|
||
est_speed += error * _L1Ts + ( torque - est_load ) * _Ts_J;
|
||
est_load -= error * _L2Ts;
|
||
|
||
}
|
||
|
||
void processing::LoadEstimation::setSampleTime( float ts_in_second ) {
|
||
|
||
_Ts = ts_in_second;
|
||
|
||
_L1Ts = _L1Base * _Ts;
|
||
_L2Ts = _L2Base * _Ts;
|
||
_Ts_J = _Base_J * _Ts;
|
||
|
||
}
|
||
|
||
const float & processing::LoadEstimation::getEstimatedLoad() const {
|
||
|
||
return est_load;
|
||
|
||
}
|
||
|
||
const float & processing::LoadEstimation::getEstimatedSpeed() const {
|
||
|
||
return est_speed;
|
||
|
||
}
|
||
|
||
processing::LoadEstimation::LoadEstimation( systemic::ISignal & speed,
|
||
systemic::ISignal & torque ) : speed(speed), torque(torque) {}
|
||
|