1 // 2 // Copyright (c) 2010-2022 Antmicro 3 // Copyright (c) 2011-2015 Realtime Embedded 4 // 5 // This file is licensed under the MIT License. 6 // Full license text is available in 'licenses/MIT.txt'. 7 // 8 using System; 9 using System.Linq; 10 using System.Collections.Generic; 11 using Antmicro.Renode.Peripherals.Bus; 12 using Antmicro.Renode.Logging; 13 using Antmicro.Renode.Core; 14 15 namespace Antmicro.Renode.Peripherals.I2C 16 { 17 public class BMC050 : II2CPeripheral 18 { BMC050()19 public BMC050() 20 { 21 Reset(); 22 } 23 24 // TODO: how do call the generate sensor data functions? 25 // Now: After receiving register address to read. 26 // Step through after read and init? 27 // Thread always running which sleeps with given frequencies? 28 Reset()29 public void Reset() 30 { 31 this.Log(LogLevel.Noisy, "Reset registers"); 32 // TODO: Control, status and image registers are reset to values stored in the EEPROM. 33 acc_z_msb = 0; 34 acc_z_lsb = 0; 35 acc_y_msb = 0; 36 acc_y_lsb = 0; 37 acc_x_msb = 0; 38 acc_x_lsb = 0; 39 temperature = 0; 40 statusReg1 = 0; 41 statusReg2 = 0; 42 statusReg3 = 0; 43 statusReg4 = 0; 44 gRange = g_RangeModes.Range2g; 45 bandwidth = BandwidthModes.Bandwidth8 | BandwidthModes.Bandwidth9; 46 power = 0; 47 daqCtrl = 0; 48 irqCtrl1 = 0; 49 irqCtrl2 = 0; 50 irqMap1 = 0; 51 irqMap2 = 0; 52 irqMap3 = 0; 53 irqSource = 0; 54 irqBehaviour = 0; 55 irqMode = 0; 56 lowDur = 0x09; 57 lowTh = 0x30; 58 hy = 0x81; 59 highDur = 0x0F; 60 highTh = 0xC0; 61 slopeDur = 0; 62 slopeTh = 0x14; 63 tapConfig1 = 0x04; 64 tapConfig2 = 0x0A; 65 orientConfig1 = 0x18; 66 orientConfig2 = 0x08; 67 flatConfig1 = 0x08; 68 flatConfig2 = 0x10; 69 selfTest = SelfTestModes.Idle; 70 eepromCtrl = 0x04; 71 interfaceConfig = 0; 72 offsetCompensation = 0; 73 offsetTarget = 0; 74 offsetFilteredX = 0; 75 offsetFilteredY = 0; 76 offsetFilteredZ = 0; 77 offsetUnfilteredX = 0; 78 offsetUnfilteredY = 0; 79 offsetUnfilteredZ = 0; 80 mag_z_msb = 0; 81 mag_z_lsb = 0x01; // Default for self test result bit 0 is 1 82 mag_y_msb = 0; 83 mag_y_lsb = 0x01; // Default for self test result bit 0 is 1 84 mag_x_msb = 0; 85 mag_x_lsb = 0x01; // Default for self test result bit 0 is 1 86 mag_rhall_lsb = 0; 87 mag_rhall_msb = 0; 88 magIntrStatus = 0; 89 magCtrl = 0x01; 90 magOpModeCtrl = 0x6; 91 magIntrCtrl1 = 0x3F; 92 magIntrCtrl2 = 0x07; 93 magLowTh = 0; 94 magHighTh = 0; 95 magRepXY = 0; 96 magRepZ = 0; 97 state = (uint)States.Idle; 98 registerAddress = 0; 99 registerData = 0; 100 sensorFlipState = (byte)SensorFlipStates.XYZ_111; 101 } 102 Write(byte[] data)103 public void Write(byte[] data) 104 { 105 // Parse the list bytes 106 if(data.Length < 2) 107 { 108 // Must always have mode and register address in list 109 this.Log(LogLevel.Warning, "Write - too few elements in list ({0}) - must be at least two", data.Length); 110 return; 111 } 112 this.NoisyLog("Write {0}", data.Select(x => x.ToString("X")).Aggregate((x, y) => x + " " + y)); 113 // First byte sets the device state 114 state = data[0]; 115 this.Log(LogLevel.Noisy, "State changed to {0}", (States)state); 116 // Second byte is always register address 117 registerAddress = data[1]; 118 if(data.Length == 3) 119 { 120 registerData = data[2]; 121 } 122 switch((States)state) 123 { 124 case States.ReceivingData: 125 switch((Registers)registerAddress) 126 { 127 case Registers.SoftReset: 128 // Soft Reset command for Bosch BMC050, BMA180 and BMP180 129 if(registerData == 0xB6) 130 { 131 Reset(); 132 } 133 break; 134 case Registers.g_Range: 135 switch((g_RangeModes)registerData) 136 { 137 case g_RangeModes.Range2g: 138 case g_RangeModes.Range4g: 139 case g_RangeModes.Range8g: 140 case g_RangeModes.Range16g: 141 gRange = (g_RangeModes)registerData; 142 break; 143 default: 144 // Other values are not allowed ans shall default to 2g selection 145 gRange = g_RangeModes.Range2g; 146 break; 147 } 148 break; 149 case Registers.Bandwidth: 150 bandwidth = (BandwidthModes)registerData; 151 break; 152 case Registers.Power: 153 // TODO: Power functionality not yet implemented (Suspend, Sleep duration etc) 154 power = registerData; 155 break; 156 case Registers.DaqCtrl: 157 // TODO: DAQ control functionality not implemented 158 daqCtrl = registerData; 159 break; 160 case Registers.IrqCtrl1: 161 // TODO: interrupt functionality not implemented 162 irqCtrl1 = registerData; 163 break; 164 case Registers.IrqCtrl2: 165 // TODO: interrupt functionality not implemented 166 irqCtrl2 = registerData; 167 break; 168 case Registers.IrqMap1: 169 // TODO: interrupt functionality not implemented 170 irqMap1 = registerData; 171 break; 172 case Registers.IrqMap2: 173 // TODO: interrupt functionality not implemented 174 irqMap2 = registerData; 175 break; 176 case Registers.IrqMap3: 177 // TODO: interrupt functionality not implemented 178 irqMap3 = registerData; 179 break; 180 case Registers.IrqSource: 181 // TODO: interrupt functionality not implemented 182 irqSource = registerData; 183 break; 184 case Registers.IrqBehaviour: 185 // TODO: interrupt functionality not implemented 186 irqBehaviour = registerData; 187 break; 188 case Registers.IrqMode: 189 // TODO: interrupt functionality not implemented 190 irqMode = registerData; 191 break; 192 case Registers.LowDur: 193 // TODO: interrupt functionality not implemented 194 lowDur = registerData; 195 break; 196 case Registers.LowTh: 197 // TODO: interrupt functionality not implemented 198 lowTh = registerData; 199 break; 200 case Registers.Hy: 201 // TODO: interrupt functionality not implemented 202 hy = registerData; 203 break; 204 case Registers.HighDur: 205 // TODO: interrupt functionality not implemented 206 highDur = registerData; 207 break; 208 case Registers.HighTh: 209 // TODO: interrupt functionality not implemented 210 highTh = registerData; 211 break; 212 case Registers.SlopeDur: 213 // TODO: interrupt functionality not implemented 214 slopeDur = registerData; 215 break; 216 case Registers.SlopeTh: 217 // TODO: interrupt functionality not implemented 218 slopeTh = registerData; 219 break; 220 case Registers.TapConfig1: 221 // TODO: interrupt functionality not implemented 222 tapConfig1 = registerData; 223 break; 224 case Registers.TapConfig2: 225 // TODO: interrupt functionality not implemented 226 tapConfig2 = registerData; 227 break; 228 case Registers.OrientConfig1: 229 // TODO: interrupt functionality not implemented 230 orientConfig1 = registerData; 231 break; 232 case Registers.OrientConfig2: 233 // TODO: interrupt functionality not implemented 234 orientConfig2 = registerData; 235 break; 236 case Registers.FlatConfig1: 237 // TODO: interrupt functionality not implemented 238 flatConfig1 = registerData; 239 break; 240 case Registers.FlatConfig2: 241 // TODO: interrupt functionality not implemented 242 flatConfig2 = registerData; 243 break; 244 case Registers.SelfTest: 245 // TODO: self test not implemented 246 selfTest = (SelfTestModes)registerData; 247 break; 248 case Registers.EEPROMCtrl: 249 // TODO: EEPROM handling not implemented 250 eepromCtrl = registerData; 251 break; 252 case Registers.InterfaceConfig: 253 // TODO: watchdog on interfaces not implemented 254 interfaceConfig = registerData; 255 break; 256 case Registers.OffsetCompensation: 257 // Setting bit 7 resets the register to value 0 258 if((registerData & 0x80) == 0x80) 259 { 260 offsetCompensation = 0; 261 } 262 else 263 { 264 offsetCompensation = registerData; 265 } 266 break; 267 case Registers.OffsetTarget: 268 offsetTarget = registerData; 269 break; 270 case Registers.OffsetFilteredX: 271 offsetFilteredX = registerData; 272 break; 273 case Registers.OffsetFilteredY: 274 offsetFilteredY = registerData; 275 break; 276 case Registers.OffsetFilteredZ: 277 offsetFilteredZ = registerData; 278 break; 279 case Registers.OffsetUnfilteredX: 280 offsetUnfilteredX = registerData; 281 break; 282 case Registers.OffsetUnfilteredY: 283 offsetUnfilteredY = registerData; 284 break; 285 case Registers.OffsetUnfilteredZ: 286 offsetUnfilteredZ = registerData; 287 break; 288 case Registers.MagCtrl: 289 magCtrl = registerData; 290 // Enforce fixed zero bits 291 magCtrl &= 0x87; 292 break; 293 case Registers.MagOpModeCtrl: 294 magOpModeCtrl = registerData; 295 break; 296 case Registers.MagIntrCtrl1: 297 magIntrCtrl1 = registerData; 298 break; 299 case Registers.MagIntrCtrl2: 300 magIntrCtrl2 = registerData; 301 break; 302 case Registers.MagLowTh: 303 magLowTh = registerData; 304 break; 305 case Registers.MagHighTh: 306 magHighTh = registerData; 307 break; 308 case Registers.MagRepXY: 309 magRepXY = registerData; 310 break; 311 case Registers.MagRepZ: 312 magRepZ = registerData; 313 break; 314 default: 315 this.Log(LogLevel.Noisy, "Register address invalid - no action"); 316 break; 317 } 318 state = (uint)States.Idle; 319 this.Log(LogLevel.Noisy, "State changed to Idle"); 320 break; 321 case States.SendingData: 322 byte[] result = new byte[1] { 0 }; 323 switch((Registers)registerAddress) 324 { 325 case Registers.Acc_X_LSB: 326 case Registers.Acc_X_MSB: 327 GetAccelerometerX(); 328 GetAccelerometerY(); 329 GetAccelerometerZ(); 330 result = new byte[6] { 0, 0, 0, 0, 0, 0 }; 331 result[0] = acc_x_lsb; 332 result[1] = acc_x_msb; 333 result[2] = acc_y_lsb; 334 result[3] = acc_y_msb; 335 result[4] = acc_z_lsb; 336 result[5] = acc_z_msb; 337 // Read clears new_data_xyz bit 338 acc_x_lsb &= 0xFE; 339 acc_y_lsb &= 0xFE; 340 acc_z_lsb &= 0xFE; 341 // Only flip the sensor after reading ACC_Z registers 342 // Assuming reads of all three axis for each turn 343 FlipSensor(); 344 break; 345 case Registers.Acc_Y_LSB: 346 case Registers.Acc_Y_MSB: 347 GetAccelerometerY(); 348 result = new byte[2] { 0, 0 }; 349 result[0] = acc_y_lsb; 350 result[1] = acc_y_msb; 351 // Read clears new_data_y bit 352 acc_y_lsb &= 0xFE; 353 break; 354 case Registers.Acc_Z_LSB: 355 case Registers.Acc_Z_MSB: 356 GetAccelerometerZ(); 357 result = new byte[2] { 0, 0 }; 358 result[0] = acc_z_lsb; 359 result[1] = acc_z_msb; 360 // Read clears new_data_z bit 361 acc_z_lsb &= 0xFE; 362 // Only flip the sensor after reading ACC_Z registers 363 // Assuming reads of all three axis for each turn 364 FlipSensor(); 365 break; 366 case Registers.ChipID: 367 // Same Chip ID for Bosch BMC050 and BMA180 368 result[0] = 0x3; 369 break; 370 case Registers.Reserved1: 371 // Reserved register, default value = 0x21, in BMC050 data sheet (aka BMA150 data sheet v1.4) 372 result[0] = 0x21; 373 break; 374 case Registers.Reserved2: 375 case Registers.Reserved3: 376 case Registers.Reserved4: 377 case Registers.Reserved5: 378 case Registers.Reserved6: 379 case Registers.Reserved7: 380 case Registers.Reserved8: 381 case Registers.Reserved9: 382 case Registers.Reserved10: 383 case Registers.Reserved11: 384 case Registers.Reserved12: 385 case Registers.Reserved13: 386 case Registers.Reserved14: 387 case Registers.Reserved15: 388 // Reserved registers, default value = 0x0, in BMC050 data sheet (aka BMA150 data sheet v1.4) 389 result[0] = 0x0; 390 break; 391 case Registers.Temperature: 392 GetTemperature(); 393 result[0] = temperature; 394 break; 395 case Registers.StatusReg1: 396 result[0] = statusReg1; 397 break; 398 case Registers.StatusReg2: 399 result[0] = statusReg2; 400 break; 401 case Registers.StatusReg3: 402 result[0] = statusReg3; 403 break; 404 case Registers.StatusReg4: 405 result[0] = statusReg4; 406 break; 407 case Registers.g_Range: 408 result[0] = (byte)gRange; 409 break; 410 case Registers.Bandwidth: 411 result[0] = (byte)bandwidth; 412 break; 413 case Registers.Power: 414 result[0] = power; 415 break; 416 case Registers.DaqCtrl: 417 result[0] = daqCtrl; 418 break; 419 case Registers.SoftReset: 420 result[0] = 0; 421 break; 422 case Registers.IrqCtrl1: 423 result[0] = irqCtrl1; 424 break; 425 case Registers.IrqCtrl2: 426 result[0] = irqCtrl2; 427 break; 428 case Registers.IrqMap1: 429 result[0] = irqMap1; 430 break; 431 case Registers.IrqMap2: 432 result[0] = irqMap2; 433 break; 434 case Registers.IrqMap3: 435 result[0] = irqMap3; 436 break; 437 case Registers.IrqSource: 438 result[0] = irqSource; 439 break; 440 case Registers.IrqBehaviour: 441 result[0] = irqBehaviour; 442 break; 443 case Registers.IrqMode: 444 result[0] = irqMode; 445 break; 446 case Registers.LowDur: 447 result[0] = lowDur; 448 break; 449 case Registers.LowTh: 450 result[0] = lowTh; 451 break; 452 case Registers.Hy: 453 result[0] = hy; 454 break; 455 case Registers.HighDur: 456 result[0] = highDur; 457 break; 458 case Registers.HighTh: 459 result[0] = highTh; 460 break; 461 case Registers.SlopeDur: 462 result[0] = slopeDur; 463 break; 464 case Registers.SlopeTh: 465 result[0] = slopeTh; 466 break; 467 case Registers.TapConfig1: 468 result[0] = tapConfig1; 469 break; 470 case Registers.TapConfig2: 471 result[0] = tapConfig2; 472 break; 473 case Registers.OrientConfig1: 474 result[0] = orientConfig1; 475 break; 476 case Registers.OrientConfig2: 477 result[0] = orientConfig2; 478 break; 479 case Registers.FlatConfig1: 480 result[0] = flatConfig1; 481 break; 482 case Registers.FlatConfig2: 483 result[0] = flatConfig2; 484 break; 485 case Registers.SelfTest: 486 result[0] = (byte)selfTest; 487 break; 488 case Registers.EEPROMCtrl: 489 result[0] = eepromCtrl; 490 break; 491 case Registers.InterfaceConfig: 492 result[0] = interfaceConfig; 493 break; 494 case Registers.OffsetCompensation: 495 result[0] = offsetCompensation; 496 break; 497 case Registers.OffsetTarget: 498 result[0] = offsetTarget; 499 break; 500 case Registers.OffsetFilteredX: 501 result[0] = offsetFilteredX; 502 break; 503 case Registers.OffsetFilteredY: 504 result[0] = offsetFilteredY; 505 break; 506 case Registers.OffsetFilteredZ: 507 result[0] = offsetFilteredZ; 508 break; 509 case Registers.OffsetUnfilteredX: 510 result[0] = offsetUnfilteredX; 511 break; 512 case Registers.OffsetUnfilteredY: 513 result[0] = offsetUnfilteredY; 514 break; 515 case Registers.OffsetUnfilteredZ: 516 result[0] = offsetUnfilteredZ; 517 break; 518 case Registers.MagChipID: 519 // Magnetometer Chip ID can only be read if power ctrl bit is set in MagCtrl (0x4B) 520 if((magCtrl & 0x1) == 0x1) 521 { 522 result[0] = 0x32; 523 } 524 else 525 { 526 result[0] = 0x0; 527 } 528 break; 529 case Registers.Mag_X_LSB: 530 case Registers.Mag_X_MSB: 531 GetMagnetometerX(); 532 GetMagnetometerY(); 533 GetMagnetometerZ(); 534 GetHallResistance(); 535 result = new byte[8] { 0, 0, 0, 0, 0, 0, 0, 0 }; 536 result[0] = mag_x_lsb; 537 result[1] = mag_x_msb; 538 result[2] = mag_y_lsb; 539 result[3] = mag_y_msb; 540 result[4] = mag_z_lsb; 541 result[5] = mag_z_msb; 542 result[6] = mag_rhall_lsb; 543 result[7] = mag_rhall_msb; 544 break; 545 case Registers.Mag_Y_LSB: 546 case Registers.Mag_Y_MSB: 547 GetMagnetometerY(); 548 result = new byte[2] { 0, 0 }; 549 result[0] = mag_y_lsb; 550 result[1] = mag_y_msb; 551 break; 552 case Registers.Mag_Z_LSB: 553 case Registers.Mag_Z_MSB: 554 GetMagnetometerZ(); 555 result = new byte[2] { 0, 0 }; 556 result[0] = mag_z_lsb; 557 result[1] = mag_z_msb; 558 break; 559 case Registers.Mag_RHALL_LSB: 560 case Registers.Mag_RHALL_MSB: 561 GetHallResistance(); 562 result = new byte[2] { 0, 0 }; 563 result[0] = mag_rhall_lsb; 564 result[1] = mag_rhall_msb; 565 break; 566 case Registers.MagIntrStatus: 567 result[0] = magIntrStatus; 568 break; 569 case Registers.MagCtrl: 570 result[0] = magCtrl; 571 break; 572 case Registers.MagOpModeCtrl: 573 result[0] = magOpModeCtrl; 574 break; 575 case Registers.MagIntrCtrl1: 576 result[0] = magIntrCtrl1; 577 break; 578 case Registers.MagIntrCtrl2: 579 result[0] = magIntrCtrl2; 580 break; 581 case Registers.MagLowTh: 582 result[0] = magLowTh; 583 break; 584 case Registers.MagHighTh: 585 result[0] = magHighTh; 586 break; 587 case Registers.MagRepXY: 588 result[0] = magRepXY; 589 break; 590 case Registers.MagRepZ: 591 result[0] = magRepZ; 592 break; 593 default: 594 break; 595 } 596 sendData = new byte[result.Length + 1]; 597 result.CopyTo(sendData, 0); 598 sendData[result.Length] = GetCRC(data, result); 599 break; 600 default: 601 break; 602 } 603 } 604 Read(int count = 0)605 public byte[] Read(int count = 0) 606 { 607 if(sendData == null || sendData.Length < count) 608 { 609 this.Log(LogLevel.Warning, "Attemped to read {0} bytes, while {1} is available", count, sendData == null ? 0 : sendData.Length); 610 return new byte[count]; 611 } 612 else 613 { 614 this.NoisyLog("Read {0}", sendData.Select(x => x.ToString("X")).Aggregate((x, y) => x + " " + y)); 615 return sendData; 616 } 617 } 618 FinishTransmission()619 public void FinishTransmission() 620 { 621 } 622 SensorData(double mean, double sigma)623 private double SensorData(double mean, double sigma) 624 { 625 // mean = mean value of Gaussian (Normal) distribution and sigma = standard deviation 626 int sign = random.Next(10); 627 double x = 0.0; 628 if(sign > 5) 629 { 630 x = mean * (1.0 + random.NextDouble() / (2 * sigma)); 631 } 632 else 633 { 634 x = mean * (1.0 - random.NextDouble() / (2 * sigma)); 635 } 636 double z = (x - mean) / sigma; 637 double variance = Math.Pow(sigma, 2); 638 double exponent = -0.5 * Math.Pow(z, 2) / variance; 639 double gaussian = (1 / (sigma * Math.Sqrt(2 * Math.PI))) * Math.Pow(Math.E, exponent); 640 this.Log(LogLevel.Noisy, "Sensordata x: " + x.ToString()); 641 this.Log(LogLevel.Noisy, "Probability of x: " + gaussian.ToString()); 642 return x; 643 } 644 GetTemperature()645 private void GetTemperature() 646 { 647 // TODO: Bogus temp data, should return value to be used with calibration data to calculate T 648 double preciseTemperature = SensorData(25.0, 1.0); 649 temperature = (byte)(Convert.ToInt16(Math.Round(preciseTemperature)) & 0xFF); 650 this.Log(LogLevel.Noisy, "Temperature: " + temperature.ToString()); 651 } 652 FlipSensor()653 private void FlipSensor() 654 { 655 // Flip the sensor 180 degrees around a central axis (x,y or z) 656 sensorFlipState <<= 1; 657 // Return to zero rotation state after six flips (bit shifts) 658 if(sensorFlipState > Convert.ToInt16(SensorFlipStates.XYZ_110)) 659 { 660 sensorFlipState = (byte)SensorFlipStates.XYZ_111; 661 } 662 } 663 GetAccelerometerX()664 private void GetAccelerometerX() 665 { 666 // TODO: Should also handle different modes 667 // Use the sensor flip state to determine mean for data generation 668 // Either close to +1g or -1g 669 // 10-bit ADCs --> value in {0, 1023} 670 Int16 accelerometerX = 0; 671 switch((SensorFlipStates)sensorFlipState) 672 { 673 case SensorFlipStates.XYZ_111: 674 case SensorFlipStates.XYZ_110: 675 case SensorFlipStates.XYZ_100: 676 accelerometerX = Convert.ToInt16(Math.Round(SensorData(900.0, 10.0))); 677 break; 678 case SensorFlipStates.XYZ_000: 679 case SensorFlipStates.XYZ_001: 680 case SensorFlipStates.XYZ_011: 681 accelerometerX = Convert.ToInt16(Math.Round(SensorData(100.0, 10.0))); 682 break; 683 default: 684 break; 685 } 686 accelerometerX &= 0x3FF; 687 // MSB is bits 9:2 688 acc_x_msb = (byte)((accelerometerX >> 2) & 0xFF); 689 // LSB is bits 1:0 | 0 | new_data_x (bit 0 shows if data has been read out or is new) 690 acc_x_lsb = (byte)((accelerometerX & 0x3) << 6); 691 acc_x_lsb |= 0x1; // Set bit for new data 692 this.Log(LogLevel.Noisy, "Acc_X_MSB: " + acc_x_msb.ToString()); 693 this.Log(LogLevel.Noisy, "Acc_X_LSB: " + acc_x_lsb.ToString()); 694 } 695 GetAccelerometerY()696 private void GetAccelerometerY() 697 { 698 // TODO: Should also handle different modes 699 // Use the sensor flip state to determine mean for data generation 700 // Either close to +1g or -1g 701 // 10-bit ADCs --> value in {0, 1023} 702 Int16 accelerometerY = 0; 703 switch((SensorFlipStates)sensorFlipState) 704 { 705 case SensorFlipStates.XYZ_111: 706 case SensorFlipStates.XYZ_110: 707 case SensorFlipStates.XYZ_100: 708 accelerometerY = Convert.ToInt16(Math.Round(SensorData(900.0, 10.0))); 709 break; 710 case SensorFlipStates.XYZ_000: 711 case SensorFlipStates.XYZ_001: 712 case SensorFlipStates.XYZ_011: 713 accelerometerY = Convert.ToInt16(Math.Round(SensorData(100.0, 10.0))); 714 break; 715 default: 716 break; 717 } 718 accelerometerY &= 0x3FF; 719 // MSB is bits 9:2 720 acc_y_msb = (byte)((accelerometerY >> 2) & 0xFF); 721 // LSB is bits 1:0 | 0 | new_data_y (bit 0 shows if data has been read out or is new) 722 acc_y_lsb = (byte)((accelerometerY & 0x3) << 6); 723 acc_y_lsb |= 0x1; // Set bit for new data 724 this.Log(LogLevel.Noisy, "Acc_Y_MSB: " + acc_y_msb.ToString()); 725 this.Log(LogLevel.Noisy, "Acc_Y_LSB: " + acc_y_lsb.ToString()); 726 } 727 GetAccelerometerZ()728 private void GetAccelerometerZ() 729 { 730 // TODO: Should also handle different modes 731 // Use the sensor flip state to determine mean for data generation 732 // Either close to +1g or -1g 733 // 10-bit ADCs --> value in {0, 1023} 734 Int16 accelerometerZ = 0; 735 switch((SensorFlipStates)sensorFlipState) 736 { 737 case SensorFlipStates.XYZ_111: 738 case SensorFlipStates.XYZ_110: 739 case SensorFlipStates.XYZ_100: 740 accelerometerZ = Convert.ToInt16(Math.Round(SensorData(900.0, 10.0))); 741 break; 742 case SensorFlipStates.XYZ_000: 743 case SensorFlipStates.XYZ_001: 744 case SensorFlipStates.XYZ_011: 745 accelerometerZ = Convert.ToInt16(Math.Round(SensorData(100.0, 10.0))); 746 break; 747 default: 748 break; 749 } 750 accelerometerZ &= 0x3FF; 751 // MSB is bits 9:2 752 acc_z_msb = (byte)((accelerometerZ >> 2) & 0xFF); 753 // LSB is bits 1:0 | 0 | new_data_z (bit 0 shows if data has been read out or is new) 754 acc_z_lsb = (byte)((accelerometerZ & 0x3) << 6); 755 acc_z_lsb |= 0x1; // Set bit for new data 756 this.Log(LogLevel.Noisy, "Acc_Z_MSB: " + acc_z_msb.ToString()); 757 this.Log(LogLevel.Noisy, "Acc_Z_LSB: " + acc_z_lsb.ToString()); 758 } 759 GetMagnetometerX()760 private void GetMagnetometerX() 761 { 762 // TODO: Should also handle different modes 763 // Use the sensor flip state to determine mean for data generation 764 // TODO: Different scenario for magnetic field data generation 765 // 13-bit ADCs --> value in {0, 8191} 766 Int16 magnetometerX = 0; 767 switch((SensorFlipStates)sensorFlipState) 768 { 769 case SensorFlipStates.XYZ_111: 770 case SensorFlipStates.XYZ_110: 771 case SensorFlipStates.XYZ_100: 772 magnetometerX = Convert.ToInt16(Math.Round(SensorData(7900.0, 40.0))); 773 break; 774 case SensorFlipStates.XYZ_000: 775 case SensorFlipStates.XYZ_001: 776 case SensorFlipStates.XYZ_011: 777 magnetometerX = Convert.ToInt16(Math.Round(SensorData(290.0, 40.0))); 778 break; 779 default: 780 break; 781 } 782 magnetometerX &= 0x1FFF; 783 // MSB is bits 12:5 784 mag_x_msb = (byte)((magnetometerX >> 5) & 0xFF); 785 // LSB is bits 4:0 | 0 | self_test_x (bit 0 shows result of self test, default 1) 786 mag_x_lsb = (byte)((magnetometerX & 0x1F) << 3); 787 // TODO: add result of self test - fault injection possibility 788 mag_x_lsb |= 0x1; // Set bit for self test 789 this.Log(LogLevel.Noisy, "Mag_X_MSB: " + mag_x_msb.ToString()); 790 this.Log(LogLevel.Noisy, "Mag_X_LSB: " + mag_x_lsb.ToString()); 791 } 792 GetMagnetometerY()793 private void GetMagnetometerY() 794 { 795 // TODO: Should also handle different modes 796 // Use the sensor flip state to determine mean for data generation 797 // TODO: Different scenario for magnetic field data generation 798 // 13-bit ADCs --> value in {0, 8191} 799 Int16 magnetometerY = 0; 800 switch((SensorFlipStates)sensorFlipState) 801 { 802 case SensorFlipStates.XYZ_111: 803 case SensorFlipStates.XYZ_110: 804 case SensorFlipStates.XYZ_100: 805 magnetometerY = Convert.ToInt16(Math.Round(SensorData(7900.0, 40.0))); 806 break; 807 case SensorFlipStates.XYZ_000: 808 case SensorFlipStates.XYZ_001: 809 case SensorFlipStates.XYZ_011: 810 magnetometerY = Convert.ToInt16(Math.Round(SensorData(290.0, 40.0))); 811 break; 812 default: 813 break; 814 } 815 magnetometerY &= 0x1FFF; 816 // MSB is bits 12:5 817 mag_y_msb = (byte)((magnetometerY >> 5) & 0xFF); 818 // LSB is bits 4:0 | 0 | self_test_y (bit 0 shows result of self test, default 1) 819 mag_y_lsb = (byte)((magnetometerY & 0x1F) << 3); 820 // TODO: add result of self test - fault injection possibility 821 mag_y_lsb |= 0x1; // Set bit for self test 822 this.Log(LogLevel.Noisy, "Mag_Y_MSB: " + mag_y_msb.ToString()); 823 this.Log(LogLevel.Noisy, "Mag_Y_LSB: " + mag_y_lsb.ToString()); 824 } 825 GetMagnetometerZ()826 private void GetMagnetometerZ() 827 { 828 // TODO: Should also handle different modes 829 // Use the sensor flip state to determine mean for data generation 830 // TODO: Different scenario for magnetic field data generation 831 // 13-bit ADCs --> value in {0, 8191} 832 Int16 magnetometerZ = 0; 833 switch((SensorFlipStates)sensorFlipState) 834 { 835 case SensorFlipStates.XYZ_111: 836 case SensorFlipStates.XYZ_110: 837 case SensorFlipStates.XYZ_100: 838 magnetometerZ = Convert.ToInt16(Math.Round(SensorData(7900.0, 40.0))); 839 break; 840 case SensorFlipStates.XYZ_000: 841 case SensorFlipStates.XYZ_001: 842 case SensorFlipStates.XYZ_011: 843 magnetometerZ = Convert.ToInt16(Math.Round(SensorData(290.0, 40.0))); 844 break; 845 default: 846 break; 847 } 848 magnetometerZ &= 0x1FFF; 849 // MSB is bits 12:5 850 mag_z_msb = (byte)((magnetometerZ >> 5) & 0xFF); 851 // LSB is bits 4:0 | 0 | self_test_z (bit 0 shows result of self test, default 1) 852 mag_z_lsb = (byte)((magnetometerZ & 0x1F) << 3); 853 // TODO: add result of self test - fault injection possibility 854 mag_z_lsb |= 0x1; // Set bit for self test 855 this.Log(LogLevel.Noisy, "Mag_Z_MSB: " + mag_z_msb.ToString()); 856 this.Log(LogLevel.Noisy, "Mag_Z_LSB: " + mag_z_lsb.ToString()); 857 } 858 GetHallResistance()859 private void GetHallResistance() 860 { 861 // TODO: Bogus Hall resistance data 862 // 14-bit ADC 863 // TODO: Should also handle different modes 864 Int16 resistanceHall = Convert.ToInt16(Math.Round(SensorData(50.0, 10.0))); 865 // MSB is bits 13:6 866 mag_rhall_msb = (byte)((resistanceHall >> 6) & 0xFF); 867 // LSB is bits 5:0 | 0 | data ready status bit 868 mag_rhall_lsb = (byte)((resistanceHall & 0x3F) << 2); 869 this.Log(LogLevel.Noisy, "Mag_RHALL_MSB: " + mag_rhall_msb.ToString()); 870 this.Log(LogLevel.Noisy, "Mag_RHALL_LSB: " + mag_rhall_lsb.ToString()); 871 } 872 GetCRC(byte[] input, byte[] output)873 private byte GetCRC(byte[] input, byte[] output) 874 { 875 var crc = input[0]; 876 for(int i = 1; i < input.Length; i++) 877 { 878 crc ^= input[i]; 879 } 880 for(int j = 0; j < output.Length; j++) 881 { 882 crc ^= output[j]; 883 } 884 return (byte)(crc); 885 } 886 887 private byte acc_z_msb; 888 private byte acc_z_lsb; 889 private byte acc_y_msb; 890 private byte acc_y_lsb; 891 private byte acc_x_msb; 892 private byte acc_x_lsb; 893 private byte temperature; 894 private byte statusReg1; 895 private byte statusReg2; 896 private byte statusReg3; 897 private byte statusReg4; 898 private g_RangeModes gRange; 899 private BandwidthModes bandwidth; 900 private byte power; 901 private byte daqCtrl; 902 private byte irqCtrl1; 903 private byte irqCtrl2; 904 private byte irqMap1; 905 private byte irqMap2; 906 private byte irqMap3; 907 private byte irqSource; 908 private byte irqBehaviour; 909 private byte irqMode; 910 private byte lowDur; 911 private byte lowTh; 912 private byte hy; 913 private byte highDur; 914 private byte highTh; 915 private byte slopeDur; 916 private byte slopeTh; 917 private byte tapConfig1; 918 private byte tapConfig2; 919 private byte orientConfig1; 920 private byte orientConfig2; 921 private byte flatConfig1; 922 private byte flatConfig2; 923 private SelfTestModes selfTest; 924 private byte eepromCtrl; 925 private byte interfaceConfig; 926 private byte offsetCompensation; 927 private byte offsetTarget; 928 private byte offsetFilteredX; 929 private byte offsetFilteredY; 930 private byte offsetFilteredZ; 931 private byte offsetUnfilteredX; 932 private byte offsetUnfilteredY; 933 private byte offsetUnfilteredZ; 934 private byte mag_x_lsb; 935 private byte mag_x_msb; 936 private byte mag_y_lsb; 937 private byte mag_y_msb; 938 private byte mag_z_lsb; 939 private byte mag_z_msb; 940 private byte mag_rhall_lsb; 941 private byte mag_rhall_msb; 942 private byte magIntrStatus; 943 private byte magCtrl; 944 private byte magOpModeCtrl; 945 private byte magIntrCtrl1; 946 private byte magIntrCtrl2; 947 private byte magLowTh; 948 private byte magHighTh; 949 private byte magRepXY; 950 private byte magRepZ; 951 952 // Internal use only 953 private uint state; 954 private byte registerAddress; 955 private byte registerData; 956 private byte[] sendData; 957 private byte sensorFlipState; 958 959 private static PseudorandomNumberGenerator random = EmulationManager.Instance.CurrentEmulation.RandomGenerator; 960 961 private enum g_RangeModes 962 { 963 // Allowed modes - other values shall result in Range2g setting, bits 0-3 964 Range2g = 0x3, // Selects ±2g range, default value 965 Range4g = 0x5, // Selects ±4g range 966 Range8g = 0x8, // Selects ±8g range 967 Range16g = 0xC // Selects ±16g range 968 } 969 970 private enum BandwidthModes 971 { 972 // Allowed modes - bits 0-4 973 Bandwidth0 = 0x7, // A mask, 00xxxb selects 7.81 Hz 974 Bandwidth1 = 0x8, // Selects 7.81 Hz 975 Bandwidth2 = 0x9, // Selects 15.63 Hz 976 Bandwidth3 = 0xA, // Selects 31.25 Hz 977 Bandwidth4 = 0xB, // Selects 62.5 Hz 978 Bandwidth5 = 0xC, // Selects 125 Hz 979 Bandwidth6 = 0xD, // Selects 250 Hz 980 Bandwidth7 = 0xE, // Selects 500 Hz 981 Bandwidth8 = 0xF, // Selects 1000 Hz 982 Bandwidth9 = 0x10, // A mask, 1xxxxb selects 1000 Hz 983 } 984 985 private enum SelfTestModes 986 { 987 Idle = 0x70, 988 Pos_X_Axis = 0x71, 989 Pos_Y_Axis = 0x72, 990 Pos_Z_Axis = 0x73, 991 Neg_X_Axis = 0x75, 992 Neg_Y_Axis = 0x76, 993 Neg_Z_Axis = 0x77 994 } 995 996 private enum SensorFlipStates 997 { 998 // State used for generation of sensor data. 999 // The idea is that between each read request the sensor is flipped 180 degrees 1000 // around one of its three central axis x, y or z from pos. 1001 // The flip changes the axis orientation in label: 1002 // 1 means positive (+1g), 0 means negative (-1g) 1003 // Sensor data generation sequence is 1004 // 1) X: +1g -> -1g 1005 // 2) Y: +1g -> -1g 1006 // 3) Z: +1g -> -1g 1007 // 4) X: -1g -> +1g 1008 // 5) Y: -1g -> +1g 1009 // 6) Z: -1g -> +1g 1010 // where number indicates bit number flipped in state tracking byte 1011 XYZ_111 = 0x01, 1012 XYZ_011 = 0x02, 1013 XYZ_001 = 0x04, 1014 XYZ_000 = 0x08, 1015 XYZ_100 = 0x10, 1016 XYZ_110 = 0x20 1017 } 1018 1019 private enum States 1020 { 1021 Idle = 0x0, 1022 ReceivingData = 0xFD, 1023 SendingData = 0xFC 1024 } 1025 1026 private enum Registers 1027 { 1028 ChipID = 0x00, // Read-Only 1029 Reserved1 = 0x01, // Reserved 1030 Acc_X_LSB = 0x02, // Read-Only 1031 Acc_X_MSB = 0x03, // Read-Only 1032 Acc_Y_LSB = 0x04, // Read-Only 1033 Acc_Y_MSB = 0x05, // Read-Only 1034 Acc_Z_LSB = 0x06, // Read-Only 1035 Acc_Z_MSB = 0x07, // Read-Only 1036 Temperature = 0x08, // Read-Only 1037 StatusReg1 = 0x09, // Read-Only 1038 StatusReg2 = 0x0A, // Read-Only 1039 StatusReg3 = 0x0B, // Read-Only 1040 StatusReg4 = 0x0C, // Read-Only 1041 Reserved2 = 0x0D, // Reserved 1042 Reserved3 = 0x0E, // Reserved 1043 g_Range = 0x0F, // Read-Write 1044 Bandwidth = 0x10, // Read-Write 1045 Power = 0x11, // Read-Write 1046 Reserved4 = 0x12, // Reserved 1047 DaqCtrl = 0x13, // Read-Write 1048 SoftReset = 0x14, // Read-Write 1049 Reserved5 = 0x15, // Reserved 1050 IrqCtrl1 = 0x16, // Read-Write 1051 IrqCtrl2 = 0x17, // Read-Write 1052 Reserved6 = 0x18, // Reserved 1053 IrqMap1 = 0x19, // Read-Write 1054 IrqMap2 = 0x1A, // Read-Write 1055 IrqMap3 = 0x1B, // Read-Write 1056 Reserved7 = 0x1C, // Reserved 1057 Reserved8 = 0x1D, // Reserved 1058 IrqSource = 0x1E, // Read-Write 1059 Reserved9 = 0x1F, // Reserved 1060 IrqBehaviour = 0x20, // Read-Write 1061 IrqMode = 0x21, // Read-Write 1062 LowDur = 0x22, // Read-Write 1063 LowTh = 0x23, // Read-Write 1064 Hy = 0x24, // Read-Write 1065 HighDur = 0x25, // Read-Write 1066 HighTh = 0x26, // Read-Write 1067 SlopeDur = 0x27, // Read-Write 1068 SlopeTh = 0x28, // Read-Write 1069 Reserved10 = 0x29, // Reserved 1070 TapConfig1 = 0x2A, // Read-Write 1071 TapConfig2 = 0x2B, // Read-Write 1072 OrientConfig1 = 0x2C, // Read-Write 1073 OrientConfig2 = 0x2D, // Read-Write 1074 FlatConfig1 = 0x2E, // Read-Write 1075 FlatConfig2 = 0x2F, // Read-Write 1076 Reserved11 = 0x30, // Reserved 1077 Reserved12 = 0x31, // Reserved 1078 SelfTest = 0x32, // Read-Write 1079 EEPROMCtrl = 0x33, // Read-Write 1080 InterfaceConfig = 0x34, // Read-Write 1081 Reserved13 = 0x35, // Reserved 1082 OffsetCompensation = 0x36, // Read-Write 1083 OffsetTarget = 0x37, // Read-Write 1084 OffsetFilteredX = 0x38, // Read-Write 1085 OffsetFilteredY = 0x39, // Read-Write 1086 OffsetFilteredZ = 0x3A, // Read-Write 1087 OffsetUnfilteredX = 0x3B, // Read-Write 1088 OffsetUnfilteredY = 0x3C, // Read-Write 1089 OffsetUnfilteredZ = 0x3D, // Read-Write 1090 Reserved14 = 0x3E, // Reserved 1091 Reserved15 = 0x3F, // Reserved 1092 MagChipID = 0x40, // Read-Only 1093 Reserved16 = 0x41, // Reserved 1094 Mag_X_LSB = 0x42, // Read-Only 1095 Mag_X_MSB = 0x43, // Read-Only 1096 Mag_Y_LSB = 0x44, // Read-Only 1097 Mag_Y_MSB = 0x45, // Read-Only 1098 Mag_Z_LSB = 0x46, // Read-Only 1099 Mag_Z_MSB = 0x47, // Read-Only 1100 Mag_RHALL_LSB = 0x48, // Read-Only 1101 Mag_RHALL_MSB = 0x49, // Read-Only 1102 MagIntrStatus = 0x4A, // Read-Only 1103 MagCtrl = 0x4B, // Read-Write 1104 MagOpModeCtrl = 0x4C, // Read-Write 1105 MagIntrCtrl1 = 0x4D, // Read-Write 1106 MagIntrCtrl2 = 0x4E, // Read-Write 1107 MagLowTh = 0x4F, // Read-Write 1108 MagHighTh = 0x50, // Read-Write 1109 MagRepXY = 0x51, // Read-Write 1110 MagRepZ = 0x52 // Read-Write 1111 // Register addresses 0x53-0x71 are reserved and N/A 1112 // Register addresses 0x53-0x54 are images of registers on EEPROM 1113 // Offset registers 0x38-0x3D are mirrored on EEPROM and restored on reset 1114 } 1115 } 1116 } 1117 1118
