MotorControlModuleSDFM_TMS320F28388D/Projects/EFC_Application/UMLibrary/driver/EncoderSsi.hpp

/*
 * EncoderSsi.h
 *
 *  Created on: 20 <EFBFBD><EFBFBD><EFBFBD>. 2019 <EFBFBD>.
 *      Author: krugliy
 */

#ifndef SOURCE_DRIVER_ENCODERSSI_H_
#define SOURCE_DRIVER_ENCODERSSI_H_

#include <cmath>
#include <stdint.h>

#include "IEncoder.hh"

#include "../common/DoubleBuffer.hpp"

namespace driver { namespace detail {

struct EncoderSsiConfig {
    struct Config {

        uint16_t num_error;                 //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
        float max_angle_delta;              //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.

        uint16_t ssi_data_bit_len;    //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
        uint16_t max_turn_bit;        //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.

        //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
        uint16_t frame_size;          //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> spi,
        //<2F><><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> ssi_data_bit_len

        uint16_t ssi_extra_bit;       //!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>-<2D><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> spi
        uint16_t shift_count;               //!<<3C><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> clk.
        bool inverse;                       //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.

        bool swap_bytes;                //!< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
    };

    EncoderSsiConfig( Config config ) : error_counter(config.num_error), max_angle_delta(config.max_angle_delta),
            turn_bit_mask( ( 1 << config.max_turn_bit ) - 1 ),
            shift_count(config.frame_size - config.ssi_data_bit_len - ( config.ssi_extra_bit + config.shift_count )),
            inverse(config.inverse), swap_bytes( config.swap_bytes )  {}

    struct ErrorCounter {

        typedef unsigned short ErrorCount;

        ErrorCount counter;
        ErrorCount max_error;

        ErrorCounter( ErrorCount num_error ) : counter(1), max_error(num_error) {}

    } error_counter;

    float max_angle_delta;
    uint32_t turn_bit_mask;

    uint16_t shift_count;
    bool inverse;

    bool swap_bytes;    //!< <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
};

template<class Interface, class DataParser>
class EncoderSsi : public IEncoder, public EncoderSsiConfig {
public:

    EncoderSsi( DataParser _parser, Interface & _interface, EncoderSsiConfig::Config config )
        : EncoderSsiConfig(config), interface(_interface), parser(_parser) {}

    virtual ~EncoderSsi() = default;

    float getAngle() const override { return error_counter.counter ? NAN : encoder_angle; }
    float getTurn() const override { return error_counter.counter ? NAN : encoder_turn & turn_bit_mask; }
    std::pair<float, float> getPosition() const override;

//    unsigned short getErrorCode() const { return encoder_error_code; }
    float getErrorCode() const { return encoder_error_code; }

    bool isValidData() const { return not error_counter.counter; }
    bool isFailure() const { return error_counter.counter >= error_counter.max_error; }
    bool isDirectionError() const { return direction_error; }

    void process();

    uint32_t getRawData();

private:
    DataParser parser;
    Interface & interface;

    unsigned short encoder_turn = 0;
    float encoder_angle = NAN;      //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>

    typename DataParser::EncoderData encoder_position;   //<2F><><EFBFBD><EFBFBD> + <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> + <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

    unsigned short encoder_error_code = 0;
    bool direction_error = false; //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.

    DoubleBuffer< std::pair<float, float> > data;


};


} /* namespace detail */
} /* namespace driver */

template<class Interface, class DataParser>
inline void driver::detail::EncoderSsi<Interface, DataParser>::process() {

    //TODO: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><> <20> connection <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>..
    uint32_t encoder_data = getRawData();

    typename DataParser::EncoderData next_position_data = parser.parseEncoderData( encoder_data );

    if( encoder_error_code = next_position_data.error ) {

        if( error_counter.counter < error_counter.max_error )
            error_counter.counter++;

    } else {

        encoder_angle = next_position_data.angle;
        encoder_turn = next_position_data.turn;

        //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
        direction_error = parser.getAngleDelta( next_position_data, encoder_position ) > max_angle_delta;

        encoder_position = next_position_data;

        if( error_counter.counter )
            --error_counter.counter;

        data.write( std::pair<float, float>( encoder_turn, encoder_angle ) );
    }

}

template<class Interface, class DataParser>
inline std::pair<float, float> driver::detail::EncoderSsi<Interface, DataParser>::getPosition() const {

    return error_counter.counter ? std::pair<float, float>( 0, NAN ) : data.read();

}

template<class Interface, class DataParser>
inline uint32_t driver::detail::EncoderSsi<Interface, DataParser>::getRawData() {

    if( swap_bytes ) {

        uint32_t raw_encoder_data = interface.get();

        uint32_t encoder_data = ( raw_encoder_data << 16 ) | ( (raw_encoder_data >> 16) & 0xFFFF );

        if( inverse ) encoder_data = ~encoder_data;

        encoder_data >>= shift_count;

        return encoder_data;

    } else {

        uint32_t encoder_data = interface.get();

        if( inverse ) encoder_data = ~encoder_data;

        return encoder_data;
    }

}

#endif /* SOURCE_DRIVER_ENCODERSSI_HPP_ */