BlueRobotics_KellerLD_Library/src/KellerLD.cpp at 6f9639afe3fef3018048df0f4c83a3d47c536c94 · bluerobotics/BlueRobotics_KellerLD_Library · GitHub

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
#include "KellerLD.h"
#include <Wire.h>

#define LD_DEFAULT_ADDR             0x40
#define LD_REQUEST                  0xAC
#define LD_CUST_ID0                 0x00
#define LD_CUST_ID1                 0x01
#define LD_SCALING0                 0x12
#define LD_SCALING1                 0x13
#define LD_SCALING2                 0x14
#define LD_SCALING3                 0x15
#define LD_SCALING4                 0x16


KellerLD::KellerLD(int i2cAddress = LD_DEFAULT_ADDR)
		: i2cAddress(i2cAddress) {
}

void KellerLD::init() {
	// Request memory map information
	cust_id0 = readMemoryMap(LD_CUST_ID0);
	cust_id1 = readMemoryMap(LD_CUST_ID1);

	code = (uint32_t(cust_id1) << 16) | cust_id0;
	equipment = cust_id0 >> 10;
	place = cust_id0 & 0b000000111111111;
	file = cust_id1;

	uint16_t scaling0;
	scaling0 = readMemoryMap(LD_SCALING0);

	mode = scaling0 & 0b00000011;
	calibrationYear = scaling0 >> 11;
	calibrationMonth = (scaling0 & 0b0000011110000000) >> 7;
	calibrationDay = (scaling0 & 0b0000000001111100) >> 2;

	// handle P-mode pressure offset (to vacuum pressure)

	if (mode == 0) {
		// PR mode, Vented Gauge. Zero when front pressure == rear pressure
		// TODO: allow updating to variable local atmosphere/enclosure pressure
		//       from an air pressure sensor
		P_mode = 1.01325;
	} else if (mode == 1) {
		// PA mode, Sealed Gauge. Zero at 1.0 bar
		P_mode = 1.0;
	} else {
		// PAA mode, Absolute. Zero at vacuum
		// (or undefined mode)
		P_mode = 0;
	}

	uint32_t scaling12 = (uint32_t(readMemoryMap(LD_SCALING1)) << 16) | readMemoryMap(LD_SCALING2);

	P_min = *reinterpret_cast<float *>(&scaling12);

	uint32_t scaling34 = (uint32_t(readMemoryMap(LD_SCALING3)) << 16) | readMemoryMap(LD_SCALING4);

	P_max = *reinterpret_cast<float *>(&scaling34);
}

void KellerLD::setFluidDensity(float density) {
	fluidDensity = density;
}

void KellerLD::read() {
	uint8_t status;

	Wire.beginTransmission(i2cAddress);
	Wire.write(LD_REQUEST);
	Wire.endTransmission();

	delay(9); // Max conversion time per datasheet

	Wire.requestFrom(i2cAddress, 5);
	status = Wire.read();
	P = (Wire.read() << 8) | Wire.read();
	uint16_t T = (Wire.read() << 8) | Wire.read();

	P_bar = (float(P) - 16384) * (P_max - P_min) / 32768 + P_min + P_mode;
	T_degc = ((T >> 4) - 24) * 0.05 - 50;
}

uint16_t KellerLD::readMemoryMap(uint8_t mtp_address) {
	uint8_t status;

	Wire.beginTransmission(i2cAddress);
	Wire.write(mtp_address);
	Wire.endTransmission();

	delay(1); // allow for response to come in

	Wire.requestFrom(i2cAddress, 3);
	status = Wire.read();
	return ((Wire.read() << 8) | Wire.read());
}

bool KellerLD::status() {
	if (equipment <= 62) {
		return true;
	} else {
		return false;
	}
}

float KellerLD::range() {
	return P_max - P_min;
}

float KellerLD::pressure(float conversion) {
	return P_bar * 1000.0f * conversion;
}

float KellerLD::temperature() {
	return T_degc;
}

float KellerLD::depth() {
	return (pressure(KellerLD::Pa) - 101325) / (fluidDensity * 9.80665);
}

float KellerLD::altitude() {
	return (1 - pow((pressure() / 1013.25), 0.190284)) * 145366.45 * .3048;
}

bool KellerLD::isInitialized() {
	return (cust_id0 >> 10) != 63; // If not connected, equipment code == 63
}