135 lines
5.1 KiB
C++
135 lines
5.1 KiB
C++
|
|
|
|
/*
|
|
* SecondaryEncoderSetup.cpp
|
|
*
|
|
* Created on: 20 ìàð. 2019 ã.
|
|
* Author: krugliy
|
|
*/
|
|
|
|
#include "SecondaryEncoderSetup.hh"
|
|
|
|
#include "../../common/Codec.hpp"
|
|
|
|
#include "../../systemic/Scheduler.hpp"
|
|
#include "../../systemic/Process.hpp"
|
|
|
|
#include "../../driver/SerialPortAdapter.hpp"
|
|
#include "../../driver/EncoderEncoderDataParser.hpp"
|
|
#include "../../driver/EncoderSsi.hpp"
|
|
|
|
typedef uint32_t SSI32_Frame;
|
|
const short ssi_frame_size = 32;
|
|
|
|
typedef driver::SerialPortAdapter<SSI32_Frame, uint16_t, true> SerialPortRegister;
|
|
|
|
template<>
|
|
uint16_t SerialPortRegister::temp = 0;
|
|
|
|
bool application::board::SecondaryEncoderSetup::input(Environment& env) {
|
|
|
|
return grab( serial_port, env.rholder.getShared< peripheral::ISerialPort >( links.serial_port_id ) )
|
|
&& grab( de_pin, env.rholder.getShared< peripheral::IGpio >( links.de_pin_id ) )
|
|
&& grab( power_pin, env.rholder.getShared< peripheral::IGpio >( links.power_pin_id ) )
|
|
&& grab( cs21_pin, env.rholder.getShared< peripheral::IGpio >( links.cs21_id ) )
|
|
&& grab( cs22_pin, env.rholder.getShared< peripheral::IGpio >( links.cs22_id ) )
|
|
;
|
|
|
|
}
|
|
|
|
bool application::board::SecondaryEncoderSetup::prepare() {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
#include "../../systemic/ValueLazyFast.hpp"
|
|
|
|
void application::board::SecondaryEncoderSetup::build(Environment& env) {
|
|
|
|
de_pin->write( config.de_active_level );
|
|
power_pin->write( config.power_pin_active_level );
|
|
|
|
cs21_pin->write( config.cs21_active_level );
|
|
cs22_pin->write( config.cs22_active_level );
|
|
|
|
const systemic::time_t update_period = config.update_period * 1000.0f;
|
|
|
|
SSI32_Frame default_frame = 0;
|
|
|
|
SerialPortRegister * obj = memories::instance_object<SerialPortRegister>( env.static_object_ma, *serial_port, ssi_frame_size, update_period,
|
|
default_frame, default_frame, default_frame );
|
|
|
|
systemic::ProcScheduler::ProcessParameter proc = {
|
|
.priority = params.priority,
|
|
.period = params.period, .phase = params.phase,
|
|
.controlled = false
|
|
};
|
|
|
|
env.scheduler.registerProcess( params.processor_id, *memories::instance_object< systemic::Process<SerialPortRegister> >( env.static_object_ma, *obj ),
|
|
proc );
|
|
|
|
typedef codec::GrayCodeDecoder DecoderType;
|
|
typedef driver::EncoderDataParser<DecoderType>::ParserType EncoderDataParserType;
|
|
|
|
DecoderType decoder;
|
|
|
|
EncoderDataParserType * encoder_data_parser = memories::instance_object<EncoderDataParserType>(
|
|
env.static_object_ma, decoder //codec::GrayCodeDecoder()
|
|
);
|
|
|
|
EncoderDataParserType::Setting parser_setting; {
|
|
parser_setting.turn_bit_len = config.turn_bit_len,
|
|
parser_setting.angle_bit_len = config.angle_bit_len,
|
|
parser_setting.error_bit_len = config.error_bit_len;
|
|
};
|
|
|
|
encoder_data_parser->cofigure( parser_setting );
|
|
|
|
typedef driver::detail::EncoderSsi<systemic::IRegister<SSI32_Frame>, EncoderDataParserType> Encoder;
|
|
|
|
driver::detail::EncoderSsiConfig::Config encoder_config;
|
|
|
|
const short ssi_bit_len = config.turn_bit_len + config.angle_bit_len + config.error_bit_len;
|
|
|
|
{
|
|
encoder_config.num_error = 2;
|
|
encoder_config.max_angle_delta = 6.28f;
|
|
encoder_config.ssi_data_bit_len = ssi_bit_len;
|
|
encoder_config.max_turn_bit = config.turn_bit_len; //todo: WTF?
|
|
encoder_config.frame_size = ssi_frame_size;
|
|
encoder_config.ssi_extra_bit = 0;
|
|
encoder_config.shift_count = config.shift_bit_count;
|
|
encoder_config.inverse = false;
|
|
encoder_config.swap_bytes = true;
|
|
}
|
|
|
|
Encoder * encoder_driver = memories::instance_object<Encoder>(
|
|
env.static_object_ma,
|
|
*encoder_data_parser,
|
|
*obj,
|
|
encoder_config
|
|
);
|
|
|
|
env.scheduler.registerProcess( params.processor_id, *memories::instance_object< systemic::Process<Encoder> >( env.static_object_ma, *encoder_driver ),
|
|
proc );
|
|
|
|
env.rholder.share<driver::IEncoder>( *encoder_driver, links.secondary_encoder_id );
|
|
|
|
//Ðåãèñòðàöèÿ ñèãíàëîâ äëÿ äåìî âåðñèè:
|
|
typedef systemic::detail::ValueLazyFast< float, Encoder, &Encoder::getAngle> EncoderAngle;
|
|
env.signals.add( links.encoder_angle_sig_id, memories::instance_object<EncoderAngle>( env.static_setup_ma, *encoder_driver ) );
|
|
|
|
typedef systemic::detail::ValueLazyFast< float, Encoder, &Encoder::getTurn> EncoderTurn;
|
|
env.signals.add( links.encoder_turn_sig_id, memories::instance_object<EncoderTurn>( env.static_setup_ma, *encoder_driver ) );
|
|
|
|
typedef systemic::detail::ValueLazyFast< float, Encoder, &Encoder::getErrorCode> EncoderErr;
|
|
env.signals.add( links.encoder_err_sig_id, memories::instance_object<EncoderErr>( env.static_setup_ma, *encoder_driver ) );
|
|
|
|
}
|
|
|
|
application::board::SecondaryEncoderSetup::SecondaryEncoderSetup(
|
|
const Links& links, const Setting& config, const configuration::ProcessConfig& proc)
|
|
: links(links), config(config), params(proc), serial_port(nullptr), de_pin(nullptr), power_pin(nullptr),
|
|
cs21_pin(nullptr), cs22_pin(nullptr) {}
|