Torsten Wylegala
/
parallel_parking
foo
Fork of parallel_parking by
Periphery/SupportSystem.cpp
- Committer:
- becheo
- Date:
- 2016-02-07
- Revision:
- 0:c871d5355b99
- Child:
- 1:719ac5a42c19
File content as of revision 0:c871d5355b99:
#include <Periphery/SupportSystem.h> SupportSystem::SupportSystem(){ init(0,0); transmissionErrorCount = 0; lightManagerCommand = 0; } SupportSystem::SupportSystem(uint8_t i2cAddress, I2C *i2c){ init(i2cAddress, i2c); transmissionErrorCount = 0; lightManagerCommand = 0; } void SupportSystem::init(uint8_t i2cAddress, I2C *i2c){ this->i2cAddress = i2cAddress; this->i2c = i2c; } void SupportSystem::readData(uint8_t registerAddress, void *buffer, size_t length){ i2c->write(i2cAddress,(const char*)®isterAddress,sizeof(uint8_t),false); i2c->read(i2cAddress,(char*)buffer,length,false); } void SupportSystem::writeData(uint8_t registerAddress, void *buffer, size_t length){ i2c->start(); if(!i2c->write(i2cAddress & 0xfe)){ i2c->stop(); return; } if(!i2c->write(registerAddress)){ i2c->stop(); return; } for(uint32_t i=0; i<length; i++){ if(!i2c->write(*((uint8_t*)buffer))){ *(uint8_t*)buffer += 1; i2c->stop(); return; } } i2c->stop(); } uint16_t SupportSystem::crc16_update(uint16_t crc, uint8_t a) { int i; crc ^= a; for (i = 0; i < 8; ++i) { if (crc & 1) crc = (crc >> 1) ^ 0xA001; else crc = (crc >> 1); } return crc; } uint16_t SupportSystem::generateChecksum(void *data, size_t len){ uint16_t i, crc = 0; char *baseAddress = (char*)(data); for(i=0;i<len;i++){ char c = baseAddress[i]; crc = crc16_update(crc, c); } return crc; } uint32_t SupportSystem::getTransmissionErrorCount(){ return transmissionErrorCount; } /************************************************************ * * Maintenance * ***********************************************************/ uint32_t SupportSystem::readMaintenanceUptimeMillis(){ uint32_t uptimeMillis; readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_MAINTENANCE_UPTIME,(void*)&uptimeMillis,sizeof(uint32_t)); return uptimeMillis; } /************************************************************ * * IMU * ***********************************************************/ //float SupportSystem::readImuDistanceX(){ // // float traveledDistance; // // readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_DISTANCE_X,(void*)&traveledDistance,sizeof(float)); // // return traveledDistance; // //} // // //void SupportSystem::writeImuCommandResetDistanceX(){ // // uint8_t command = (1<<1); // // writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND,(void*)&command,sizeof(uint8_t)); // //} void SupportSystem::writeImuConfig(float *config_register, size_t length) { writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CONVERSION_FACTOR, config_register, sizeof(float)*length); uint8_t command = 1<<2; writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND, &command, sizeof(uint8_t)); wait(0.1); } IMU_RegisterDataBuffer_t *SupportSystem::getImuRegisterDataBuffer(){ uint8_t tries = 0; uint16_t checksum; do{ readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET,(void*)&IMU_registerDataBuffer,sizeof(IMU_RegisterDataBuffer_t)); checksum = generateChecksum(&IMU_registerDataBuffer,(sizeof(IMU_RegisterDataBuffer_t) - sizeof(uint16_t))); }while(tries++ < 5 && checksum != IMU_registerDataBuffer.completeChecksum); transmissionErrorCount += tries-1; return &IMU_registerDataBuffer; } /************************************************************ * * LightManager * ***********************************************************/ void SupportSystem::writeLightManagerCommand(){ uint8_t tries = 0; uint8_t control; do{ writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND,(void*)&lightManagerCommand,sizeof(uint8_t)); readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND,(void*)&control,sizeof(uint8_t)); }while(tries++ < 5 && control != lightManagerCommand); transmissionErrorCount += tries-1; } void SupportSystem::setLightManagerCommandBit(bool value, uint8_t bit){ if(value){ lightManagerCommand |= (1<<bit); }else{ lightManagerCommand &= ~(1<<bit); } } void SupportSystem::setLightManagerBrakeLight(bool active, bool writeOut){ setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_BRAKE_LIGHT); if(writeOut){ writeLightManagerCommand(); } } void SupportSystem::setLightManagerRemoteLight(bool active, bool writeOut){ setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_REMOTE_LIGHT); if(writeOut){ writeLightManagerCommand(); } } void SupportSystem::setLightManagerSignalLeft(bool active, bool writeOut){ setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_LEFT); if(writeOut){ writeLightManagerCommand(); } } void SupportSystem::setLightManagerSignalRight(bool active, bool writeOut){ setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_RIGHT); if(writeOut){ writeLightManagerCommand(); } } void SupportSystem::setLightManagerSignalBoth(bool active, bool writeOut){ setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_BOTH); if(writeOut){ writeLightManagerCommand(); } } /************************************************************ * * RadioDecoder * ***********************************************************/ RadioDecoder_RegisterDataBuffer_t *SupportSystem::getRadioDecoderRegisterDataBuffer(){ uint8_t tries = 0; uint16_t checksum; do{ readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET,(void*)&RadioDecoder_registerDataBuffer,sizeof(RadioDecoder_RegisterDataBuffer_t)); checksum = generateChecksum(&RadioDecoder_registerDataBuffer,sizeof(uint8_t)*12); }while(tries++ < 5 && checksum != RadioDecoder_registerDataBuffer.checksum); transmissionErrorCount += tries-1; return &RadioDecoder_registerDataBuffer; }