SupportSystem.cpp

00001 #include <Periphery/SupportSystem.h>
00002 
00003 
00004 SupportSystem::SupportSystem(){
00005 
00006     init(0,0);
00007 
00008     transmissionErrorCount = 0;
00009     lightManagerCommand = 0;
00010 }
00011 
00012 SupportSystem::SupportSystem(uint8_t i2cAddress, I2C *i2c){
00013 
00014     init(i2cAddress, i2c);
00015 
00016     transmissionErrorCount = 0;
00017     lightManagerCommand = 0;
00018 }
00019 
00020 void SupportSystem::init(uint8_t i2cAddress, I2C *i2c){
00021 
00022     this->i2cAddress = i2cAddress;
00023     this->i2c = i2c;
00024 
00025 }
00026 
00027 
00028 
00029 void SupportSystem::readData(uint8_t registerAddress, void *buffer, size_t length){
00030 
00031 
00032     i2c->write(i2cAddress,(const char*)&registerAddress,sizeof(uint8_t),false);
00033     i2c->read(i2cAddress,(char*)buffer,length,false);
00034 
00035 }
00036 
00037 void SupportSystem::writeData(uint8_t registerAddress, void *buffer, size_t length){
00038 
00039 
00040     i2c->start();
00041 
00042 
00043     if(!i2c->write(i2cAddress & 0xfe)){
00044         i2c->stop();
00045         return;
00046     }
00047 
00048     if(!i2c->write(registerAddress)){
00049         i2c->stop();
00050         return;
00051     }
00052 
00053     for(uint32_t i=0; i<length; i++){
00054         if(!i2c->write(*((uint8_t*)buffer))){
00055             *(uint8_t*)buffer += 1;
00056             i2c->stop();
00057             return;
00058         }
00059     }
00060 
00061     i2c->stop();
00062 
00063 
00064 }
00065 
00066 
00067 uint16_t SupportSystem::crc16_update(uint16_t crc, uint8_t a) {
00068     int i;
00069 
00070     crc ^= a;
00071     for (i = 0; i < 8; ++i) {
00072         if (crc & 1)
00073             crc = (crc >> 1) ^ 0xA001;
00074         else
00075             crc = (crc >> 1);
00076     }
00077 
00078     return crc;
00079 }
00080 
00081 uint16_t SupportSystem::generateChecksum(void *data, size_t len){
00082 
00083     uint16_t i, crc = 0;
00084 
00085     char *baseAddress = (char*)(data);
00086 
00087     for(i=0;i<len;i++){
00088         char c = baseAddress[i];
00089         crc = crc16_update(crc, c);
00090     }
00091 
00092     return crc;
00093 
00094 }
00095 
00096 uint32_t SupportSystem::getTransmissionErrorCount(){
00097     return transmissionErrorCount;
00098 }
00099 
00100 /************************************************************
00101  *
00102  * Maintenance
00103  *
00104  ***********************************************************/
00105 
00106 uint32_t SupportSystem::readMaintenanceUptimeMillis(){
00107 
00108     uint32_t uptimeMillis;
00109 
00110     readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_MAINTENANCE_UPTIME,(void*)&uptimeMillis,sizeof(uint32_t));
00111 
00112     return uptimeMillis;
00113 }
00114 
00115 /************************************************************
00116  *
00117  * IMU
00118  *
00119  ***********************************************************/
00120 
00121 //float SupportSystem::readImuDistanceX(){
00122 //
00123 //  float traveledDistance;
00124 //
00125 //  readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_DISTANCE_X,(void*)&traveledDistance,sizeof(float));
00126 //
00127 //  return traveledDistance;
00128 //
00129 //}
00130 //
00131 //
00132 //void SupportSystem::writeImuCommandResetDistanceX(){
00133 //
00134 //  uint8_t command = (1<<1);
00135 //
00136 //  writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND,(void*)&command,sizeof(uint8_t));
00137 //
00138 //}
00139 
00140 void SupportSystem::writeImuConfig(float *config_register, size_t length) {
00141     writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CONVERSION_FACTOR, config_register, sizeof(float)*length);
00142     
00143     uint8_t command = 1<<2;
00144     writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND, &command, sizeof(uint8_t));
00145     
00146     wait(0.1);
00147 }
00148 
00149 
00150 IMU_RegisterDataBuffer_t *SupportSystem::getImuRegisterDataBuffer(){
00151 
00152     uint8_t tries = 0;
00153     uint16_t checksum;
00154 
00155     do{
00156 
00157         readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET,(void*)&IMU_registerDataBuffer,sizeof(IMU_RegisterDataBuffer_t));
00158         checksum = generateChecksum(&IMU_registerDataBuffer,(sizeof(IMU_RegisterDataBuffer_t) - sizeof(uint16_t)));
00159 
00160     }while(tries++ < 5 && checksum != IMU_registerDataBuffer.completeChecksum);
00161 
00162     transmissionErrorCount += tries-1;
00163 
00164     return &IMU_registerDataBuffer;
00165 
00166 }
00167 
00168 
00169 /************************************************************
00170  *
00171  * LightManager
00172  *
00173  ***********************************************************/
00174 
00175 void SupportSystem::writeLightManagerCommand(){
00176 
00177     uint8_t tries = 0;
00178     uint8_t control;
00179 
00180     do{
00181 
00182         writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND,(void*)&lightManagerCommand,sizeof(uint8_t));
00183         readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND,(void*)&control,sizeof(uint8_t));
00184 
00185     }while(tries++ < 5 && control != lightManagerCommand);
00186 
00187     transmissionErrorCount += tries-1;
00188 
00189 }
00190 
00191 void SupportSystem::setLightManagerCommandBit(bool value, uint8_t bit){
00192 
00193     if(value){
00194         lightManagerCommand |= (1<<bit);
00195     }else{
00196         lightManagerCommand &= ~(1<<bit);
00197     }
00198 }
00199 
00200 void SupportSystem::setLightManagerBrakeLight(bool active, bool writeOut){
00201 
00202     setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_BRAKE_LIGHT);
00203 
00204     if(writeOut){
00205         writeLightManagerCommand();
00206     }
00207 
00208 }
00209 
00210 void SupportSystem::setLightManagerRemoteLight(bool active, bool writeOut){
00211 
00212     setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_REMOTE_LIGHT);
00213 
00214     if(writeOut){
00215         writeLightManagerCommand();
00216     }
00217 
00218 }
00219 
00220 void SupportSystem::setLightManagerSignalLeft(bool active, bool writeOut){
00221 
00222     setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_LEFT);
00223 
00224     if(writeOut){
00225         writeLightManagerCommand();
00226     }
00227 
00228 }
00229 
00230 void SupportSystem::setLightManagerSignalRight(bool active, bool writeOut){
00231 
00232     setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_RIGHT);
00233 
00234     if(writeOut){
00235         writeLightManagerCommand();
00236     }
00237 
00238 }
00239 
00240 void SupportSystem::setLightManagerSignalBoth(bool active, bool writeOut){
00241 
00242 
00243     setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_BOTH);
00244 
00245     if(writeOut){
00246         writeLightManagerCommand();
00247     }
00248 
00249 }
00250 
00251 
00252 /************************************************************
00253  *
00254  * RadioDecoder
00255  *
00256  ***********************************************************/
00257 
00258 RadioDecoder_RegisterDataBuffer_t *SupportSystem::getRadioDecoderRegisterDataBuffer(){
00259 
00260     uint8_t tries = 0;
00261     uint16_t checksum;
00262 
00263     do{
00264 
00265         readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET,(void*)&RadioDecoder_registerDataBuffer,sizeof(RadioDecoder_RegisterDataBuffer_t));
00266         checksum = generateChecksum(&RadioDecoder_registerDataBuffer,sizeof(uint8_t)*12);
00267 
00268     }while(tries++ < 5 && checksum != RadioDecoder_registerDataBuffer.checksum);
00269 
00270     transmissionErrorCount += tries-1;
00271 
00272     return &RadioDecoder_registerDataBuffer;
00273 
00274 }