1 // 2 // Copyright (c) 2010-2020 Antmicro 3 // 4 // This file is licensed under the MIT License. 5 // Full license text is available in 'licenses/MIT.txt'. 6 // 7 using System; 8 using System.Collections.Generic; 9 using System.IO; 10 using System.Linq; 11 using Antmicro.Renode.Core; 12 using Antmicro.Renode.Core.Structure.Registers; 13 using Antmicro.Renode.Logging; 14 using Antmicro.Renode.Peripherals.I2C; 15 using Antmicro.Renode.Peripherals.Sensor; 16 using Antmicro.Renode.Utilities; 17 using Antmicro.Renode.Exceptions; 18 19 namespace Antmicro.Renode.Peripherals.Sensors 20 { 21 public class LSM303DLHC_Accelerometer : II2CPeripheral, IProvidesRegisterCollection<ByteRegisterCollection>, ISensor 22 { LSM303DLHC_Accelerometer()23 public LSM303DLHC_Accelerometer() 24 { 25 RegistersCollection = new ByteRegisterCollection(this); 26 IRQ0 = new GPIO(); 27 IRQ1 = new GPIO(); 28 irqs[0] = IRQ0; 29 irqs[1] = IRQ1; 30 DefineRegisters(); 31 } 32 FinishTransmission()33 public void FinishTransmission() 34 { 35 registerAddress = 0; 36 multipleOperation = false; 37 } 38 Reset()39 public void Reset() 40 { 41 RegistersCollection.Reset(); 42 multipleOperation = false; 43 registerAddress = 0; 44 IRQ0.Unset(); 45 IRQ1.Unset(); 46 } 47 Write(byte[] data)48 public void Write(byte[] data) 49 { 50 if(data.Length == 0) 51 { 52 this.Log(LogLevel.Warning, "Unexpected write with no data"); 53 return; 54 } 55 56 this.Log(LogLevel.Noisy, "Write with {0} bytes of data", data.Length); 57 // Bits 6-0 represent the first register address 58 registerAddress = (Registers)(data[0] & 0x7F); 59 // The most significant bit means multiple read/write operation 60 multipleOperation = data[0] > 0x80; 61 62 if(data.Length > 1) 63 { 64 // skip the first byte as it contains register address 65 foreach(var b in data.Skip(1)) 66 { 67 this.Log(LogLevel.Noisy, "Writing 0x{0:X} to register {1} (0x{1:X})", b, registerAddress); 68 RegistersCollection.Write((byte)registerAddress, b); 69 RegistersAutoIncrement(); 70 } 71 } 72 else 73 { 74 this.Log(LogLevel.Noisy, "Preparing to read register {0} (0x{0:X})", registerAddress); 75 if(dataRate.Value == 0) 76 { 77 this.Log(LogLevel.Debug, "Power-down mode is set"); 78 xyzDataAvailable.Value = false; 79 xDataAvailable.Value = false; 80 yDataAvailable.Value = false; 81 zDataAvailable.Value = false; 82 } 83 else 84 { 85 if(xAxisEnable.Value && yAxisEnable.Value && zAxisEnable.Value) 86 { 87 xyzDataAvailable.Value = true; 88 xDataAvailable.Value = false; 89 yDataAvailable.Value = false; 90 zDataAvailable.Value = false; 91 } 92 else 93 { 94 xyzDataAvailable.Value = false; 95 xDataAvailable.Value = xAxisEnable.Value; 96 yDataAvailable.Value = yAxisEnable.Value; 97 zDataAvailable.Value = zAxisEnable.Value; 98 } 99 } 100 UpdateInterrupts(); 101 } 102 } 103 Read(int count)104 public byte[] Read(int count) 105 { 106 this.Log(LogLevel.Debug, "Reading {0} bytes from register {1} (0x{1:X})", count, registerAddress); 107 var result = new byte[count]; 108 for(var i = 0; i < result.Length; i++) 109 { 110 result[i] = RegistersCollection.Read((byte)registerAddress); 111 this.Log(LogLevel.Noisy, "Read value: {0}", result[i]); 112 RegistersAutoIncrement(); 113 } 114 return result; 115 } 116 117 public decimal AccelerationX 118 { 119 get => accelarationX; 120 set 121 { 122 if(!IsAccelerationOutOfRange(value)) 123 { 124 accelarationX = value; 125 this.Log(LogLevel.Noisy, "AccelerationX set to {0}", accelarationX); 126 } 127 } 128 } 129 130 public decimal AccelerationY 131 { 132 get => accelarationY; 133 set 134 { 135 if(!IsAccelerationOutOfRange(value)) 136 { 137 accelarationY = value; 138 this.Log(LogLevel.Noisy, "AccelerationY set to {0}", accelarationY); 139 } 140 } 141 } 142 143 public decimal AccelerationZ 144 { 145 get => accelarationZ; 146 set 147 { 148 if(!IsAccelerationOutOfRange(value)) 149 { 150 accelarationZ = value; 151 this.Log(LogLevel.Noisy, "AccelerationZ set to {0}", accelarationZ); 152 } 153 } 154 } 155 156 public ByteRegisterCollection RegistersCollection { get; } 157 public GPIO IRQ0 { get; } 158 public GPIO IRQ1 { get; } 159 DefineRegisters()160 private void DefineRegisters() 161 { 162 Registers.ChipID.Define(this, 0x33); 163 164 Registers.Control1.Define(this, 0x43) //RW 165 .WithFlag(0, out xAxisEnable, name: "X_AXIS_ENABLE") 166 .WithFlag(1, out yAxisEnable, name: "Y_AXIS_ENABLE") 167 .WithFlag(2, out zAxisEnable, name: "Z_AXIS_ENABLE") 168 .WithTaggedFlag("LOW_POWER_ENABLE", 3) 169 .WithValueField(4, 4, out dataRate, name: "DATA_RATE"); 170 171 Registers.Control4.Define(this) //RW 172 .WithTaggedFlag("SIM", 0) 173 .WithTag("PREREQ", 1, 2) 174 .WithTaggedFlag("HR", 3) 175 .WithValueField(4, 2, out fullScale, name: "FS") 176 .WithTaggedFlag("BLE", 6) 177 .WithTaggedFlag("BDU", 7); 178 179 Registers.StatusReg.Define(this, 0x08) //RO 180 .WithFlag(0, out xDataAvailable, FieldMode.Read, name: "X_DATA_AVAILABLE") 181 .WithFlag(1, out yDataAvailable, FieldMode.Read, name: "Y_DATA_AVAILABLE") 182 .WithFlag(2, out zDataAvailable, FieldMode.Read, name: "Z_DATA_AVAILABLE") 183 .WithFlag(3, out xyzDataAvailable, FieldMode.Read, name: "XYZ_DATA_AVAILABLE") 184 .WithTaggedFlag("X_DATA_OVERRUN", 4) 185 .WithTaggedFlag("Y_DATA_OVERRUN", 5) 186 .WithTaggedFlag("Z_DATA_OVERRUN", 6) 187 .WithTaggedFlag("XYZ_DATA_OVERRUN", 7); 188 189 Registers.DataOutXL.Define(this) 190 .WithValueField(0, 8, FieldMode.Read, name: "X_ACCEL_DATA[7:0]", valueProviderCallback: _ => Convert(AccelerationX, upperByte: false)); 191 192 Registers.DataOutXH.Define(this) 193 .WithValueField(0, 8, FieldMode.Read, name: "X_ACCEL_DATA[15:8]", valueProviderCallback: _ => Convert(AccelerationX, upperByte: true)); 194 195 Registers.DataOutYL.Define(this) 196 .WithValueField(0, 8, FieldMode.Read, name: "Y_ACCEL_DATA[7:0]", valueProviderCallback: _ => Convert(AccelerationY, upperByte: false)); 197 198 Registers.DataOutYH.Define(this) 199 .WithValueField(0, 8, FieldMode.Read, name: "Y_ACCEL_DATA[15:8]", valueProviderCallback: _ => Convert(AccelerationY, upperByte: true)); 200 201 Registers.DataOutZL.Define(this) 202 .WithValueField(0, 8, FieldMode.Read, name: "Z_ACCEL_DATA[7:0]", valueProviderCallback: _ => Convert(AccelerationZ, upperByte: false)); 203 204 Registers.DataOutZH.Define(this) 205 .WithValueField(0, 8, FieldMode.Read, name: "Z_ACCEL_DATA[15:8]", valueProviderCallback: _ => Convert(AccelerationZ, upperByte: true)); 206 207 Registers.Interrupt1Config.Define(this, 0x3F) //RW 208 .WithFlag(0, out readyEnabledXIrq[0, 0], name: "XL_EVENT_IRQ_ENABLE") 209 .WithFlag(1, out readyEnabledXIrq[0, 1], name: "XH_EVENT_IRQ_ENABLE") 210 .WithFlag(2, out readyEnabledYIrq[0, 0], name: "YL_EVENT_IRQ_ENABLE") 211 .WithFlag(3, out readyEnabledYIrq[0, 1], name: "YH_EVENT_IRQ_ENABLE") 212 .WithFlag(4, out readyEnabledZIrq[0, 0], name: "ZL_EVENT_IRQ_ENABLE") 213 .WithFlag(5, out readyEnabledZIrq[0, 1], name: "ZH_EVENT_IRQ_ENABLE") 214 .WithTaggedFlag("6DIR_DETECT_ENABLE", 6) 215 .WithTaggedFlag("AO_IRQ_EVENTS", 7) 216 .WithWriteCallback((_, __) => UpdateInterrupts()); 217 218 Registers.Interrupt1Source.Define(this, 0x40) //RO 219 .WithFlag(0, out readyPendingXIrq[0, 0], FieldMode.Read, name: "XL_IRQ") 220 .WithFlag(1, out readyPendingXIrq[0, 1], FieldMode.Read, name: "XH_IRQ") 221 .WithFlag(2, out readyPendingYIrq[0, 0], FieldMode.Read, name: "YL_IRQ") 222 .WithFlag(3, out readyPendingYIrq[0, 1], FieldMode.Read, name: "YH_IRQ") 223 .WithFlag(4, out readyPendingZIrq[0, 0], FieldMode.Read, name: "ZL_IRQ") 224 .WithFlag(5, out readyPendingZIrq[0, 1], FieldMode.Read, name: "ZH_IRQ") 225 .WithFlag(6, out activeIrq[0] , FieldMode.Read, name: "IRQ_ACTIVE") 226 .WithTaggedFlag("PREREQ", 7); 227 228 Registers.Interrupt2Config.Define(this, 0x3F) //RW 229 .WithFlag(0, out readyEnabledXIrq[1, 0], name: "XL_EVENT_IRQ_ENABLE") 230 .WithFlag(1, out readyEnabledXIrq[1, 1], name: "XH_EVENT_IRQ_ENABLE") 231 .WithFlag(2, out readyEnabledYIrq[1, 0], name: "YL_EVENT_IRQ_ENABLE") 232 .WithFlag(3, out readyEnabledYIrq[1, 1], name: "YH_EVENT_IRQ_ENABLE") 233 .WithFlag(4, out readyEnabledZIrq[1, 0], name: "ZL_EVENT_IRQ_ENABLE") 234 .WithFlag(5, out readyEnabledZIrq[1, 1], name: "ZH_EVENT_IRQ_ENABLE") 235 .WithTaggedFlag("6DIR_DETECT_ENABLE", 6) 236 .WithTaggedFlag("AO_IRQ_EVENTS", 7) 237 .WithWriteCallback((_, __) => UpdateInterrupts()); 238 239 Registers.Interrupt2Source.Define(this, 0x40) //RO 240 .WithFlag(0, out readyPendingXIrq[1, 0], FieldMode.Read, name: "XL_IRQ") 241 .WithFlag(1, out readyPendingXIrq[1, 1], FieldMode.Read, name: "XH_IRQ") 242 .WithFlag(2, out readyPendingYIrq[1, 0], FieldMode.Read, name: "YL_IRQ") 243 .WithFlag(3, out readyPendingYIrq[1, 1], FieldMode.Read, name: "YH_IRQ") 244 .WithFlag(4, out readyPendingZIrq[1, 0], FieldMode.Read, name: "ZL_IRQ") 245 .WithFlag(5, out readyPendingZIrq[1, 1], FieldMode.Read, name: "ZH_IRQ") 246 .WithFlag(6, out activeIrq[1], FieldMode.Read, name: "IRQ_ACTIVE") 247 .WithTaggedFlag("PREREQ", 7); 248 } 249 RegistersAutoIncrement()250 private void RegistersAutoIncrement() 251 { 252 if(multipleOperation) 253 { 254 registerAddress = (Registers)((int)registerAddress + 1); 255 this.Log(LogLevel.Noisy, "Auto-incrementing to the next register 0x{0:X} - {0}", registerAddress); 256 } 257 } 258 GetSensitivity()259 private ushort GetSensitivity() 260 { 261 ushort sensitivity = 0; // [mg/LSB] 262 switch(fullScale.Value) 263 { 264 case 0: 265 sensitivity = 1; 266 break; 267 case 1: 268 sensitivity = 2; 269 break; 270 case 2: 271 sensitivity = 4; 272 break; 273 case 3: 274 sensitivity = 12; 275 break; 276 default: 277 this.Log(LogLevel.Warning, "Unsupported value of sensor sensitivity."); 278 break; 279 } 280 return sensitivity; 281 } 282 IsAccelerationOutOfRange(decimal acceleration)283 private bool IsAccelerationOutOfRange(decimal acceleration) 284 { 285 // This range protects from the overflow of the short variables in the 'Convert' function. 286 if(acceleration < MinAcceleration || acceleration > MaxAcceleration) 287 { 288 this.Log(LogLevel.Warning, "Acceleration is out of range, use value from the range <{0};{1}>", 289 MinAcceleration, MaxAcceleration); 290 return true; 291 } 292 return false; 293 } 294 Convert(decimal value, bool upperByte)295 private byte Convert(decimal value, bool upperByte) 296 { 297 decimal convertedValue = (decimal)(((short)value << 14) / (GetSensitivity() * GravitationalConst)); 298 short convertedValueAsShort = (short)convertedValue; 299 return upperByte ? (byte)(convertedValueAsShort >> 8) : (byte)convertedValueAsShort; 300 } 301 UpdateInterrupts()302 private void UpdateInterrupts() 303 { 304 var statusIrq = false; 305 var statusX = xDataAvailable.Value || xyzDataAvailable.Value; 306 var statusY = yDataAvailable.Value || xyzDataAvailable.Value; 307 var statusZ = yDataAvailable.Value || xyzDataAvailable.Value; 308 for(var irqNo = 0; irqNo < IrqAmount; ++irqNo) 309 { 310 for(var i = 0; i < AxisBytes; ++i) 311 { 312 if(activeIrq[irqNo].Value) 313 { 314 readyPendingXIrq[irqNo, i].Value = statusX && readyEnabledXIrq[irqNo, i].Value; 315 this.Log(LogLevel.Noisy, "Setting readyPendingXIrq[{0}, {1}] to {2}", 316 irqNo, i, readyPendingXIrq[irqNo, i].Value); 317 318 readyPendingYIrq[irqNo, i].Value = statusY && readyEnabledYIrq[irqNo, i].Value; 319 this.Log(LogLevel.Noisy, "Setting readyPendingYIrq[{0}, {1}] to {2}", 320 irqNo, i, readyPendingYIrq[irqNo, i].Value); 321 322 readyPendingZIrq[irqNo, i].Value = statusZ && readyEnabledZIrq[irqNo, i].Value; 323 this.Log(LogLevel.Noisy, "Setting readyPendingZIrq[{0}, {1}] to {2}", 324 irqNo, i, readyPendingZIrq[irqNo, i].Value); 325 326 statusIrq = (readyPendingXIrq[irqNo, i].Value || 327 readyPendingYIrq[irqNo, i].Value || 328 readyPendingZIrq[irqNo, i].Value); 329 } 330 } 331 332 irqs[irqNo].Set(statusIrq); 333 this.Log(LogLevel.Debug, "Setting IRQ{0} to {1}", irqNo, statusIrq); 334 } 335 } 336 337 private Registers registerAddress; 338 private bool multipleOperation; 339 340 private IValueRegisterField dataRate; 341 private IValueRegisterField fullScale; 342 private IFlagRegisterField xyzDataAvailable; 343 private IFlagRegisterField xAxisEnable, yAxisEnable, zAxisEnable; 344 private IFlagRegisterField xDataAvailable, yDataAvailable, zDataAvailable; 345 346 private GPIO[] irqs = new GPIO[IrqAmount]; 347 private IFlagRegisterField[] activeIrq = new IFlagRegisterField[IrqAmount]; 348 private IFlagRegisterField[,] readyEnabledXIrq = new IFlagRegisterField[IrqAmount, AxisBytes]; 349 private IFlagRegisterField[,] readyEnabledYIrq = new IFlagRegisterField[IrqAmount, AxisBytes]; 350 private IFlagRegisterField[,] readyEnabledZIrq = new IFlagRegisterField[IrqAmount, AxisBytes]; 351 private IFlagRegisterField[,] readyPendingXIrq = new IFlagRegisterField[IrqAmount, AxisBytes]; 352 private IFlagRegisterField[,] readyPendingYIrq = new IFlagRegisterField[IrqAmount, AxisBytes]; 353 private IFlagRegisterField[,] readyPendingZIrq = new IFlagRegisterField[IrqAmount, AxisBytes]; 354 355 private decimal accelarationX; 356 private decimal accelarationY; 357 private decimal accelarationZ; 358 359 private const decimal MinAcceleration = -19.0m; 360 private const decimal MaxAcceleration = 19.0m; 361 private const decimal GravitationalConst = 9.806650m; // [m/s^2] 362 private const ushort AxisBytes = 2; 363 private const ushort IrqAmount = 2; 364 365 private enum Registers : byte 366 { 367 ChipID = 0x0F, 368 // Reserved: 0x00 - 0x1F 369 Control1 = 0x20, 370 Control4 = 0x23, 371 StatusReg = 0x27, 372 DataOutXL = 0x28, 373 DataOutXH = 0x29, 374 DataOutYL = 0x2A, 375 DataOutYH = 0x2B, 376 DataOutZL = 0x2C, 377 DataOutZH = 0x2D, 378 Interrupt1Config = 0x30, 379 Interrupt1Source = 0x31, 380 Interrupt2Config = 0x34, 381 Interrupt2Source = 0x35, 382 // Reserved: 0x3E - 0x3F 383 } 384 } 385 } 386
