Torsten Wylegala
/
parallel_parking
foo
Fork of parallel_parking by
Periphery/SupportSystem.cpp@0:c871d5355b99, 2016-02-07 (annotated)
- Committer:
- becheo
- Date:
- Sun Feb 07 06:14:32 2016 +0000
- Revision:
- 0:c871d5355b99
- Child:
- 1:719ac5a42c19
Einparken mit verschiedenen Geschwindigkeiten
Who changed what in which revision?
User | Revision | Line number | New 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*)®isterAddress,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 | |
becheo | 0:c871d5355b99 | 155 | do{ |
becheo | 0:c871d5355b99 | 156 | |
becheo | 0:c871d5355b99 | 157 | readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET,(void*)&IMU_registerDataBuffer,sizeof(IMU_RegisterDataBuffer_t)); |
becheo | 0:c871d5355b99 | 158 | checksum = generateChecksum(&IMU_registerDataBuffer,(sizeof(IMU_RegisterDataBuffer_t) - sizeof(uint16_t))); |
becheo | 0:c871d5355b99 | 159 | |
becheo | 0:c871d5355b99 | 160 | }while(tries++ < 5 && checksum != IMU_registerDataBuffer.completeChecksum); |
becheo | 0:c871d5355b99 | 161 | |
becheo | 0:c871d5355b99 | 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 | } |