41 lines
908 B
C++
41 lines
908 B
C++
|
/*
|
|||
|
|
* FieldCalculation.cpp
|
|||
|
|
*
|
|||
|
|
* Created on: 16 <EFBFBD><EFBFBD><EFBFBD>. 2021 <EFBFBD>.
|
|||
|
|
* Author: titov
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#include "FieldCalculation.hh"
|
|||
|
|
|
|||
|
|
processing::FieldCalculation::FieldCalculation(systemic::ISignal & position,
|
|||
|
|
systemic::ISignal & speed ) : position(position), speed(speed),
|
|||
|
|
est_theta(0.f), est_omega(0.f), _Np(NAN), _ThetaZero(0.f) {}
|
|||
|
|
|
|||
|
|
void processing::FieldCalculation::configure( const Setting & config ) {
|
|||
|
|
|
|||
|
|
_ThetaZero = config.ThetaZero;
|
|||
|
|
_Np = config.Np;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const float & processing::FieldCalculation::theta() const {
|
|||
|
|
|
|||
|
|
return est_theta;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const float & processing::FieldCalculation::omega() const {
|
|||
|
|
|
|||
|
|
return est_omega;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void processing::FieldCalculation::process() {
|
|||
|
|
|
|||
|
|
using namespace std;
|
|||
|
|
|
|||
|
|
est_theta = fmodf( position * _Np + _ThetaZero, math::constants::pi2 );
|
|||
|
|
est_omega = speed * _Np;
|
|||
|
|
|
|||
|
|
}
|