Diff: Periphery/SupportSystem.cpp

Revision:

0:c871d5355b99

Child:

1:719ac5a42c19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Periphery/SupportSystem.cpp	Sun Feb 07 06:14:32 2016 +0000
@@ -0,0 +1,274 @@
+#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*)&registerAddress,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;
+
+}
\ No newline at end of file