Skip to content

Commit b8ece60

Browse files
author
brody.zhang
committed
NEW: add swarm combo example
1 parent f6516ee commit b8ece60

File tree

17 files changed

+3702
-0
lines changed

17 files changed

+3702
-0
lines changed

‎examples/turorial/io_ctrl/.gitignore‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
build/*
2+
core_cache/*
3+
.vscode/*

‎examples/turorial/io_ctrl/MS5837.cpp‎

Lines changed: 258 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,258 @@
1+
#include "MS5837.h"
2+
#include <Wire.h>
3+
4+
const uint8_t MS5837_ADDR = 0x76;
5+
const uint8_t MS5837_RESET = 0x1E;
6+
const uint8_t MS5837_ADC_READ = 0x00;
7+
const uint8_t MS5837_PROM_READ = 0xA0;
8+
const uint8_t MS5837_CONVERT_D1_8192 = 0x4A;
9+
const uint8_t MS5837_CONVERT_D2_8192 = 0x5A;
10+
11+
const float MS5837::Pa = 100.0f;
12+
const float MS5837::bar = 0.001f;
13+
const float MS5837::mbar = 1.0f;
14+
15+
const uint8_t MS5837::MS5837_30BA = 0;
16+
const uint8_t MS5837::MS5837_02BA = 1;
17+
const uint8_t MS5837::MS5837_UNRECOGNISED = 255;
18+
19+
const uint8_t MS5837_02BA01 = 0x00; // Sensor version: From MS5837_02BA datasheet Version PROM Word 0
20+
const uint8_t MS5837_02BA21 = 0x15; // Sensor version: From MS5837_02BA datasheet Version PROM Word 0
21+
const uint8_t MS5837_30BA26 = 0x1A; // Sensor version: From MS5837_30BA datasheet Version PROM Word 0
22+
23+
MS5837::MS5837() {
24+
fluidDensity = 1029;
25+
}
26+
27+
bool MS5837::begin(TwoWire &wirePort) {
28+
return (init(wirePort));
29+
}
30+
31+
bool MS5837::init(TwoWire &wirePort) {
32+
_i2cPort = &wirePort; //Grab which port the user wants us to use
33+
34+
// Reset the MS5837, per datasheet
35+
_i2cPort->beginTransmission(MS5837_ADDR);
36+
_i2cPort->write(MS5837_RESET);
37+
_i2cPort->endTransmission();
38+
39+
// Wait for reset to complete
40+
delay(10);
41+
42+
// Read calibration values and CRC
43+
for ( uint8_t i = 0 ; i < 7 ; i++ ) {
44+
_i2cPort->beginTransmission(MS5837_ADDR);
45+
_i2cPort->write(MS5837_PROM_READ+i*2);
46+
_i2cPort->endTransmission();
47+
48+
_i2cPort->requestFrom(MS5837_ADDR,2);
49+
C[i] = (_i2cPort->read() << 8) | _i2cPort->read();
50+
}
51+
52+
// Verify that data is correct with CRC
53+
uint8_t crcRead = C[0] >> 12;
54+
uint8_t crcCalculated = crc4(C);
55+
56+
if ( crcCalculated != crcRead ) {
57+
return false; // CRC fail
58+
}
59+
60+
uint8_t version = (C[0] >> 5) & 0x7F; // Extract the sensor version from PROM Word 0
61+
62+
// Set _model according to the sensor version
63+
if (version == MS5837_02BA01)
64+
{
65+
_model = MS5837_02BA;
66+
}
67+
else if (version == MS5837_02BA21)
68+
{
69+
_model = MS5837_02BA;
70+
}
71+
else if (version == MS5837_30BA26)
72+
{
73+
_model = MS5837_30BA;
74+
}
75+
else
76+
{
77+
_model = MS5837_UNRECOGNISED;
78+
}
79+
// The sensor has passed the CRC check, so we should return true even if
80+
// the sensor version is unrecognised.
81+
// (The MS5637 has the same address as the MS5837 and will also pass the CRC check)
82+
// (but will hopefully be unrecognised.)
83+
return true;
84+
}
85+
86+
void MS5837::setModel(uint8_t model) {
87+
_model = model;
88+
}
89+
90+
uint8_t MS5837::getModel() {
91+
return (_model);
92+
}
93+
94+
void MS5837::setFluidDensity(float density) {
95+
fluidDensity = density;
96+
}
97+
98+
void MS5837::read() {
99+
//Check that _i2cPort is not NULL (i.e. has the user forgoten to call .init or .begin?)
100+
if (_i2cPort == NULL)
101+
{
102+
return;
103+
}
104+
105+
// Request D1 conversion
106+
_i2cPort->beginTransmission(MS5837_ADDR);
107+
_i2cPort->write(MS5837_CONVERT_D1_8192);
108+
_i2cPort->endTransmission();
109+
110+
delay(20); // Max conversion time per datasheet
111+
112+
_i2cPort->beginTransmission(MS5837_ADDR);
113+
_i2cPort->write(MS5837_ADC_READ);
114+
_i2cPort->endTransmission();
115+
116+
_i2cPort->requestFrom(MS5837_ADDR,3);
117+
D1_pres = 0;
118+
D1_pres = _i2cPort->read();
119+
D1_pres = (D1_pres << 8) | _i2cPort->read();
120+
D1_pres = (D1_pres << 8) | _i2cPort->read();
121+
122+
// Request D2 conversion
123+
_i2cPort->beginTransmission(MS5837_ADDR);
124+
_i2cPort->write(MS5837_CONVERT_D2_8192);
125+
_i2cPort->endTransmission();
126+
127+
delay(20); // Max conversion time per datasheet
128+
129+
_i2cPort->beginTransmission(MS5837_ADDR);
130+
_i2cPort->write(MS5837_ADC_READ);
131+
_i2cPort->endTransmission();
132+
133+
_i2cPort->requestFrom(MS5837_ADDR,3);
134+
D2_temp = 0;
135+
D2_temp = _i2cPort->read();
136+
D2_temp = (D2_temp << 8) | _i2cPort->read();
137+
D2_temp = (D2_temp << 8) | _i2cPort->read();
138+
139+
calculate();
140+
}
141+
142+
void MS5837::calculate() {
143+
// Given C1-C6 and D1, D2, calculated TEMP and P
144+
// Do conversion first and then second order temp compensation
145+
146+
int32_t dT = 0;
147+
int64_t SENS = 0;
148+
int64_t OFF = 0;
149+
int32_t SENSi = 0;
150+
int32_t OFFi = 0;
151+
int32_t Ti = 0;
152+
int64_t OFF2 = 0;
153+
int64_t SENS2 = 0;
154+
155+
// Terms called
156+
dT = D2_temp-uint32_t(C[5])*256l;
157+
if ( _model == MS5837_02BA ) {
158+
SENS = int64_t(C[1])*65536l+(int64_t(C[3])*dT)/128l;
159+
OFF = int64_t(C[2])*131072l+(int64_t(C[4])*dT)/64l;
160+
P = (D1_pres*SENS/(2097152l)-OFF)/(32768l);
161+
} else {
162+
SENS = int64_t(C[1])*32768l+(int64_t(C[3])*dT)/256l;
163+
OFF = int64_t(C[2])*65536l+(int64_t(C[4])*dT)/128l;
164+
P = (D1_pres*SENS/(2097152l)-OFF)/(8192l);
165+
}
166+
167+
// Temp conversion
168+
TEMP = 2000l+int64_t(dT)*C[6]/8388608LL;
169+
170+
//Second order compensation
171+
if ( _model == MS5837_02BA ) {
172+
if((TEMP/100)<20){ //Low temp
173+
Ti = (11*int64_t(dT)*int64_t(dT))/(34359738368LL);
174+
OFFi = (31*(TEMP-2000)*(TEMP-2000))/8;
175+
SENSi = (63*(TEMP-2000)*(TEMP-2000))/32;
176+
}
177+
} else {
178+
if((TEMP/100)<20){ //Low temp
179+
Ti = (3*int64_t(dT)*int64_t(dT))/(8589934592LL);
180+
OFFi = (3*(TEMP-2000)*(TEMP-2000))/2;
181+
SENSi = (5*(TEMP-2000)*(TEMP-2000))/8;
182+
if((TEMP/100)<-15){ //Very low temp
183+
OFFi = OFFi+7*(TEMP+1500l)*(TEMP+1500l);
184+
SENSi = SENSi+4*(TEMP+1500l)*(TEMP+1500l);
185+
}
186+
}
187+
else if((TEMP/100)>=20){ //High temp
188+
Ti = 2*(dT*dT)/(137438953472LL);
189+
OFFi = (1*(TEMP-2000)*(TEMP-2000))/16;
190+
SENSi = 0;
191+
}
192+
}
193+
194+
OFF2 = OFF-OFFi; //Calculate pressure and temp second order
195+
SENS2 = SENS-SENSi;
196+
197+
TEMP = (TEMP-Ti);
198+
199+
if ( _model == MS5837_02BA ) {
200+
P = (((D1_pres*SENS2)/2097152l-OFF2)/32768l);
201+
} else {
202+
P = (((D1_pres*SENS2)/2097152l-OFF2)/8192l);
203+
}
204+
}
205+
206+
float MS5837::pressure(float conversion) {
207+
if ( _model == MS5837_02BA ) {
208+
return P*conversion/100.0f;
209+
}
210+
else {
211+
return P*conversion/10.0f;
212+
}
213+
}
214+
215+
float MS5837::temperature() {
216+
return TEMP/100.0f;
217+
}
218+
219+
// The pressure sensor measures absolute pressure, so it will measure the atmospheric pressure + water pressure
220+
// We subtract the atmospheric pressure to calculate the depth with only the water pressure
221+
// The average atmospheric pressure of 101300 pascal is used for the calcuation, but atmospheric pressure varies
222+
// If the atmospheric pressure is not 101300 at the time of reading, the depth reported will be offset
223+
// In order to calculate the correct depth, the actual atmospheric pressure should be measured once in air, and
224+
// that value should subtracted for subsequent depth calculations.
225+
float MS5837::depth() {
226+
return (pressure(MS5837::Pa)-101300)/(fluidDensity*9.80665);
227+
}
228+
229+
float MS5837::altitude() {
230+
return (1-pow((pressure()/1013.25),.190284))*145366.45*.3048;
231+
}
232+
233+
234+
uint8_t MS5837::crc4(uint16_t n_prom[]) {
235+
uint16_t n_rem = 0;
236+
237+
n_prom[0] = ((n_prom[0]) & 0x0FFF);
238+
n_prom[7] = 0;
239+
240+
for ( uint8_t i = 0 ; i < 16; i++ ) {
241+
if ( i%2 == 1 ) {
242+
n_rem ^= (uint16_t)((n_prom[i>>1]) & 0x00FF);
243+
} else {
244+
n_rem ^= (uint16_t)(n_prom[i>>1] >> 8);
245+
}
246+
for ( uint8_t n_bit = 8 ; n_bit > 0 ; n_bit-- ) {
247+
if ( n_rem & 0x8000 ) {
248+
n_rem = (n_rem << 1) ^ 0x3000;
249+
} else {
250+
n_rem = (n_rem << 1);
251+
}
252+
}
253+
}
254+
255+
n_rem = ((n_rem >> 12) & 0x000F);
256+
257+
return n_rem ^ 0x00;
258+
}

‎examples/turorial/io_ctrl/MS5837.h‎

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
/* Blue Robotics Arduino MS5837-30BA Pressure/Temperature Sensor Library
2+
------------------------------------------------------------
3+
4+
Title: Blue Robotics Arduino MS5837-30BA Pressure/Temperature Sensor Library
5+
6+
Description: This library provides utilities to communicate with and to
7+
read data from the Measurement Specialties MS5837-30BA pressure/temperature
8+
sensor.
9+
10+
Authors: Rustom Jehangir, Blue Robotics Inc.
11+
Adam Šimko, Blue Robotics Inc.
12+
13+
-------------------------------
14+
The MIT License (MIT)
15+
16+
Copyright (c) 2015 Blue Robotics Inc.
17+
18+
Permission is hereby granted, free of charge, to any person obtaining a copy
19+
of this software and associated documentation files (the "Software"), to deal
20+
in the Software without restriction, including without limitation the rights
21+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
22+
copies of the Software, and to permit persons to whom the Software is
23+
furnished to do so, subject to the following conditions:
24+
25+
The above copyright notice and this permission notice shall be included in
26+
all copies or substantial portions of the Software.
27+
28+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
29+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
30+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
31+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
32+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
33+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
34+
THE SOFTWARE.
35+
-------------------------------*/
36+
37+
#ifndef MS5837_H_BLUEROBOTICS
38+
#define MS5837_H_BLUEROBOTICS
39+
40+
#include "Arduino.h"
41+
#include <Wire.h>
42+
43+
class MS5837 {
44+
public:
45+
static const float Pa;
46+
static const float bar;
47+
static const float mbar;
48+
49+
static const uint8_t MS5837_30BA;
50+
static const uint8_t MS5837_02BA;
51+
static const uint8_t MS5837_UNRECOGNISED;
52+
53+
MS5837();
54+
55+
bool init(TwoWire &wirePort = Wire);
56+
bool begin(TwoWire &wirePort = Wire); // Calls init()
57+
58+
/** Set model of MS5837 sensor. Valid options are MS5837::MS5837_30BA (default)
59+
* and MS5837::MS5837_02BA.
60+
*/
61+
void setModel(uint8_t model);
62+
uint8_t getModel();
63+
64+
/** Provide the density of the working fluid in kg/m^3. Default is for
65+
* seawater. Should be 997 for freshwater.
66+
*/
67+
void setFluidDensity(float density);
68+
69+
/** The read from I2C takes up to 40 ms, so use sparingly is possible.
70+
*/
71+
void read();
72+
73+
/** Pressure returned in mbar or mbar*conversion rate.
74+
*/
75+
float pressure(float conversion = 1.0f);
76+
77+
/** Temperature returned in deg C.
78+
*/
79+
float temperature();
80+
81+
/** Depth returned in meters (valid for operation in incompressible
82+
* liquids only. Uses density that is set for fresh or seawater.
83+
*/
84+
float depth();
85+
86+
/** Altitude returned in meters (valid for operation in air only).
87+
*/
88+
float altitude();
89+
90+
private:
91+
92+
//This stores the requested i2c port
93+
TwoWire * _i2cPort;
94+
95+
uint16_t C[8];
96+
uint32_t D1_pres, D2_temp;
97+
int32_t TEMP;
98+
int32_t P;
99+
uint8_t _model;
100+
101+
float fluidDensity;
102+
103+
/** Performs calculations per the sensor data sheet for conversion and
104+
* second order compensation.
105+
*/
106+
void calculate();
107+
108+
uint8_t crc4(uint16_t n_prom[]);
109+
};
110+
111+
#endif

0 commit comments

Comments
 (0)