101 lines
4.0 KiB
C++
101 lines
4.0 KiB
C++
/*
|
||
* PositionMixerv2.h
|
||
*
|
||
* Created on: 20 июл. 2020 г.
|
||
* Author: user
|
||
*/
|
||
|
||
#ifndef UMLIBRARY_DRIVER_POSITIONMIXERV2_H_
|
||
#define UMLIBRARY_DRIVER_POSITIONMIXERV2_H_
|
||
|
||
#include "IncrementCounter.hh"
|
||
#include "IResolver.hh"
|
||
#include "HiperfacePositionData.hh"
|
||
|
||
#include <cmath>
|
||
|
||
namespace driver {
|
||
|
||
class PositionMixer_v2 {
|
||
private:
|
||
//Источники данных о положении:
|
||
IncrementCounter increment_counter;
|
||
driver::IResolver & resolver;
|
||
|
||
const unsigned short step_per_turn; //!< Битов абсолютной (цифровой) позиции в механическом обороте.
|
||
|
||
const float resolver_to_rad; //!< Коэффициент преобразования координаты по резольверу в абсолютную.
|
||
|
||
const unsigned long line_in_turn;
|
||
const unsigned long max_turn_value;
|
||
|
||
const unsigned long long max_angle_code;
|
||
|
||
mutable bool phase_error_flag = false; //!< Несоответствие угла по резольверу и по счетчику.
|
||
bool scrt_valid = false;
|
||
|
||
PositionComponents last_position;
|
||
bool new_code_after_resolver_fault = false;
|
||
|
||
bool is_sector_synchronized = false;
|
||
unsigned long counter_before_synchronized = 0;
|
||
|
||
short sector_offset:2;
|
||
|
||
const float sector_deviation;
|
||
|
||
const unsigned short resolver_hyst; //! Количество подряд валидных сигналов синуса и косинуса.
|
||
const unsigned short counter_hyst; //! Количество подряд невалидных сигналов синуса и косинуса.
|
||
unsigned short resolver_valid_value_count = 0;
|
||
unsigned short resolver_invalid_value_count = 0;
|
||
|
||
const float half_increment; //!<Половина дистанции между отсчетами, рад.
|
||
|
||
PositionCode incSinCosPeriod(PositionCode code);
|
||
PositionCode decSinCosPeriod(PositionCode code);
|
||
|
||
public:
|
||
/// Битов абсолютной (цифровой) позиции в периоде синуса/косинуса. Всегда.
|
||
static const unsigned short periods_in_turn_power = 5;
|
||
|
||
/// Количество отсчетов инкрементального энкодера в одном периоде синуса/косинуса, степень двойки. Всегда.
|
||
static const unsigned short line_in_period_power = 2;
|
||
|
||
/// Проверка источников позиции - потеря квадратурного сигнала - отказ.
|
||
bool isValid() const { return increment_counter.isValid(); }
|
||
|
||
bool isPhaseError() const {
|
||
bool error = phase_error_flag;
|
||
if(phase_error_flag) phase_error_flag = false;
|
||
return error;
|
||
}
|
||
|
||
/// Признак использования данных от СКВТ.
|
||
bool isScrtValid() const { return scrt_valid and is_sector_synchronized; }
|
||
|
||
/// Сброс счетчиков.
|
||
void reset();
|
||
|
||
//!Предоставляет смикшированные данные в конечном виде.
|
||
PositionData calculatePosition();
|
||
|
||
/// Предоставляет отдельные данные о положении со всех источников.
|
||
PositionComponents getPositionComponents() const;
|
||
|
||
/// Установка абсолютного положения в отсчетах абсолютного энкодера.
|
||
void setAbsolutePosition(unsigned long absolute_position);
|
||
|
||
PositionMixer_v2( peripheral::ICounter & counter, unsigned long sin_period_resolution, unsigned long max_turn_value,
|
||
driver::IResolver & resolver, unsigned short resolver_hyst, unsigned short counter_hyst, float sector_deviation );
|
||
|
||
private:
|
||
PositionCode cross_correction(const PositionCode & position, const float period_angle);
|
||
void sector_synchronization(const PositionCode & position, const float period_angle);
|
||
|
||
void sector_resynchronizarion();
|
||
};
|
||
|
||
} /* namespace driver */
|
||
|
||
#endif /* UMLIBRARY_DRIVER_POSITIONMIXERV2_H_ */
|