133 lines
3.6 KiB
C++
133 lines
3.6 KiB
C++
/*
|
||
* TrackingPositionScrt.h
|
||
*
|
||
* Created on: 26 янв. 2018 г.
|
||
* Author: titov
|
||
*/
|
||
|
||
#ifndef SOURCE_PROCESSING_TRACKINGPOSITIONSCRT_H_
|
||
#define SOURCE_PROCESSING_TRACKINGPOSITIONSCRT_H_
|
||
|
||
#include "../driver/IResolver.hh"
|
||
|
||
#include "../math/math_inc.hh"
|
||
|
||
namespace processing {
|
||
|
||
//class Observer_FirstOrder {
|
||
//private:
|
||
// float est_dx;
|
||
// float est_x;
|
||
//
|
||
// const float est_range;
|
||
//};
|
||
|
||
//!Класс обработки путевой информации из данных от резольвера.
|
||
/*!Модуль использует интерфейс резольвера для получения исходных данных.
|
||
* Исходными данными является квадратурный сигнал позиции.
|
||
* Восстанавливает скорость, позицию из квадратурного сигнала.
|
||
*/
|
||
class TrackingPositionScrt {
|
||
public:
|
||
typedef driver::IResolver TrackerInterface;
|
||
|
||
struct Setting {
|
||
float L1; //!<Коэффициент следящей системы, установить 1000.
|
||
float L2; //!<Коэффициент следящей системы, установить1000^2/2.
|
||
float ThetaNull; //!<Угол коммутации, рад.
|
||
unsigned long Np; //!<Количество пар полюсов двигателя.
|
||
|
||
float scrt_power_max; //!<Максимальное значение суммы квадратов для синус-косинусного вращающегося трансформатора.
|
||
float scrt_power_min; //!<Минимальное значение суммы квадратов для синус-косинусного вращающегося трансформатора.
|
||
float scrt_speed_max; //!<Максимальная скорость синус-косинусного вращающегося трансформатора, рад/с.
|
||
|
||
unsigned long Nr; //!<Количество пар полюсов резольвера.
|
||
|
||
bool isValid();
|
||
|
||
Setting() : L1(0.0f), L2(0.0f), ThetaNull(NAN), Np(0xFFFFu),
|
||
scrt_power_max(0.0f), scrt_power_min(0.0f), scrt_speed_max(0.0f), Nr(0xFFFFu) {}
|
||
|
||
};
|
||
|
||
|
||
private:
|
||
driver::IResolver & scrt;
|
||
|
||
//Модель датчика.
|
||
float est_sens_speed;
|
||
float est_sens_position_pi2;
|
||
float est_sens_turn;
|
||
float est_sens_error;
|
||
|
||
//todo: Вынести представления в отдельные модули.
|
||
|
||
//Представление в электрических координатах.
|
||
float est_theta;
|
||
float est_omega;
|
||
|
||
//Представление в физических координатах.
|
||
float est_position;
|
||
float est_speed;
|
||
|
||
unsigned int error_counter; //!<Счетчик ошибки.
|
||
|
||
private:
|
||
float _Ts;
|
||
|
||
//Данные расчета ошибки:
|
||
float _power_max; //!<Максимально-допустимая сумма квадратов сигналов СКВТ.
|
||
float _power_min; //todo Проверить почему здесь!
|
||
float _Tun; //!<Допустимое время без данных.
|
||
unsigned int _Nerr;
|
||
float prev_scrt_sin;
|
||
float prev_scrt_cos;
|
||
float prev_error;
|
||
|
||
//Настройки наблюдателя позиции.
|
||
float _L1Base;
|
||
float _L2Base;
|
||
float _L1Ts;
|
||
float _L2Ts;
|
||
|
||
//Настройки предстваления в электрических координатах.
|
||
float _ThetaZero;
|
||
float _Np_Nr;
|
||
|
||
//Настройки представления в физических координатах.
|
||
float _1_Nr;
|
||
float _pi2_Nr;
|
||
|
||
void bad_data_handler();
|
||
|
||
~TrackingPositionScrt() = default;
|
||
public:
|
||
TrackingPositionScrt( driver::IResolver & resolver );
|
||
|
||
void configure( const Setting & setting );
|
||
void setSampleTime( float Ts );
|
||
|
||
void process();
|
||
|
||
//!Функция позволяет получить текущую позицию.
|
||
const float & position() { return est_position; }
|
||
//!Функция позволяет получить текущую скорость.
|
||
const float & speed() { return est_speed; }
|
||
|
||
//!Функция позволяет получить текущий электрический угол.
|
||
const float & theta() { return est_theta; }
|
||
//!Функция позволяет получить текущий электрическую скорость.
|
||
const float & omega() { return est_omega; }
|
||
|
||
const float & pos_error() { return est_sens_error; }
|
||
|
||
bool isError() const { return error_counter == _Nerr; }
|
||
};
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
#endif /* SOURCE_PROCESSING_TRACKINGPOSITIONSCRT_H_ */
|