#include <IFCT.h>
#include <Wire.h>
#include "globals.h"
#include <Adafruit_Sensor.h>
#include <Adafruit_BME280.h>
#define SEALEVELPRESSURE_HPA (1013.25)
Adafruit_BME280 bme; // I2C
#include <ublox2.h>
#include "Streaming.h"
#include <string>
// a uBlox object, which is on Teensy hardware
// GPS serial port
UBLOX GPS(GPShwSERIAL);
// the uBlox data structure
gpsData uBloxData;
struct gps rtk;
CAN_message_t msg; // setup a storage buffer
void setup() {
Serial.begin(115200);
delay(5000);
bool status;
// default settings
// (you can also pass in a Wire library object like &Wire2)
status = bme.begin();
if (!status) {
Serial.println("Could not find a valid BME280 sensor, check wiring!");
while (1);
}
//GPS SETUP
// -- AutoBauding test --
// Try communication with the GPS
// receiver at 9600 baud, default settings
// then set GPS UART Baud to 460800
GPS.begin(9600);
GPS.SetGPSbaud(460800, true);
GPS.end();
GPS.begin(460800);
GPS.SetRATE(100, false); // Navigation/Measurement Rate Settings, e.g. 100ms => 10Hz, 200 => 5.00Hz, 1000ms => 1Hz, 10000ms => 0.1Hz
// Possible Configurations:
// 60=>16.67Hz, 64=>15.63Hz, 72=>13.89Hz, 80=>12.50Hz, 100=>10.00Hz, 125=>8.00Hz, 200=>5.00Hz, 250=>4.00Hz, 500=>2.00Hz
// 800=>1.25Hz, 1000=>1.00Hz, 2000=>0.50Hz, 4000=>0.25Hz, 10000=>0.10Hz, 20000=>0.05Hz, 50000=>0.02Hz
// NOTE: Dis_all_NMEA -strongly suggest changing RX buffer to 255 or more,*otherwise you will miss ACKs*on serial monitor
GPS.Dis_all_NMEA_Child_MSGs(false); // Disable All NMEA Child Messages Command
GPS.SetNAV5(4, false); // Set Dynamic platform model Navigation Engine Settings (0:portable, 2: stationary, 3:pedestrian, Etc)
// Possible Configurations
// 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne with <1g, 7: airborne with <2g
// 8: airborne with <4g, 9: wrist worn watch (not supported in protocol v.less than 18)
// ### Periodic auto update ON,OFF Command ###
GPS.Ena_NAV_PVT(true); // Enable periodic auto update NAV_PVT
//GPS.Dis_NAV_PVT(false); // Disable periodic auto update NAV_PVT
Can0.setMB(MB0, TX); //Add ,IDE for extended frames.
Can0.setMB(MB1, TX); //Add ,IDE for extended frames.
delay(2500);
Serial.println("-- Default Test --");
Serial.println();
}
void loop() {
static uint32_t _timer0 = millis();
if ( millis() - _timer0 > 100 ) {
msg.flags.extended = 0; //1 = extended frame
msg.flags.remote = 0;
msg.len = 8;
//Send Pressure and Temperature from BME280
msg.id = 0x69;
//setup two msg buffer (convert float to uint8's
convertFloat(bme.readPressure()/100.0F, bme.readTemperature(), msg);
Can0.write(MB0, msg);
_timer0 = millis();
}
if(GPS.read(&uBloxData)) {
rtk.iTOW = uBloxData.iTOW;
rtk.fixType = uBloxData.fixType;
rtk.numSV = uBloxData.numSV;
rtk.pDOP = uBloxData.pDOP * 100;
rtk.lat1 = uBloxData.lat;
rtk.lon1 = uBloxData.lon;
rtk.heading = uBloxData.heading;
rtk.gSpeed = uBloxData.gSpeed;
rtk.hMSL = uBloxData.hMSL;
if((rtk.iTOW - rtk.ts) > 0 ) {
Serial.println(rtk.lat1, 7);
sendGPS();
}
rtk.ts = rtk.iTOW;
}
static uint32_t _timer = millis();
if ( millis() - _timer > 4000 ) {
pinMode(13, OUTPUT); digitalWrite(13, !digitalRead(13));
Serial.print("*HEARTBEAT* "); Serial.println(millis());
//Can0.mailboxStatus();
_timer = millis();
}
}
void sendGPS(){
msg.flags.extended = 0; //1 = extended frame
msg.flags.remote = 0;
msg.len = 8;
//Sends GPS data from the M8N
msg.id = 0x01;
convertDouble(rtk.lat1, msg);
//Serial.print("ID1 TX Status:");
//Serial.println(Can0.write(MB1, msg));
Can0.write(MB1, msg);
//delay(2);
msg.id = 0x02;
convertDouble(rtk.lon1, msg);
//Serial.print("ID2 TX Status:");
//Serial.println(Can0.write(MB1, msg));
Can0.write(MB1, msg);
//delay(2);
msg.id = 0x03;
convertInt(rtk.fixType, rtk.numSV, msg);
//Serial.print("ID2 TX Status:");
//Serial.println(Can0.write(MB1, msg));
Can0.write(MB1, msg);
//delay(2);
msg.id = 0x04;
convertFloat(rtk.heading, (float) rtk.gSpeed, msg);
//Serial.print("ID3 TX Status:");
//Serial.println(Can0.write(MB1, msg));
Can0.write(MB1, msg);
//delay(2);
msg.id = 0x05;
convertFloat(rtk.iTOW, 0, msg);
//Serial.print("ID3 TX Status:");
//Serial.println(Can0.write(MB1, msg));
Can0.write(MB1, msg);
}
void convertFloat(float s_data, float s_data1, CAN_message_t &frame){
union ifct {
float val;
uint8_t b[4];
};
ifct data, data1;
//Serial.print("C2F_FRAME: ");
data.val = s_data;
data1.val = s_data1;
for (uint8_t i = 0; i < 4; i++) {
frame.buf[i] = data.b[i];
frame.buf[i+4] = data1.b[i];
}
}
void convertInt(int32_t s_data, int32_t s_data1, CAN_message_t &frame) {
union ifct {
int32_t val;
uint8_t b[4];
};
ifct data, data1;
//Serial.print("C2F_FRAME: ");
data.val = s_data;
data1.val = s_data1;
for (uint8_t i = 0; i < 4; i++) {
frame.buf[i] = data.b[i];
frame.buf[i+4] = data1.b[i];
}
}
void convertDouble(double s_data, CAN_message_t &frame){
union ifct {
double val;
uint8_t b[8];
};
ifct data;
//Serial.print("C2F_FRAME: ");
data.val = s_data;
for (uint8_t i = 0; i < 8; i++) {
frame.buf[i] = data.b[i];
}
}