Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
// INA219 Current Sensor with OLED Display for Arduino Uno
//
// This sketch was modified from the Adafruit INA219 library example
//
// Gadget Reboot
//
// Required Libraries
// https://github.com/adafruit/Adafruit_INA219
// https://github.com/adafruit/Adafruit_SSD1306
#include <Wire.h>
#include <Adafruit_INA219.h>
#include <Adafruit_SSD1306.h>
Adafruit_INA219 ina219;
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);
//include <SoftwareSerial.h>
//SoftwareSerial mySerial(3, 4); // RX, TX
#include <NeoSWSerial.h>
NeoSWSerial mySerial(3, 4);
float busvoltage = 0;
float current_mA = 0;
double lon = 0;
double lat = 0;
unsigned char tdFix = 0;
const unsigned char UBX_HEADER[] = { 0xB5, 0x62 };
const unsigned char NAV_POSLLH_HEADER[] = { 0x01, 0x02 };
const unsigned char NAV_STATUS_HEADER[] = { 0x01, 0x03 };
enum _ubxMsgType {
MT_NONE,
MT_NAV_POSLLH,
MT_NAV_STATUS
};
struct NAV_POSLLH {
unsigned char cls;
unsigned char id;
unsigned short len;
unsigned long iTOW;
long lon;
long lat;
long height;
long hMSL;
unsigned long hAcc;
unsigned long vAcc;
};
struct NAV_STATUS {
unsigned char cls;
unsigned char id;
unsigned short len;
unsigned long iTOW;
unsigned char gpsFix;
char flags;
char fixStat;
char flags2;
unsigned long ttff;
unsigned long msss;
};
union UBXMessage {
NAV_POSLLH navPosllh;
NAV_STATUS navStatus;
};
UBXMessage ubxMessage;
uint8_t config1[] = {
// Disable NMEA
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
// Enable UBX
0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x07,0x00,0x01,0x00,0x00,0x00,0x00,0x18,0xE1, //NAV-PVT on
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,0x03,0x00,0x01,0x00,0x00,0x00,0x00,0x14,0xC5, //NAV-STATUS on
0xB5,0x62,0x06,0x08,0x06,0x00,0xE8,0x03,0x01,0x00,0x01,0x00,0x01,0x39, //(1Hz)
};
byte gps_set_sucess = 0 ;
// The last two bytes of the message is a checksum value, used to confirm that the received payload is valid.
// The procedure used to calculate this is given as pseudo-code in the uBlox manual.
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];
}
}
// Compares the first two bytes of the ubxMessage struct with a specific message header.
// Returns true if the two bytes match.
boolean compareMsgHeader(const unsigned char* msgHeader) {
unsigned char* ptr = (unsigned char*)(&ubxMessage);
return ptr[0] == msgHeader[0] && ptr[1] == msgHeader[1];
}
// Reads in bytes from the GPS module and checks to see if a valid message has been constructed.
// Returns the type of the message found if successful, or MT_NONE if no message was found.
// After a successful return the contents of the ubxMessage union will be valid, for the
// message type that was found. Note that further calls to this function can invalidate the
// message content, so you must use the obtained values before calling this function again.
int processGPS() {
static int fpos = 0;
static unsigned char checksum[2];
static byte currentMsgType = MT_NONE;
static int payloadSize = sizeof(UBXMessage);
while ( mySerial.available() ) {
byte c = mySerial.read();
if ( fpos < 2 ) {
// For the first two bytes we are simply looking for a match with the UBX header bytes (0xB5,0x62)
if ( c == UBX_HEADER[fpos] )
fpos++;
else
fpos = 0; // Reset to beginning state.
}
else {
// If we come here then fpos >= 2, which means we have found a match with the UBX_HEADER
// and we are now reading in the bytes that make up the payload.
// Place the incoming byte into the ubxMessage struct. The position is fpos-2 because
// the struct does not include the initial two-byte header (UBX_HEADER).
if ( (fpos-2) < payloadSize )
((unsigned char*)(&ubxMessage))[fpos-2] = c;
fpos++;
if ( fpos == 4 ) {
// We have just received the second byte of the message type header,
// so now we can check to see what kind of message it is.
if ( compareMsgHeader(NAV_POSLLH_HEADER) ) {
currentMsgType = MT_NAV_POSLLH;
payloadSize = sizeof(NAV_POSLLH);
}
else if ( compareMsgHeader(NAV_STATUS_HEADER) ) {
currentMsgType = MT_NAV_STATUS;
payloadSize = sizeof(NAV_STATUS);
}
else {
// unknown message type, bail
fpos = 0;
continue;
}
}
if ( fpos == (payloadSize+2) ) {
// All payload bytes have now been received, so we can calculate the
// expected checksum value to compare with the next two incoming bytes.
calcChecksum(checksum, payloadSize);
}
else if ( fpos == (payloadSize+3) ) {
// First byte after the payload, ie. first byte of the checksum.
// Does it match the first byte of the checksum we calculated?
if ( c != checksum[0] ) {
// Checksum doesn't match, reset to beginning state and try again.
fpos = 0;
}
}
else if ( fpos == (payloadSize+4) ) {
// Second byte after the payload, ie. second byte of the checksum.
// Does it match the second byte of the checksum we calculated?
fpos = 0; // We will reset the state regardless of whether the checksum matches.
if ( c == checksum[1] ) {
// Checksum matches, we have a valid message.
return currentMsgType;
}
}
else if ( fpos > (payloadSize+4) ) {
// We have now read more bytes than both the expected payload and checksum
// together, so something went wrong. Reset to beginning state and try again.
fpos = 0;
}
}
}
return MT_NONE;
}
// Calculate expected UBX ACK packet and parse UBX response from GPS
boolean getUBX_ACK(uint8_t *MSG) {
uint8_t b;
uint8_t ackByteID = 0;
uint8_t ackPacket[10];
unsigned long startTime = millis();
Serial.print(" * Reading ACK response: ");
// Construct the expected ACK packet
ackPacket[0] = 0xB5; // header
ackPacket[1] = 0x62; // header
ackPacket[2] = 0x05; // class
ackPacket[3] = 0x01; // id
ackPacket[4] = 0x02; // length
ackPacket[5] = 0x00;
ackPacket[6] = MSG[2]; // ACK class
ackPacket[7] = MSG[3]; // ACK id
ackPacket[8] = 0; // CK_A
ackPacket[9] = 0; // CK_B
// Calculate the checksums
for (uint8_t i=2; i<8; i++) {
ackPacket[8] = ackPacket[8] + ackPacket[i];
ackPacket[9] = ackPacket[9] + ackPacket[8];
}
while (1) {
// Test for success
if (ackByteID > 9) {
// All packets in order!
Serial.println(" (SUCCESS!)");
return true;
}
// Timeout if no valid response in 3 seconds
if (millis() - startTime > 3000) {
Serial.println(" (FAILED!)");
return false;
}
// Make sure data is available to read
if (mySerial.available()) {
b = mySerial.read();
// Check that bytes arrive in sequence as per expected ACK packet
if (b == ackPacket[ackByteID]) {
ackByteID++;
mySerial.print(b, HEX);
}
else {
ackByteID = 0; // Reset and look again, invalid order
}
}
}
}
void setup() {
// initialize ina219 with default measurement range of 32V, 2A
ina219.begin();
// ina219.setCalibration_32V_2A(); // set measurement range to 32V, 2A (do not exceed 26V!)
// ina219.setCalibration_32V_1A(); // set measurement range to 32V, 1A (do not exceed 26V!)
ina219.setCalibration_16V_400mA(); // set measurement range to 16V, 400mA
// initialize OLED display
display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
display.clearDisplay();
display.setTextColor(WHITE);
display.setTextSize(1);
display.display();
Serial.begin(9600);
mySerial.begin(9600);
wakeUp();
Serial.println("wait for 5 seconds");
delay(5000);
//Serial.println("power off");
//send to sleep again!
//powerOff();
while(!gps_set_sucess) {
Serial.write("Try to switch off NMEA\n");
sendUBX(config1, sizeof(config1)/sizeof(uint8_t));
gps_set_sucess = getUBX_ACK(config1);
delay(1000);
}
}
// Send a byte array of UBX protocol to the GPS
void sendUBX(uint8_t *MSG, uint8_t len) {
for(int i=0; i<len; i++) {
mySerial.write(MSG[i]);
Serial.print(MSG[i], HEX);
}
mySerial.println();
}
void powerOff() {
uint8_t setPowerSaveMode[] = { 0xB5, 0x62, 0x02, 0x41, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x4D, 0x3B };
sendUBX(setPowerSaveMode, sizeof(setPowerSaveMode)/sizeof(uint8_t));
}
void wakeUp() {
uint8_t setUp[] = { 0x00 };
sendUBX(setUp, sizeof(setUp)/sizeof(uint8_t));
delay(500);
}
int displayCounter = 0;
int gpsCounter = 0;
int sleeping = 0;
void processDisplay() {
// read data from ina219
busvoltage = ina219.getBusVoltage_V();
current_mA = ina219.getCurrent_mA();
// show data on OLED
display.clearDisplay();
display.setCursor(0, 0);
display.print(busvoltage);
display.print(" V");
display.setCursor(50, 0);
display.print(current_mA);
display.print(" mA");
display.setCursor(0, 10);
display.print(lat);
display.print(" lat, 3dFix:");
display.print(tdFix);
display.setCursor(0, 20);
display.print(ubxMessage.navPosllh.lon/10000000.0f);
display.print(" lon");
display.display();
}
void loop() {
if (displayCounter > 10) {
processDisplay();
displayCounter = 0;
}
displayCounter++;
if (gpsCounter > 500) {
if (sleeping == 0) {
sleeping = 1;
powerOff();
} else {
sleeping = 0;
wakeUp();
}
gpsCounter = 0;
}
gpsCounter++;
if (sleeping == 0) {
int msgType = processGPS();
if ( msgType == MT_NAV_POSLLH ) {
lat = ubxMessage.navPosllh.lat/10000000.0f;
lon = ubxMessage.navPosllh.lon/10000000.0f;
Serial.print("iTOW:");
Serial.print(ubxMessage.navPosllh.iTOW);
Serial.print(" lat/lon: ");
Serial.print(ubxMessage.navPosllh.lat/10000000.0f);
Serial.print(",");
Serial.print(ubxMessage.navPosllh.lon/10000000.0f);
Serial.print(" hAcc: ");
Serial.print(ubxMessage.navPosllh.hAcc/1000.0f);
Serial.println();
}
else if ( msgType == MT_NAV_STATUS ) {
Serial.print("gpsFix:");
Serial.print(ubxMessage.navStatus.gpsFix);
Serial.println();
tdFix = ubxMessage.navStatus.gpsFix;
}
}
//bool ready = false;
//if (mySerial.available()) {
// char c = mySerial.read();
// Serial.write(c);
//}
delay(500);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment