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@0:8a6003b8bb5b, 2016-02-03 (annotated)

Committer:
mborchers
Date:
Wed Feb 03 17:19:21 2016 +0000
Revision:
0:8a6003b8bb5b
Child:
13:34f7f783ad24
Erster Commit

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 0:8a6003b8bb5b 140
mborchers 0:8a6003b8bb5b 141 IMU_RegisterDataBuffer_t *SupportSystem::getImuRegisterDataBuffer(){
mborchers 0:8a6003b8bb5b 142
mborchers 0:8a6003b8bb5b 143 uint8_t tries = 0;
mborchers 0:8a6003b8bb5b 144 uint16_t checksum;
mborchers 0:8a6003b8bb5b 145
mborchers 0:8a6003b8bb5b 146 do{
mborchers 0:8a6003b8bb5b 147
mborchers 0:8a6003b8bb5b 148 readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET,(void*)&IMU_registerDataBuffer,sizeof(IMU_RegisterDataBuffer_t));
mborchers 0:8a6003b8bb5b 149 checksum = generateChecksum(&IMU_registerDataBuffer,(sizeof(IMU_RegisterDataBuffer_t) - sizeof(uint16_t)));
mborchers 0:8a6003b8bb5b 150
mborchers 0:8a6003b8bb5b 151 }while(tries++ < 5 && checksum != IMU_registerDataBuffer.completeChecksum);
mborchers 0:8a6003b8bb5b 152
mborchers 0:8a6003b8bb5b 153 transmissionErrorCount += tries-1;
mborchers 0:8a6003b8bb5b 154
mborchers 0:8a6003b8bb5b 155 return &IMU_registerDataBuffer;
mborchers 0:8a6003b8bb5b 156
mborchers 0:8a6003b8bb5b 157 }
mborchers 0:8a6003b8bb5b 158
mborchers 0:8a6003b8bb5b 159
mborchers 0:8a6003b8bb5b 160 /************************************************************
mborchers 0:8a6003b8bb5b 161 *
mborchers 0:8a6003b8bb5b 162 * LightManager
mborchers 0:8a6003b8bb5b 163 *
mborchers 0:8a6003b8bb5b 164 ***********************************************************/
mborchers 0:8a6003b8bb5b 165
mborchers 0:8a6003b8bb5b 166 void SupportSystem::writeLightManagerCommand(){
mborchers 0:8a6003b8bb5b 167
mborchers 0:8a6003b8bb5b 168 uint8_t tries = 0;
mborchers 0:8a6003b8bb5b 169 uint8_t control;
mborchers 0:8a6003b8bb5b 170
mborchers 0:8a6003b8bb5b 171 do{
mborchers 0:8a6003b8bb5b 172
mborchers 0:8a6003b8bb5b 173 writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND,(void*)&lightManagerCommand,sizeof(uint8_t));
mborchers 0:8a6003b8bb5b 174 readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND,(void*)&control,sizeof(uint8_t));
mborchers 0:8a6003b8bb5b 175
mborchers 0:8a6003b8bb5b 176 }while(tries++ < 5 && control != lightManagerCommand);
mborchers 0:8a6003b8bb5b 177
mborchers 0:8a6003b8bb5b 178 transmissionErrorCount += tries-1;
mborchers 0:8a6003b8bb5b 179
mborchers 0:8a6003b8bb5b 180 }
mborchers 0:8a6003b8bb5b 181
mborchers 0:8a6003b8bb5b 182 void SupportSystem::setLightManagerCommandBit(bool value, uint8_t bit){
mborchers 0:8a6003b8bb5b 183
mborchers 0:8a6003b8bb5b 184 if(value){
mborchers 0:8a6003b8bb5b 185 lightManagerCommand |= (1<<bit);
mborchers 0:8a6003b8bb5b 186 }else{
mborchers 0:8a6003b8bb5b 187 lightManagerCommand &= ~(1<<bit);
mborchers 0:8a6003b8bb5b 188 }
mborchers 0:8a6003b8bb5b 189 }
mborchers 0:8a6003b8bb5b 190
mborchers 0:8a6003b8bb5b 191 void SupportSystem::setLightManagerBrakeLight(bool active, bool writeOut){
mborchers 0:8a6003b8bb5b 192
mborchers 0:8a6003b8bb5b 193 setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_BRAKE_LIGHT);
mborchers 0:8a6003b8bb5b 194
mborchers 0:8a6003b8bb5b 195 if(writeOut){
mborchers 0:8a6003b8bb5b 196 writeLightManagerCommand();
mborchers 0:8a6003b8bb5b 197 }
mborchers 0:8a6003b8bb5b 198
mborchers 0:8a6003b8bb5b 199 }
mborchers 0:8a6003b8bb5b 200
mborchers 0:8a6003b8bb5b 201 void SupportSystem::setLightManagerRemoteLight(bool active, bool writeOut){
mborchers 0:8a6003b8bb5b 202
mborchers 0:8a6003b8bb5b 203 setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_REMOTE_LIGHT);
mborchers 0:8a6003b8bb5b 204
mborchers 0:8a6003b8bb5b 205 if(writeOut){
mborchers 0:8a6003b8bb5b 206 writeLightManagerCommand();
mborchers 0:8a6003b8bb5b 207 }
mborchers 0:8a6003b8bb5b 208
mborchers 0:8a6003b8bb5b 209 }
mborchers 0:8a6003b8bb5b 210
mborchers 0:8a6003b8bb5b 211 void SupportSystem::setLightManagerSignalLeft(bool active, bool writeOut){
mborchers 0:8a6003b8bb5b 212
mborchers 0:8a6003b8bb5b 213 setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_LEFT);
mborchers 0:8a6003b8bb5b 214
mborchers 0:8a6003b8bb5b 215 if(writeOut){
mborchers 0:8a6003b8bb5b 216 writeLightManagerCommand();
mborchers 0:8a6003b8bb5b 217 }
mborchers 0:8a6003b8bb5b 218
mborchers 0:8a6003b8bb5b 219 }
mborchers 0:8a6003b8bb5b 220
mborchers 0:8a6003b8bb5b 221 void SupportSystem::setLightManagerSignalRight(bool active, bool writeOut){
mborchers 0:8a6003b8bb5b 222
mborchers 0:8a6003b8bb5b 223 setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_RIGHT);
mborchers 0:8a6003b8bb5b 224
mborchers 0:8a6003b8bb5b 225 if(writeOut){
mborchers 0:8a6003b8bb5b 226 writeLightManagerCommand();
mborchers 0:8a6003b8bb5b 227 }
mborchers 0:8a6003b8bb5b 228
mborchers 0:8a6003b8bb5b 229 }
mborchers 0:8a6003b8bb5b 230
mborchers 0:8a6003b8bb5b 231 void SupportSystem::setLightManagerSignalBoth(bool active, bool writeOut){
mborchers 0:8a6003b8bb5b 232
mborchers 0:8a6003b8bb5b 233
mborchers 0:8a6003b8bb5b 234 setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_BOTH);
mborchers 0:8a6003b8bb5b 235
mborchers 0:8a6003b8bb5b 236 if(writeOut){
mborchers 0:8a6003b8bb5b 237 writeLightManagerCommand();
mborchers 0:8a6003b8bb5b 238 }
mborchers 0:8a6003b8bb5b 239
mborchers 0:8a6003b8bb5b 240 }
mborchers 0:8a6003b8bb5b 241
mborchers 0:8a6003b8bb5b 242
mborchers 0:8a6003b8bb5b 243 /************************************************************
mborchers 0:8a6003b8bb5b 244 *
mborchers 0:8a6003b8bb5b 245 * RadioDecoder
mborchers 0:8a6003b8bb5b 246 *
mborchers 0:8a6003b8bb5b 247 ***********************************************************/
mborchers 0:8a6003b8bb5b 248
mborchers 0:8a6003b8bb5b 249 RadioDecoder_RegisterDataBuffer_t *SupportSystem::getRadioDecoderRegisterDataBuffer(){
mborchers 0:8a6003b8bb5b 250
mborchers 0:8a6003b8bb5b 251 uint8_t tries = 0;
mborchers 0:8a6003b8bb5b 252 uint16_t checksum;
mborchers 0:8a6003b8bb5b 253
mborchers 0:8a6003b8bb5b 254 do{
mborchers 0:8a6003b8bb5b 255
mborchers 0:8a6003b8bb5b 256 readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET,(void*)&RadioDecoder_registerDataBuffer,sizeof(RadioDecoder_RegisterDataBuffer_t));
mborchers 0:8a6003b8bb5b 257 checksum = generateChecksum(&RadioDecoder_registerDataBuffer,sizeof(uint8_t)*12);
mborchers 0:8a6003b8bb5b 258
mborchers 0:8a6003b8bb5b 259 }while(tries++ < 5 && checksum != RadioDecoder_registerDataBuffer.checksum);
mborchers 0:8a6003b8bb5b 260
mborchers 0:8a6003b8bb5b 261 transmissionErrorCount += tries-1;
mborchers 0:8a6003b8bb5b 262
mborchers 0:8a6003b8bb5b 263 return &RadioDecoder_registerDataBuffer;
mborchers 0:8a6003b8bb5b 264
mborchers 0:8a6003b8bb5b 265 }