That makes a lot of sense. A second reference point would definitely help. Since our project moves through the atmosphere and we
have a way to send the stationary data to the device in motion, we will work on adding that feature. In addition, another teacher has provided some methods of mediating values locally to slow the rate of change. Still just averaging but at least not as fast as the barometer jumps around...see below and we look forward to your feedback on other code improvements we could make:
Code: Select all
#include <SPI.h>
#include <Wire.h>
#include <nRF24L01.h>
#include <RF24.h>
#include "Adafruit_DPS310b.h" //Adafruit_DPS310.cpp mods
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#define LED_PIN 2
boolean ledOn = false;
int ledCounter = 0;
#define CE_PIN 9
#define CSN_PIN 10
const uint64_t pipeOut = 0xE8E8F0F0E1LL; // address through which two modules communicate
RF24 radio(CE_PIN, CSN_PIN); // Create an RF24 object using CE, CSN pins
int packetCounts[10];
int packetCountIndex = 0;
int packetCountTotal = 0;
char ppsBuf[16];
char dropsBuf[16];
char pressBuf[16];
char tempBuf[16];
char altBuf[16];
char lonBuf[16];
char latBuf[16];
unsigned long packetsSent = 0;
unsigned long lastSendTime = 0;
unsigned long drops = 0;
struct PacketData {
long lon;
long lat;
int32_t pktpressure;
int32_t pkttemperature;
int32_t pktaltitude;
long velN;
long velE;
};
PacketData data;
void resetData() {
data.lon = 0;
data.lat = 0;
data.pktpressure = 0;
data.pkttemperature = 0;
data.pktaltitude = 0;
}
#define GPSSERIAL Serial
const unsigned char UBLOX_INIT[] PROGMEM = {
0xB5,0x62,0x06,0x16,0x08,0x00,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x25,0x90, // Disable SBAS
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x00,0x24, // GxGGA off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x01,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x2B, // GxGLL off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x02,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x32, // GxGSA off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x03,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x39, // GxGSV off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x04,0x00,0x00,0x00,0x00,0x00,0x01,0x04,0x40, // GxRMC off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x05,0x00,0x00,0x00,0x00,0x00,0x01,0x05,0x47, // GxVTG off
0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x17,0xDC, //NAV-PVT off
0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x12,0xB9, //NAV-POSLLH off
0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x13,0xC0, //NAV-STATUS off
0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x12,0x00,0x00,0x00,0x00,0x00,0x00,0x22,0x29, //NAV-VELNED off
0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x02,0x00,0x01,0x00,0x00,0x00,0x00,0x13,0xBE, //NAV-POSLLH on
0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x12,0x00,0x01,0x00,0x00,0x00,0x00,0x23,0x2E, //NAV-VELNED on
0xB5,0x62,0x06,0x08,0x06,0x00,0x64,0x00,0x01,0x00,0x01,0x00,0x7A,0x12, //(10Hz)
};
const unsigned char UBX_HEADER[] = { 0xB5, 0x62 };
const unsigned char NAV_POSLLH_HEADER[] = { 0x01, 0x02 };
const unsigned char NAV_VELNED_HEADER[] = { 0x01, 0x12 };
enum _ubxMsgType {
MT_NONE,
MT_NAV_POSLLH,
MT_NAV_VELNED
};
struct NAV_POSLLH {
unsigned char cls; // 0x01
unsigned char id; // 0x02
unsigned short len; // 28
unsigned long iTOW; // millisecond time of week
long lon; // longitude
long lat; // latitude
long height; // height above ellipsoid
long hMSL; // height above mean sea level
unsigned long hAcc; // horizontal accuracy
unsigned long vAcc; // vertical accuracy
};
struct NAV_VELNED {
unsigned char cls; // 0x01
unsigned char id; // 0x12
unsigned short len; // 36
unsigned long iTOW; // millisecond time of week
long velN; // north velocity (cm/s)
long velE; // east velocity (cm/s)
long velD; // down velocity (cm/s)
unsigned long speed; // 3d (cm/s)
unsigned long gSpeed; // ground speed (2d)
long heading; // heading of motion (2d)
unsigned long sAcc; // speed accuracy
unsigned long cAcc; // course/heading accuracy
};
union UBXMessage {
NAV_POSLLH navPosllh;
NAV_VELNED navVelned;
};
UBXMessage ubxMessage;
void calcChecksum(unsigned char* CK, int msgSize) {
memset(CK, 0, 2);
for (int i = 0; i < msgSize; i++) {
CK[0] += ((unsigned char*)(&ubxMessage))[i];
CK[1] += CK[0];
}
}
boolean compareMsgHeader(const unsigned char* msgHeader) {
unsigned char* ptr = (unsigned char*)(&ubxMessage);
return ptr[0] == msgHeader[0] && ptr[1] == msgHeader[1];
}
int processGPS() {
static int fpos = 0;
static unsigned char checksum[2];
static byte currentMsgType = MT_NONE;
static int payloadSize = sizeof(UBXMessage);
while ( GPSSERIAL.available() ) {
byte c = GPSSERIAL.read();
if ( fpos < 2 ) {
if ( c == UBX_HEADER[fpos] )
fpos++;
else
fpos = 0; // Reset to beginning state.
}
else {
if ( (fpos-2) < payloadSize )
((unsigned char*)(&ubxMessage))[fpos-2] = c;
fpos++;
if ( fpos == 4 ) {
if ( compareMsgHeader(NAV_POSLLH_HEADER) ) {
currentMsgType = MT_NAV_POSLLH;
payloadSize = sizeof(NAV_POSLLH);
}
else if ( compareMsgHeader(NAV_VELNED_HEADER) ) {
currentMsgType = MT_NAV_VELNED;
payloadSize = sizeof(NAV_VELNED);
}
else {
// unknown message type, bail
fpos = 0;
continue;
}
}
if ( fpos == (payloadSize+2) ) {
calcChecksum(checksum, payloadSize);
}
else if ( fpos == (payloadSize+3) ) {
if ( c != checksum[0] ) {
fpos = 0;
}
}
else if ( fpos == (payloadSize+4) ) {
fpos = 0; // We will reset the state regardless of whether the checksum matches.
if ( c == checksum[1] ) {
return currentMsgType;
}
}
else if ( fpos > (payloadSize+4) ) {
fpos = 0;
}
}
}
return MT_NONE;
}
/*Baro Variables Begin*/
#define SENSOR_NEEDS_SAMPLES 0
#define SENSOR_NEEDS_CALCULATION 1
#define SENSOR_NEEDS_PROCESSING 2
#define SAMPLE_COUNT_MAX 48
#define SENSOR_SAMPLE_COUNT 21
#define SAMPLE_COUNT 20
#define SAMPLES_MEDIAN 3
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
int32_t baroAlt = 0;
static uint32_t baroGroundAltitude = 0;
static uint32_t baroGroundPressure = 0;
// static uint32_t baroGroundPressure = 1011.50; // hPa, get from local air port readings
int32_t readingSum = 0;
int32_t alt_start_reading = 0; // altitude at barometer startup (in cm)
int32_t alt_delta_reading = 0; // alt_delta_reading = alt_reading - alt_start_reading (in cm)
int32_t alt_reading = 0; // current barameter alt reading (in cm)
int32_t pktpressure = 0; // value passed from tag to controller
int32_t pkttemperature = 0; // value passed from tag to controller
int32_t pktaltitude = 0; // value passed from tag to controller
int count = 0;
boolean doneCalibration = false;
Adafruit_DPS310 dps;
Adafruit_Sensor *dps_temp = dps.getTemperatureSensor(); // create dps temperature object
Adafruit_Sensor *dps_pressure = dps.getPressureSensor(); // create dps pressure object
#define QMF_SORT(a,b) { if ((a)>(b)) QMF_SWAP((a),(b)); }
#define QMF_SWAP(a,b) { int32_t temp=(a);(a)=(b);(b)=temp; }
#define QMP_COPY(p,v,n) { int32_t i; for (i=0; i<n; i++) p[i]=v[i]; }
int32_t quickMedianFilter3(int32_t * v) {
int32_t p[3];
QMP_COPY(p, v, 3);
QMF_SORT(p[0], p[1]); QMF_SORT(p[1], p[2]); QMF_SORT(p[0], p[1]) ;
return p[1];
}
static int32_t applyMedianFilter(int32_t newReading) {
static int32_t FilterSamples[SAMPLES_MEDIAN];
static int currentFilterSampleIndex = 0;
static bool medianFilterReady = false;
int nextSampleIndex;
nextSampleIndex = (currentFilterSampleIndex + 1);
if (nextSampleIndex == SAMPLES_MEDIAN) {
nextSampleIndex = 0;
medianFilterReady = true;
}
FilterSamples[currentFilterSampleIndex] = newReading;
currentFilterSampleIndex = nextSampleIndex;
if (medianFilterReady)
return quickMedianFilter3(FilterSamples);
else
return newReading;
}
static uint32_t recalculateTotal(uint8_t SampleCount, uint32_t readingTotal, int32_t newReading) {
static int32_t readingSamples[SAMPLE_COUNT_MAX];
static int currentSampleIndex = 0;
int nextSampleIndex;
nextSampleIndex = (currentSampleIndex + 1);
if (nextSampleIndex == SampleCount) {
nextSampleIndex = 0;
}
readingSamples[currentSampleIndex] = applyMedianFilter(newReading);
readingTotal += readingSamples[currentSampleIndex];
readingTotal -= readingSamples[nextSampleIndex];
currentSampleIndex = nextSampleIndex;
return readingTotal;
}
void baroUpdate(uint32_t currentTime) {
static uint32_t baroDeadline = 0;
static int state = SENSOR_NEEDS_SAMPLES;
if ((int32_t)(currentTime - baroDeadline) < 0)
return;
baroDeadline = 0;
switch (state) {
case SENSOR_NEEDS_SAMPLES:
sensors_event_t temp_event, pressure_event;
while (!dps.temperatureAvailable() || !dps.pressureAvailable()) {
return; // wait until there's something to read
}
dps.getEvents(&temp_event, &pressure_event);
baroTemperature = temp_event.temperature;
baroPressure = pressure_event.pressure;
alt_reading = (dps.readAltitude() * 100 ); // in cm
alt_delta_reading = alt_reading - alt_start_reading; // in cm
state = SENSOR_NEEDS_CALCULATION;
baroDeadline += 10000;
break;
case SENSOR_NEEDS_CALCULATION:
baroDeadline += 10000;
state = SENSOR_NEEDS_PROCESSING;
break;
case SENSOR_NEEDS_PROCESSING:
state = SENSOR_NEEDS_SAMPLES;
readingSum = recalculateTotal(SENSOR_SAMPLE_COUNT, readingSum, alt_reading);
break;
}
baroDeadline += micros(); // make sure deadline is set after calling baro callbacks
}
void setup() {
pinMode(LED_PIN, OUTPUT);
pinMode(CSN_PIN, OUTPUT); // Arduino Uno R3 (Atmega 328 datasheet) pin 10 must set as OUTPUT to make the chip work as an SPI master
GPSSERIAL.begin(9600);
for(int i = 0; i < (int)sizeof(UBLOX_INIT); i++) {
GPSSERIAL.write( pgm_read_byte(UBLOX_INIT+i) );
delay(5); // simulating a 38400baud pace (or less), otherwise commands are not accepted by the device.
}
resetData();
if (! dps.begin_I2C(DPS310_I2CADDR_DEFAULT)) { // Can pass in I2C address here
while (1) yield();
}
dps.configurePressure(DPS310_64HZ, DPS310_64SAMPLES);
dps.configureTemperature(DPS310_64HZ, DPS310_64SAMPLES);
dps.setMode(DPS310_CONT_PRESTEMP);
sensors_event_t temp_event, pressure_event;
while (!dps.temperatureAvailable() || !dps.pressureAvailable()) {
return; // wait until there's something to read
}
dps.getEvents(&temp_event, &pressure_event);
alt_start_reading = ( dps.readAltitude() * 100 ); // in cm
alt_reading = alt_start_reading;
radio.begin();
radio.setDataRate(RF24_250KBPS);
radio.setAutoAck(false);
radio.openWritingPipe(pipeOut);
radio.stopListening(); // set module as transmitter
delay(5000); // wait a few seconds to close the device
}
void toggleLed() {
ledOn = ! ledOn;
digitalWrite(LED_PIN, ledOn);
}
void sendData() {
radio.write(&data, sizeof(PacketData));
packetsSent++;
if ( ledCounter++ > 5 ) {
toggleLed();
ledCounter = 0;
}
lastSendTime = millis();
unsigned long now = millis();
if ( now - lastSendTime > 1000 )
drops = 1;
else
drops = 0;
}
void loop() {
unsigned long now = micros();
baroUpdate(now);
if ( !doneCalibration ) {
if ( count++ > 200 ) {
for (int i = 0; i < 200; i++) {
//performBaroCalibrationCycle();
//delay(10);
}
doneCalibration = true;
}
}
else {
// Get altitude
alt_delta_reading = alt_reading - alt_start_reading; // in cm
data.pktpressure = baroPressure; // this value gets sent over the radio (hPa)
data.pkttemperature = baroTemperature; // this value gets sent over the radio (*C)
data.pktaltitude = alt_delta_reading; // this value gets sent over the radio (cm)
}
sendData();
int msgType = processGPS();
if ( msgType == MT_NAV_POSLLH ) {
Serial.print(posllh.lon/10000000.0f);
data.lon = ubxMessage.navPosllh.lon;
data.lat = ubxMessage.navPosllh.lat;
}
else if ( msgType == MT_NAV_VELNED ) {
data.velN = ubxMessage.navVelned.velN;
data.velE = ubxMessage.navVelned.velE;
}
delay(20);
// Start sensor time = 10:00 PM CST
// PPS = 67
// PRS = 1026
// TMP = 29
// Alt = between -1 and 5 (try to beat -5000) in cm
// new code Alt = between 15 and -150 (much better) in cm and matches manual calculations
}