Torsten Wylegala / Mbed 2 deprecated parallel_parking

foo

Dependencies:   mbed-rtos mbed

Fork of parallel_parking by Oliver Becher

Periphery/SupportSystem.cpp@2:fae63e87900e, 2016-02-08 (annotated)

Committer:
torstenwylegala
Date:
Mon Feb 08 11:40:03 2016 +0000
Revision:
2:fae63e87900e
Parent:
1:719ac5a42c19 
foo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
becheo 0:c871d5355b99 1 #include <Periphery/SupportSystem.h>
becheo 0:c871d5355b99 2
becheo 0:c871d5355b99 3
becheo 0:c871d5355b99 4 SupportSystem::SupportSystem(){
becheo 0:c871d5355b99 5
becheo 0:c871d5355b99 6 init(0,0);
becheo 0:c871d5355b99 7
becheo 0:c871d5355b99 8 transmissionErrorCount = 0;
becheo 0:c871d5355b99 9 lightManagerCommand = 0;
becheo 0:c871d5355b99 10 }
becheo 0:c871d5355b99 11
becheo 0:c871d5355b99 12 SupportSystem::SupportSystem(uint8_t i2cAddress, I2C *i2c){
becheo 0:c871d5355b99 13
becheo 0:c871d5355b99 14 init(i2cAddress, i2c);
becheo 0:c871d5355b99 15
becheo 0:c871d5355b99 16 transmissionErrorCount = 0;
becheo 0:c871d5355b99 17 lightManagerCommand = 0;
becheo 0:c871d5355b99 18 }
becheo 0:c871d5355b99 19
becheo 0:c871d5355b99 20 void SupportSystem::init(uint8_t i2cAddress, I2C *i2c){
becheo 0:c871d5355b99 21
becheo 0:c871d5355b99 22 this->i2cAddress = i2cAddress;
becheo 0:c871d5355b99 23 this->i2c = i2c;
becheo 0:c871d5355b99 24
becheo 0:c871d5355b99 25 }
becheo 0:c871d5355b99 26
becheo 0:c871d5355b99 27
becheo 0:c871d5355b99 28
becheo 0:c871d5355b99 29 void SupportSystem::readData(uint8_t registerAddress, void *buffer, size_t length){
becheo 0:c871d5355b99 30
becheo 0:c871d5355b99 31
becheo 0:c871d5355b99 32 i2c->write(i2cAddress,(const char*)&registerAddress,sizeof(uint8_t),false);
becheo 0:c871d5355b99 33 i2c->read(i2cAddress,(char*)buffer,length,false);
becheo 0:c871d5355b99 34
becheo 0:c871d5355b99 35 }
becheo 0:c871d5355b99 36
becheo 0:c871d5355b99 37 void SupportSystem::writeData(uint8_t registerAddress, void *buffer, size_t length){
becheo 0:c871d5355b99 38
becheo 0:c871d5355b99 39
becheo 0:c871d5355b99 40 i2c->start();
becheo 0:c871d5355b99 41
becheo 0:c871d5355b99 42
becheo 0:c871d5355b99 43 if(!i2c->write(i2cAddress & 0xfe)){
becheo 0:c871d5355b99 44 i2c->stop();
becheo 0:c871d5355b99 45 return;
becheo 0:c871d5355b99 46 }
becheo 0:c871d5355b99 47
becheo 0:c871d5355b99 48 if(!i2c->write(registerAddress)){
becheo 0:c871d5355b99 49 i2c->stop();
becheo 0:c871d5355b99 50 return;
becheo 0:c871d5355b99 51 }
becheo 0:c871d5355b99 52
becheo 0:c871d5355b99 53 for(uint32_t i=0; i<length; i++){
becheo 0:c871d5355b99 54 if(!i2c->write(*((uint8_t*)buffer))){
becheo 0:c871d5355b99 55 *(uint8_t*)buffer += 1;
becheo 0:c871d5355b99 56 i2c->stop();
becheo 0:c871d5355b99 57 return;
becheo 0:c871d5355b99 58 }
becheo 0:c871d5355b99 59 }
becheo 0:c871d5355b99 60
becheo 0:c871d5355b99 61 i2c->stop();
becheo 0:c871d5355b99 62
becheo 0:c871d5355b99 63
becheo 0:c871d5355b99 64 }
becheo 0:c871d5355b99 65
becheo 0:c871d5355b99 66
becheo 0:c871d5355b99 67 uint16_t SupportSystem::crc16_update(uint16_t crc, uint8_t a) {
becheo 0:c871d5355b99 68 int i;
becheo 0:c871d5355b99 69
becheo 0:c871d5355b99 70 crc ^= a;
becheo 0:c871d5355b99 71 for (i = 0; i < 8; ++i) {
becheo 0:c871d5355b99 72 if (crc & 1)
becheo 0:c871d5355b99 73 crc = (crc >> 1) ^ 0xA001;
becheo 0:c871d5355b99 74 else
becheo 0:c871d5355b99 75 crc = (crc >> 1);
becheo 0:c871d5355b99 76 }
becheo 0:c871d5355b99 77
becheo 0:c871d5355b99 78 return crc;
becheo 0:c871d5355b99 79 }
becheo 0:c871d5355b99 80
becheo 0:c871d5355b99 81 uint16_t SupportSystem::generateChecksum(void *data, size_t len){
becheo 0:c871d5355b99 82
becheo 0:c871d5355b99 83 uint16_t i, crc = 0;
becheo 0:c871d5355b99 84
becheo 0:c871d5355b99 85 char *baseAddress = (char*)(data);
becheo 0:c871d5355b99 86
becheo 0:c871d5355b99 87 for(i=0;i<len;i++){
becheo 0:c871d5355b99 88 char c = baseAddress[i];
becheo 0:c871d5355b99 89 crc = crc16_update(crc, c);
becheo 0:c871d5355b99 90 }
becheo 0:c871d5355b99 91
becheo 0:c871d5355b99 92 return crc;
becheo 0:c871d5355b99 93
becheo 0:c871d5355b99 94 }
becheo 0:c871d5355b99 95
becheo 0:c871d5355b99 96 uint32_t SupportSystem::getTransmissionErrorCount(){
becheo 0:c871d5355b99 97 return transmissionErrorCount;
becheo 0:c871d5355b99 98 }
becheo 0:c871d5355b99 99
becheo 0:c871d5355b99 100 /************************************************************
becheo 0:c871d5355b99 101 *
becheo 0:c871d5355b99 102 * Maintenance
becheo 0:c871d5355b99 103 *
becheo 0:c871d5355b99 104 ***********************************************************/
becheo 0:c871d5355b99 105
becheo 0:c871d5355b99 106 uint32_t SupportSystem::readMaintenanceUptimeMillis(){
becheo 0:c871d5355b99 107
becheo 0:c871d5355b99 108 uint32_t uptimeMillis;
becheo 0:c871d5355b99 109
becheo 0:c871d5355b99 110 readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_MAINTENANCE_UPTIME,(void*)&uptimeMillis,sizeof(uint32_t));
becheo 0:c871d5355b99 111
becheo 0:c871d5355b99 112 return uptimeMillis;
becheo 0:c871d5355b99 113 }
becheo 0:c871d5355b99 114
becheo 0:c871d5355b99 115 /************************************************************
becheo 0:c871d5355b99 116 *
becheo 0:c871d5355b99 117 * IMU
becheo 0:c871d5355b99 118 *
becheo 0:c871d5355b99 119 ***********************************************************/
becheo 0:c871d5355b99 120
becheo 0:c871d5355b99 121 //float SupportSystem::readImuDistanceX(){
becheo 0:c871d5355b99 122 //
becheo 0:c871d5355b99 123 // float traveledDistance;
becheo 0:c871d5355b99 124 //
becheo 0:c871d5355b99 125 // readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_DISTANCE_X,(void*)&traveledDistance,sizeof(float));
becheo 0:c871d5355b99 126 //
becheo 0:c871d5355b99 127 // return traveledDistance;
becheo 0:c871d5355b99 128 //
becheo 0:c871d5355b99 129 //}
becheo 0:c871d5355b99 130 //
becheo 0:c871d5355b99 131 //
becheo 0:c871d5355b99 132 //void SupportSystem::writeImuCommandResetDistanceX(){
becheo 0:c871d5355b99 133 //
becheo 0:c871d5355b99 134 // uint8_t command = (1<<1);
becheo 0:c871d5355b99 135 //
becheo 0:c871d5355b99 136 // writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND,(void*)&command,sizeof(uint8_t));
becheo 0:c871d5355b99 137 //
becheo 0:c871d5355b99 138 //}
becheo 0:c871d5355b99 139
becheo 0:c871d5355b99 140 void SupportSystem::writeImuConfig(float *config_register, size_t length) {
becheo 0:c871d5355b99 141 writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CONVERSION_FACTOR, config_register, sizeof(float)*length);
becheo 0:c871d5355b99 142
becheo 0:c871d5355b99 143 uint8_t command = 1<<2;
becheo 0:c871d5355b99 144 writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND, &command, sizeof(uint8_t));
becheo 0:c871d5355b99 145
becheo 0:c871d5355b99 146 wait(0.1);
becheo 0:c871d5355b99 147 }
becheo 0:c871d5355b99 148
becheo 0:c871d5355b99 149
becheo 0:c871d5355b99 150 IMU_RegisterDataBuffer_t *SupportSystem::getImuRegisterDataBuffer(){
becheo 0:c871d5355b99 151
becheo 0:c871d5355b99 152 uint8_t tries = 0;
becheo 0:c871d5355b99 153 uint16_t checksum;
becheo 0:c871d5355b99 154
torstenwylegala 2:fae63e87900e 155 do{
becheo 0:c871d5355b99 156
becheo 0:c871d5355b99 157 readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET,(void*)&IMU_registerDataBuffer,sizeof(IMU_RegisterDataBuffer_t));
torstenwylegala 2:fae63e87900e 158 checksum = generateChecksum(&IMU_registerDataBuffer,(sizeof(IMU_RegisterDataBuffer_t) - sizeof(uint16_t)));
becheo 0:c871d5355b99 159
torstenwylegala 2:fae63e87900e 160 }while(tries++ < 5 && checksum != IMU_registerDataBuffer.completeChecksum);
becheo 0:c871d5355b99 161
torstenwylegala 2:fae63e87900e 162 transmissionErrorCount += tries-1;
becheo 0:c871d5355b99 163
becheo 0:c871d5355b99 164 return &IMU_registerDataBuffer;
becheo 0:c871d5355b99 165
becheo 0:c871d5355b99 166 }
becheo 0:c871d5355b99 167
becheo 0:c871d5355b99 168
becheo 0:c871d5355b99 169 /************************************************************
becheo 0:c871d5355b99 170 *
becheo 0:c871d5355b99 171 * LightManager
becheo 0:c871d5355b99 172 *
becheo 0:c871d5355b99 173 ***********************************************************/
becheo 0:c871d5355b99 174
becheo 0:c871d5355b99 175 void SupportSystem::writeLightManagerCommand(){
becheo 0:c871d5355b99 176
becheo 0:c871d5355b99 177 uint8_t tries = 0;
becheo 0:c871d5355b99 178 uint8_t control;
becheo 0:c871d5355b99 179
becheo 0:c871d5355b99 180 do{
becheo 0:c871d5355b99 181
becheo 0:c871d5355b99 182 writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND,(void*)&lightManagerCommand,sizeof(uint8_t));
becheo 0:c871d5355b99 183 readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND,(void*)&control,sizeof(uint8_t));
becheo 0:c871d5355b99 184
becheo 0:c871d5355b99 185 }while(tries++ < 5 && control != lightManagerCommand);
becheo 0:c871d5355b99 186
becheo 0:c871d5355b99 187 transmissionErrorCount += tries-1;
becheo 0:c871d5355b99 188
becheo 0:c871d5355b99 189 }
becheo 0:c871d5355b99 190
becheo 0:c871d5355b99 191 void SupportSystem::setLightManagerCommandBit(bool value, uint8_t bit){
becheo 0:c871d5355b99 192
becheo 0:c871d5355b99 193 if(value){
becheo 0:c871d5355b99 194 lightManagerCommand |= (1<<bit);
becheo 0:c871d5355b99 195 }else{
becheo 0:c871d5355b99 196 lightManagerCommand &= ~(1<<bit);
becheo 0:c871d5355b99 197 }
becheo 0:c871d5355b99 198 }
becheo 0:c871d5355b99 199
becheo 0:c871d5355b99 200 void SupportSystem::setLightManagerBrakeLight(bool active, bool writeOut){
becheo 0:c871d5355b99 201
becheo 0:c871d5355b99 202 setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_BRAKE_LIGHT);
becheo 0:c871d5355b99 203
becheo 0:c871d5355b99 204 if(writeOut){
becheo 0:c871d5355b99 205 writeLightManagerCommand();
becheo 0:c871d5355b99 206 }
becheo 0:c871d5355b99 207
becheo 0:c871d5355b99 208 }
becheo 0:c871d5355b99 209
becheo 0:c871d5355b99 210 void SupportSystem::setLightManagerRemoteLight(bool active, bool writeOut){
becheo 0:c871d5355b99 211
becheo 0:c871d5355b99 212 setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_REMOTE_LIGHT);
becheo 0:c871d5355b99 213
becheo 0:c871d5355b99 214 if(writeOut){
becheo 0:c871d5355b99 215 writeLightManagerCommand();
becheo 0:c871d5355b99 216 }
becheo 0:c871d5355b99 217
becheo 0:c871d5355b99 218 }
becheo 0:c871d5355b99 219
becheo 0:c871d5355b99 220 void SupportSystem::setLightManagerSignalLeft(bool active, bool writeOut){
becheo 0:c871d5355b99 221
becheo 0:c871d5355b99 222 setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_LEFT);
becheo 0:c871d5355b99 223
becheo 0:c871d5355b99 224 if(writeOut){
becheo 0:c871d5355b99 225 writeLightManagerCommand();
becheo 0:c871d5355b99 226 }
becheo 0:c871d5355b99 227
becheo 0:c871d5355b99 228 }
becheo 0:c871d5355b99 229
becheo 0:c871d5355b99 230 void SupportSystem::setLightManagerSignalRight(bool active, bool writeOut){
becheo 0:c871d5355b99 231
becheo 0:c871d5355b99 232 setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_RIGHT);
becheo 0:c871d5355b99 233
becheo 0:c871d5355b99 234 if(writeOut){
becheo 0:c871d5355b99 235 writeLightManagerCommand();
becheo 0:c871d5355b99 236 }
becheo 0:c871d5355b99 237
becheo 0:c871d5355b99 238 }
becheo 0:c871d5355b99 239
becheo 0:c871d5355b99 240 void SupportSystem::setLightManagerSignalBoth(bool active, bool writeOut){
becheo 0:c871d5355b99 241
becheo 0:c871d5355b99 242
becheo 0:c871d5355b99 243 setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_BOTH);
becheo 0:c871d5355b99 244
becheo 0:c871d5355b99 245 if(writeOut){
becheo 0:c871d5355b99 246 writeLightManagerCommand();
becheo 0:c871d5355b99 247 }
becheo 0:c871d5355b99 248
becheo 0:c871d5355b99 249 }
becheo 0:c871d5355b99 250
becheo 0:c871d5355b99 251
becheo 0:c871d5355b99 252 /************************************************************
becheo 0:c871d5355b99 253 *
becheo 0:c871d5355b99 254 * RadioDecoder
becheo 0:c871d5355b99 255 *
becheo 0:c871d5355b99 256 ***********************************************************/
becheo 0:c871d5355b99 257
becheo 0:c871d5355b99 258 RadioDecoder_RegisterDataBuffer_t *SupportSystem::getRadioDecoderRegisterDataBuffer(){
becheo 0:c871d5355b99 259
becheo 0:c871d5355b99 260 uint8_t tries = 0;
becheo 0:c871d5355b99 261 uint16_t checksum;
becheo 0:c871d5355b99 262
becheo 0:c871d5355b99 263 do{
becheo 0:c871d5355b99 264
becheo 0:c871d5355b99 265 readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET,(void*)&RadioDecoder_registerDataBuffer,sizeof(RadioDecoder_RegisterDataBuffer_t));
becheo 0:c871d5355b99 266 checksum = generateChecksum(&RadioDecoder_registerDataBuffer,sizeof(uint8_t)*12);
becheo 0:c871d5355b99 267
becheo 0:c871d5355b99 268 }while(tries++ < 5 && checksum != RadioDecoder_registerDataBuffer.checksum);
becheo 0:c871d5355b99 269
becheo 0:c871d5355b99 270 transmissionErrorCount += tries-1;
becheo 0:c871d5355b99 271
becheo 0:c871d5355b99 272 return &RadioDecoder_registerDataBuffer;
becheo 0:c871d5355b99 273
becheo 0:c871d5355b99 274 }