File content as of revision 0:8a6003b8bb5b:

#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));
//
//}


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;

}