Matti Borchers / Mbed 2 deprecated mbed_amf_controlsystem

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed

Periphery/SupportSystem.cpp@13:34f7f783ad24, 2016-02-05 (annotated)

Committer:
mborchers
Date:
Fri Feb 05 16:06:44 2016 +0000
Revision:
13:34f7f783ad24
Parent:
0:8a6003b8bb5b 
L?ngsregelung ausgelagert

Who changed what in which revision?

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