This repository has been archived by the owner on Jun 29, 2023. It is now read-only.
/
DpsClass.cpp
803 lines (720 loc) · 17.6 KB
/
DpsClass.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
#include "DpsClass.h"
using namespace dps;
const int32_t DpsClass::scaling_facts[DPS__NUM_OF_SCAL_FACTS] = {524288, 1572864, 3670016, 7864320, 253952, 516096, 1040384, 2088960};
//////// Constructor, Destructor, begin, end ////////
DpsClass::DpsClass(void)
{
//assume that initialization has failed before it has been done
m_initFail = 1U;
}
DpsClass::~DpsClass(void)
{
end();
}
void DpsClass::begin(TwoWire &bus)
{
begin(bus, DPS__STD_SLAVE_ADDRESS);
}
void DpsClass::begin(TwoWire &bus, uint8_t slaveAddress)
{
//this flag will show if the initialization was successful
m_initFail = 0U;
//Set I2C bus connection
m_SpiI2c = 1U;
m_i2cbus = &bus;
m_slaveAddress = slaveAddress;
// Init bus
m_i2cbus->begin();
delay(50); //startup time of Dps310
init();
}
#ifndef DPS_DISABLESPI
void DpsClass::begin(SPIClass &bus, int32_t chipSelect)
{
begin(bus, chipSelect, 0U);
}
#endif
#ifndef DPS_DISABLESPI
void DpsClass::begin(SPIClass &bus, int32_t chipSelect, uint8_t threeWire)
{
//this flag will show if the initialization was successful
m_initFail = 0U;
//Set SPI bus connection
m_SpiI2c = 0U;
m_spibus = &bus;
m_chipSelect = chipSelect;
// Init bus
m_spibus->begin();
m_spibus->setDataMode(SPI_MODE3);
pinMode(m_chipSelect, OUTPUT);
digitalWrite(m_chipSelect, HIGH);
delay(50); //startup time of Dps310
//switch to 3-wire mode if necessary
//do not use writeByteBitfield or check option to set SPI mode!
//Reading is not possible until SPI-mode is valid
if (threeWire)
{
m_threeWire = 1U;
if (writeByte(DPS310__REG_ADR_SPI3W, DPS310__REG_CONTENT_SPI3W))
{
m_initFail = 1U;
return;
}
}
init();
}
#endif
void DpsClass::end(void)
{
standby();
}
//////// Declaration of other public functions starts here ////////
uint8_t DpsClass::getProductId(void)
{
return m_productID;
}
uint8_t DpsClass::getRevisionId(void)
{
return m_revisionID;
}
int16_t DpsClass::getContResults(float *tempBuffer,
uint8_t &tempCount,
float *prsBuffer,
uint8_t &prsCount, RegMask_t fifo_empty_reg)
{
if (m_initFail)
{
return DPS__FAIL_INIT_FAILED;
}
//abort if device is not in background mode
if (!(m_opMode & 0x04))
{
return DPS__FAIL_TOOBUSY;
}
if (!tempBuffer || !prsBuffer)
{
return DPS__FAIL_UNKNOWN;
}
tempCount = 0U;
prsCount = 0U;
//while FIFO is not empty
while (readByteBitfield(fifo_empty_reg) == 0)
{
int32_t raw_result;
float result;
//read next result from FIFO
int16_t type = getFIFOvalue(&raw_result);
switch (type)
{
case 0: //temperature
if (tempCount < DPS__FIFO_SIZE)
{
result = calcTemp(raw_result);
tempBuffer[tempCount++] = result;
}
break;
case 1: //pressure
if (prsCount < DPS__FIFO_SIZE)
{
result = calcPressure(raw_result);
prsBuffer[prsCount++] = result;
}
break;
case -1: //read failed
break;
}
}
return DPS__SUCCEEDED;
}
int16_t DpsClass::getSingleResult(float &result)
{
//abort if initialization failed
if (m_initFail)
{
return DPS__FAIL_INIT_FAILED;
}
//read finished bit for current opMode
int16_t rdy;
switch (m_opMode)
{
case CMD_TEMP: //temperature
rdy = readByteBitfield(config_registers[TEMP_RDY]);
break;
case CMD_PRS: //pressure
rdy = readByteBitfield(config_registers[PRS_RDY]);
break;
default: //DPS310 not in command mode
return DPS__FAIL_TOOBUSY;
}
//read new measurement result
switch (rdy)
{
case DPS__FAIL_UNKNOWN: //could not read ready flag
return DPS__FAIL_UNKNOWN;
case 0: //ready flag not set, measurement still in progress
return DPS__FAIL_UNFINISHED;
case 1: //measurement ready, expected case
Mode oldMode = m_opMode;
m_opMode = IDLE; //opcode was automatically reseted by DPS310
int32_t raw_val;
switch (oldMode)
{
case CMD_TEMP: //temperature
getRawResult(&raw_val, registerBlocks[TEMP]);
result = calcTemp(raw_val);
return DPS__SUCCEEDED; // TODO
case CMD_PRS: //pressure
getRawResult(&raw_val, registerBlocks[PRS]);
result = calcPressure(raw_val);
return DPS__SUCCEEDED; // TODO
default:
return DPS__FAIL_UNKNOWN; //should already be filtered above
}
}
return DPS__FAIL_UNKNOWN;
}
int16_t DpsClass::measureTempOnce(float &result)
{
return measureTempOnce(result, m_tempOsr);
}
int16_t DpsClass::measureTempOnce(float &result, uint8_t oversamplingRate)
{
//Start measurement
int16_t ret = startMeasureTempOnce(oversamplingRate);
if (ret != DPS__SUCCEEDED)
{
return ret;
}
//wait until measurement is finished
delay(calcBusyTime(0U, m_tempOsr) / DPS__BUSYTIME_SCALING);
delay(DPS310__BUSYTIME_FAILSAFE);
ret = getSingleResult(result);
if (ret != DPS__SUCCEEDED)
{
standby();
}
return ret;
}
int16_t DpsClass::startMeasureTempOnce(void)
{
return startMeasureTempOnce(m_tempOsr);
}
int16_t DpsClass::startMeasureTempOnce(uint8_t oversamplingRate)
{
//abort if initialization failed
if (m_initFail)
{
return DPS__FAIL_INIT_FAILED;
}
//abort if device is not in idling mode
if (m_opMode != IDLE)
{
return DPS__FAIL_TOOBUSY;
}
if (oversamplingRate != m_tempOsr)
{
//configuration of oversampling rate
if (configTemp(0U, oversamplingRate) != DPS__SUCCEEDED)
{
return DPS__FAIL_UNKNOWN;
}
}
//set device to temperature measuring mode
return setOpMode(CMD_TEMP);
}
int16_t DpsClass::measurePressureOnce(float &result)
{
return measurePressureOnce(result, m_prsOsr);
}
int16_t DpsClass::measurePressureOnce(float &result, uint8_t oversamplingRate)
{
//start the measurement
int16_t ret = startMeasurePressureOnce(oversamplingRate);
if (ret != DPS__SUCCEEDED)
{
return ret;
}
//wait until measurement is finished
delay(calcBusyTime(0U, m_prsOsr) / DPS__BUSYTIME_SCALING);
delay(DPS310__BUSYTIME_FAILSAFE);
ret = getSingleResult(result);
if (ret != DPS__SUCCEEDED)
{
standby();
}
return ret;
}
int16_t DpsClass::startMeasurePressureOnce(void)
{
return startMeasurePressureOnce(m_prsOsr);
}
int16_t DpsClass::startMeasurePressureOnce(uint8_t oversamplingRate)
{
//abort if initialization failed
if (m_initFail)
{
return DPS__FAIL_INIT_FAILED;
}
//abort if device is not in idling mode
if (m_opMode != IDLE)
{
return DPS__FAIL_TOOBUSY;
}
//configuration of oversampling rate, lowest measure rate to avoid conflicts
if (oversamplingRate != m_prsOsr)
{
if (configPressure(0U, oversamplingRate))
{
return DPS__FAIL_UNKNOWN;
}
}
//set device to pressure measuring mode
return setOpMode(CMD_PRS);
}
int16_t DpsClass::startMeasureTempCont(uint8_t measureRate, uint8_t oversamplingRate)
{
//abort if initialization failed
if (m_initFail)
{
return DPS__FAIL_INIT_FAILED;
}
//abort if device is not in idling mode
if (m_opMode != IDLE)
{
return DPS__FAIL_TOOBUSY;
}
//abort if speed and precision are too high
if (calcBusyTime(measureRate, oversamplingRate) >= DPS310__MAX_BUSYTIME)
{
return DPS__FAIL_UNFINISHED;
}
//update precision and measuring rate
if (configTemp(measureRate, oversamplingRate))
{
return DPS__FAIL_UNKNOWN;
}
if (enableFIFO())
{
return DPS__FAIL_UNKNOWN;
}
//Start measuring in background mode
if (DpsClass::setOpMode(CONT_TMP))
{
return DPS__FAIL_UNKNOWN;
}
return DPS__SUCCEEDED;
}
int16_t DpsClass::startMeasurePressureCont(uint8_t measureRate, uint8_t oversamplingRate)
{
//abort if initialization failed
if (m_initFail)
{
return DPS__FAIL_INIT_FAILED;
}
//abort if device is not in idling mode
if (m_opMode != IDLE)
{
return DPS__FAIL_TOOBUSY;
}
//abort if speed and precision are too high
if (calcBusyTime(measureRate, oversamplingRate) >= DPS310__MAX_BUSYTIME)
{
return DPS__FAIL_UNFINISHED;
}
//update precision and measuring rate
if (configPressure(measureRate, oversamplingRate))
return DPS__FAIL_UNKNOWN;
//enable result FIFO
if (enableFIFO())
{
return DPS__FAIL_UNKNOWN;
}
//Start measuring in background mode
if (DpsClass::setOpMode(CONT_PRS))
{
return DPS__FAIL_UNKNOWN;
}
return DPS__SUCCEEDED;
}
int16_t DpsClass::startMeasureBothCont(uint8_t tempMr,
uint8_t tempOsr,
uint8_t prsMr,
uint8_t prsOsr)
{
//abort if initialization failed
if (m_initFail)
{
return DPS__FAIL_INIT_FAILED;
}
//abort if device is not in idling mode
if (m_opMode != IDLE)
{
return DPS__FAIL_TOOBUSY;
}
//abort if speed and precision are too high
if (calcBusyTime(tempMr, tempOsr) + calcBusyTime(prsMr, prsOsr) >= DPS310__MAX_BUSYTIME)
{
return DPS__FAIL_UNFINISHED;
}
//update precision and measuring rate
if (configTemp(tempMr, tempOsr))
{
return DPS__FAIL_UNKNOWN;
}
//update precision and measuring rate
if (configPressure(prsMr, prsOsr))
return DPS__FAIL_UNKNOWN;
//enable result FIFO
if (enableFIFO())
{
return DPS__FAIL_UNKNOWN;
}
//Start measuring in background mode
if (setOpMode(CONT_BOTH))
{
return DPS__FAIL_UNKNOWN;
}
return DPS__SUCCEEDED;
}
int16_t DpsClass::standby(void)
{
//abort if initialization failed
if (m_initFail)
{
return DPS__FAIL_INIT_FAILED;
}
//set device to idling mode
int16_t ret = setOpMode(IDLE);
if (ret != DPS__SUCCEEDED)
{
return ret;
}
ret = disableFIFO();
return ret;
}
int16_t DpsClass::correctTemp(void)
{
if (m_initFail)
{
return DPS__FAIL_INIT_FAILED;
}
writeByte(0x0E, 0xA5);
writeByte(0x0F, 0x96);
writeByte(0x62, 0x02);
writeByte(0x0E, 0x00);
writeByte(0x0F, 0x00);
//perform a first temperature measurement (again)
//the most recent temperature will be saved internally
//and used for compensation when calculating pressure
float trash;
measureTempOnce(trash);
return DPS__SUCCEEDED;
}
int16_t DpsClass::getIntStatusFifoFull(void)
{
return readByteBitfield(config_registers[INT_FLAG_FIFO]);
}
int16_t DpsClass::getIntStatusTempReady(void)
{
return readByteBitfield(config_registers[INT_FLAG_TEMP]);
}
int16_t DpsClass::getIntStatusPrsReady(void)
{
return readByteBitfield(config_registers[INT_FLAG_PRS]);
}
//////// Declaration of private functions starts here ////////
int16_t DpsClass::setOpMode(uint8_t opMode)
{
if (writeByteBitfield(opMode, config_registers[MSR_CTRL]) == -1)
{
return DPS__FAIL_UNKNOWN;
}
m_opMode = (Mode)opMode;
return DPS__SUCCEEDED;
}
int16_t DpsClass::configTemp(uint8_t tempMr, uint8_t tempOsr)
{
tempMr &= 0x07;
tempOsr &= 0x07;
// two accesses to the same register; for readability
int16_t ret = writeByteBitfield(tempMr, config_registers[TEMP_MR]);
ret = writeByteBitfield(tempOsr, config_registers[TEMP_OSR]);
//abort immediately on fail
if (ret != DPS__SUCCEEDED)
{
return DPS__FAIL_UNKNOWN;
}
m_tempMr = tempMr;
m_tempOsr = tempOsr;
}
int16_t DpsClass::configPressure(uint8_t prsMr, uint8_t prsOsr)
{
prsMr &= 0x07;
prsOsr &= 0x07;
int16_t ret = writeByteBitfield(prsMr, config_registers[PRS_MR]);
ret = writeByteBitfield(prsOsr, config_registers[PRS_OSR]);
//abort immediately on fail
if (ret != DPS__SUCCEEDED)
{
return DPS__FAIL_UNKNOWN;
}
m_prsMr = prsMr;
m_prsOsr = prsOsr;
}
int16_t DpsClass::enableFIFO()
{
return writeByteBitfield(1U, config_registers[FIFO_EN]);
}
int16_t DpsClass::disableFIFO()
{
int16_t ret = flushFIFO();
ret = writeByteBitfield(0U, config_registers[FIFO_EN]);
return ret;
}
uint16_t DpsClass::calcBusyTime(uint16_t mr, uint16_t osr)
{
//formula from datasheet (optimized)
return ((uint32_t)20U << mr) + ((uint32_t)16U << (osr + mr));
}
int16_t DpsClass::getFIFOvalue(int32_t *value)
{
uint8_t buffer[DPS__RESULT_BLOCK_LENGTH] = {0};
//abort on invalid argument or failed block reading
if (value == NULL || readBlock(registerBlocks[PRS], buffer) != DPS__RESULT_BLOCK_LENGTH)
return DPS__FAIL_UNKNOWN;
*value = (uint32_t)buffer[0] << 16 | (uint32_t)buffer[1] << 8 | (uint32_t)buffer[2];
getTwosComplement(value, 24);
return buffer[2] & 0x01;
}
int16_t DpsClass::readByte(uint8_t regAddress)
{
#ifndef DPS_DISABLESPI
//delegate to specialized function if Dps310 is connected via SPI
if (m_SpiI2c == 0)
{
return readByteSPI(regAddress);
}
#endif
m_i2cbus->beginTransmission(m_slaveAddress);
m_i2cbus->write(regAddress);
m_i2cbus->endTransmission(false);
//request 1 byte from slave
if (m_i2cbus->requestFrom(m_slaveAddress, 1U, 1U) > 0)
{
return m_i2cbus->read(); //return this byte on success
}
else
{
return DPS__FAIL_UNKNOWN; //if 0 bytes were read successfully
}
}
#ifndef DPS_DISABLESPI
int16_t DpsClass::readByteSPI(uint8_t regAddress)
{
//this function is only made for communication via SPI
if (m_SpiI2c != 0)
{
return DPS__FAIL_UNKNOWN;
}
//mask regAddress
regAddress &= ~DPS310__SPI_RW_MASK;
//reserve and initialize bus
m_spibus->beginTransaction(SPISettings(DPS310__SPI_MAX_FREQ,
MSBFIRST,
SPI_MODE3));
//enable ChipSelect for Dps310
digitalWrite(m_chipSelect, LOW);
//send address with read command to Dps310
m_spibus->transfer(regAddress | DPS310__SPI_READ_CMD);
//receive register content from Dps310
uint8_t ret = m_spibus->transfer(0xFF); //send a dummy byte while receiving
//disable ChipSelect for Dps310
digitalWrite(m_chipSelect, HIGH);
//close current SPI transaction
m_spibus->endTransaction();
//return received data
return ret;
}
#endif
#ifndef DPS_DISABLESPI
int16_t DpsClass::readBlockSPI(RegBlock_t regBlock, uint8_t *buffer)
{
//this function is only made for communication via SPI
if (m_SpiI2c != 0)
{
return DPS__FAIL_UNKNOWN;
}
//do not read if there is no buffer
if (buffer == NULL)
{
return 0; //0 bytes were read successfully
}
//mask regAddress
regBlock.regAddress &= ~DPS310__SPI_RW_MASK;
//reserve and initialize bus
m_spibus->beginTransaction(SPISettings(DPS310__SPI_MAX_FREQ,
MSBFIRST,
SPI_MODE3));
//enable ChipSelect for Dps310
digitalWrite(m_chipSelect, LOW);
//send address with read command to Dps310
m_spibus->transfer(regBlock.regAddress | DPS310__SPI_READ_CMD);
//receive register contents from Dps310
for (uint8_t count = 0; count < regBlock.length; count++)
{
buffer[count] = m_spibus->transfer(0xFF); //send a dummy byte while receiving
}
//disable ChipSelect for Dps310
digitalWrite(m_chipSelect, HIGH);
//close current SPI transaction
m_spibus->endTransaction();
//return received data
return regBlock.length;
}
#endif
int16_t DpsClass::writeByte(uint8_t regAddress, uint8_t data)
{
return writeByte(regAddress, data, 0U);
}
int16_t DpsClass::writeByte(uint8_t regAddress, uint8_t data, uint8_t check)
{
#ifndef DPS_DISABLESPI
//delegate to specialized function if Dps310 is connected via SPI
if (m_SpiI2c == 0)
{
return writeByteSpi(regAddress, data, check);
}
#endif
m_i2cbus->beginTransmission(m_slaveAddress);
m_i2cbus->write(regAddress); //Write Register number to buffer
m_i2cbus->write(data); //Write data to buffer
if (m_i2cbus->endTransmission() != 0) //Send buffer content to slave
{
return DPS__FAIL_UNKNOWN;
}
else
{
if (check == 0)
return 0; //no checking
if (readByte(regAddress) == data) //check if desired by calling function
{
return DPS__SUCCEEDED;
}
else
{
return DPS__FAIL_UNKNOWN;
}
}
}
#ifndef DPS_DISABLESPI
int16_t DpsClass::writeByteSpi(uint8_t regAddress, uint8_t data, uint8_t check)
{
//this function is only made for communication via SPI
if (m_SpiI2c != 0)
{
return DPS__FAIL_UNKNOWN;
}
//mask regAddress
regAddress &= ~DPS310__SPI_RW_MASK;
//reserve and initialize bus
m_spibus->beginTransaction(SPISettings(DPS310__SPI_MAX_FREQ,
MSBFIRST,
SPI_MODE3));
//enable ChipSelect for Dps310
digitalWrite(m_chipSelect, LOW);
//send address with read command to Dps310
m_spibus->transfer(regAddress | DPS310__SPI_WRITE_CMD);
//write register content from Dps310
m_spibus->transfer(data);
//disable ChipSelect for Dps310
digitalWrite(m_chipSelect, HIGH);
//close current SPI transaction
m_spibus->endTransaction();
//check if necessary
if (check == 0)
{
//no checking necessary
return DPS__SUCCEEDED;
}
//checking necessary
if (readByte(regAddress) == data)
{
//check passed
return DPS__SUCCEEDED;
}
else
{
//check failed
return DPS__FAIL_UNKNOWN;
}
}
#endif
int16_t DpsClass::writeByteBitfield(uint8_t data, RegMask_t regMask)
{
return writeByteBitfield(data, regMask.regAddress, regMask.mask, regMask.shift, 0U);
}
int16_t DpsClass::writeByteBitfield(uint8_t data,
uint8_t regAddress,
uint8_t mask,
uint8_t shift,
uint8_t check)
{
int16_t old = readByte(regAddress);
if (old < 0)
{
//fail while reading
return old;
}
return writeByte(regAddress, ((uint8_t)old & ~mask) | ((data << shift) & mask), check);
}
int16_t DpsClass::readByteBitfield(RegMask_t regMask)
{
int16_t ret = readByte(regMask.regAddress);
if (ret < 0)
{
return ret;
}
return (((uint8_t)ret) & regMask.mask) >> regMask.shift;
}
int16_t DpsClass::readBlock(RegBlock_t regBlock, uint8_t *buffer)
{
#ifndef DPS_DISABLESPI
//delegate to specialized function if Dps310 is connected via SPI
if (m_SpiI2c == 0)
{
return readBlockSPI(regBlock, buffer);
}
#endif
//do not read if there is no buffer
if (buffer == NULL)
{
return 0; //0 bytes read successfully
}
m_i2cbus->beginTransmission(m_slaveAddress);
m_i2cbus->write(regBlock.regAddress);
m_i2cbus->endTransmission(false);
//request length bytes from slave
int16_t ret = m_i2cbus->requestFrom(m_slaveAddress, regBlock.length, 1U);
//read all received bytes to buffer
for (int16_t count = 0; count < ret; count++)
{
buffer[count] = m_i2cbus->read();
}
return ret;
}
void DpsClass::getTwosComplement(int32_t *raw, uint8_t length)
{
if (*raw & ((uint32_t)1 << (length - 1)))
{
*raw -= (uint32_t)1 << length;
}
}
int16_t DpsClass::getRawResult(int32_t *raw, RegBlock_t reg)
{
uint8_t buffer[DPS__RESULT_BLOCK_LENGTH] = {0};
if (readBlock(reg, buffer) != DPS__RESULT_BLOCK_LENGTH)
return DPS__FAIL_UNKNOWN;
*raw = (uint32_t)buffer[0] << 16 | (uint32_t)buffer[1] << 8 | (uint32_t)buffer[2];
getTwosComplement(raw, 24);
return DPS__SUCCEEDED;
}