// Integracion Final Proyecto Lata

// Equipo Orbitard
// Integrantes:
// Alvaro Zapata
// Daniel De La Rosa
// Carolina Campagna
// Oliver Rosario
// Joseph Molesky
// Luis Guzman
// Gilbert Olivero

// ---Librerias---
// Acelerometro y Gyroscopio
#include "FastIMU.h"
#include <Wire.h>
#include "Madgwick.h"
#include <Adafruit_BMP085.h>

// RF
#include "SPI.h"
#include "NRFLite.h"

// GPS
#include <TinyGPSPlus.h>
#include <SoftwareSerial.h>

// ---Macros y Objetos---
//Altura a la que se realizara el desacople
#define ALTURA_DESACOPLE 20

//Acelerometro y Gyroscopio
#define IMU_ADDRESS 0x68 //Direccion I2C
#define PERFORM_CALIBRATION //Calibracion Inicial
MPU9250 IMU;
calData calib = { 0 }; //Calibration data
AccelData accelData; //Sensor data
GyroData gyroData;
MagData magData;
Madgwick filter;
Adafruit_BMP085 bmp;

double lastTemp = 0;
double lastAltitude = 0;

// RF
uint8_t ID_CARGA_PRIM = 2; // ID del Radio de la carga primaria
uint8_t ID_CARGA_SEC = 3; // ID del Radio de la carga secundaria
uint8_t ID_EST_TERRENA = 0; // ID del Radio a Transmitir
uint8_t PIN_RADIO_CE = 8;
uint8_t PIN_RADIO_CSN = 10;

struct RadioPacket_Loc { // Paquete a enviar (Max. 32 bits)

uint8_t FromRadioId;
char Loc[26] = "GPS aun no localizado";
uint32_t FailedTxCount = 0;
};
struct RadioPacket_Acc { // Paquete a enviar (Max. 32 bits)
uint8_t FromRadioId;
char Dat[21] = "Acc no configurado";
uint32_t FailedTxCount = 0;
};

struct packetTemp {
uint8_t FromRadioId = 87;
char Temp[23] = "!Temp";
uint32_t FailedTxCount = 0;
};

struct packetQuat {
uint8_t FromRadioId;
char Quat[28] = "!quaternions";
};

struct packetGyro {
uint8_t FromRadioId;
char Gyro[24] = "no gyro";
};

struct packetMag {
uint8_t FromRadioId;
char Mag[21] = "no mag";
};

struct Quaternions {
double w, x, y, z;
};

Quaternions q;
uint8_t Alt_Desacople = 1;

NRFLite _radio;
RadioPacket_Loc _packetGPS;
RadioPacket_Acc _packetACC;
packetQuat _packetQUAT;
packetGyro _packetGYRO;
packetMag _packetMAG;
packetTemp _dataTEMP;

// GPS
int RXPin = 2, TXPin = 3;
uint32_t GPSBaud = 115200;
// The TinyGPSPlus object
TinyGPSPlus gps;
// The serial connection to the GPS device
SoftwareSerial ss(RXPin, TXPin);

// ---Variables del Proceso---

char AccX[8];
char AccY[8];
char AccZ[8];

char GyroX[7];
char GyroY[7];
char GyroZ[7];

char MagX[6];
char MagY[6];
char MagZ[6];

char Temp[6];
char Alt[8];
char Pres[6];

char Lat[13];
char Long[13];

char quatX[6];
char quatY[6];
char quatZ[6];
char quatW[6];

bool Dasacople = false;

double primaraLecturaAltura = 0;
bool cayendo = false;
float altura_inicial = 0;

char error_init_serial[29] = "Error Inicializando serial: ";

char error_finding_bmp_sensor[52] = "Could not find a valid BMP180 sensor, check
wiring!";
char error_init_imu[26] = "Error Inicializando IMU: ";

void setup() {
// ---Configuracion Inicial---
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000); //400khz clock
while (!Serial) {
Serial.println(error_init_serial);
BlinkError(100); // Wait here forever.
}

if (!bmp.begin()) {
Serial.println(error_finding_bmp_sensor);
}

//----------------------------------------------- INICIALIZACION
IMU-------------------------------------------

int err = IMU.init(calib, IMU_ADDRESS);

if (err != 0) {
Serial.print(error_init_imu);
Serial.println(err);
while (true) {
BlinkError(250); // Wait here forever.
}
}

#ifdef PERFORM_CALIBRATION
CalibracionIMU();
#endif
 err = IMU.setGyroRange(100); //Rango Gyroscopio
err = IMU.setAccelRange(2); //Rango Accelerometro (cantidad de gravedades, Gs)

filter.begin(0.2f);
//--------------------------------------------FIN INICIALIZACION
IMU-------------------------------------------

//----------------------------------------------- INICIALIZACION
RF--------------------------------------------
_radio.init(ID_CARGA_PRIM, PIN_RADIO_CE, PIN_RADIO_CSN, NRFLite::BITRATE2MBPS,
100);
// if (!_radio.init(ID_CARGA_PRIM, PIN_RADIO_CE, PIN_RADIO_CSN,
NRFLite::BITRATE2MBPS, 100)) {
// Serial.println("Cannot communicate with radio");
// while (1)
// BlinkError(500); // Wait here forever.
// digitalWrite(LED_BUILTIN, HIGH);
// }
// Serial.println("Iniciado RF");

//Ids de los paquetes

_packetGPS.FromRadioId = ID_CARGA_PRIM;
_packetACC.FromRadioId = ID_CARGA_PRIM + 15;
_dataTEMP.FromRadioId = ID_CARGA_PRIM + 20;
_packetQUAT.FromRadioId = ID_CARGA_PRIM + 25;
_packetGYRO.FromRadioId = ID_CARGA_PRIM + 30;
_packetMAG.FromRadioId = ID_CARGA_PRIM + 35;

//--------------------------------------------FIN INICIALIZACION
RF-------------------------------------------

//----------------------------------------------- INICIALIZACION
GPS-------------------------------------------
ss.begin(GPSBaud);
//--------------------------------------------FIN INICIALIZACION
GPS-------------------------------------------

// Confirmacion
// pinMode(LED_BUILTIN, OUTPUT);
// digitalWrite(LED_BUILTIN, HIGH);
// delay(500);
// digitalWrite(LED_BUILTIN, LOW);
// delay(500);
// digitalWrite(LED_BUILTIN, HIGH);
// delay(500);
// digitalWrite(LED_BUILTIN, LOW);
// delay(500);
// digitalWrite(LED_BUILTIN, HIGH);
// delay(500);
// digitalWrite(LED_BUILTIN, LOW);
// delay(500);
// Serial.println("Inicializado");
}

void loop() {
BAR_Loop();
IMU_Loop();
 GPS_Loop();
}

void BAR_Loop(){
float alt = bmp.readAltitude();
dtostrf(bmp.readTemperature(), 1, 2, Temp);
dtostrf(bmp.readPressure(), 1, 0, Pres);
// Calculate altitude assuming 'standard' barometric
// pressure of 1013.25 millibar = 101325 Pascal
if (altura_inicial = 0) {
altura_inicial = alt;
}
dtostrf((alt - altura_inicial), 1, 2, Alt);

snprintf(_dataTEMP.Temp, 23, "%s;%s;%s", Temp, Pres, Alt);

delay(100);

_radio.send(ID_EST_TERRENA, &_dataTEMP, sizeof(_dataTEMP)); // Note how '&' must

be placed in front of the variable name.
}

void IMU_Loop() {
IMU.update();
IMU.getAccel(&accelData);
IMU.getGyro(&gyroData);
IMU.getMag(&magData);

dtostrf(accelData.accelX, 1, 3, AccX);
delay(1);
dtostrf(accelData.accelY, 1, 3, AccY);
delay(1);
dtostrf(accelData.accelZ, 1, 3, AccZ);
delay(1);
snprintf(_packetACC.Dat, 21, "%s;%s;%s", AccX, AccY, AccZ);

_radio.send(ID_EST_TERRENA, &_packetACC, sizeof(_packetACC)); // Note how '&'

must be placed in front of the variable name.

dtostrf((gyroData.gyroX), 1, 1, GyroX);
dtostrf((gyroData.gyroY), 1, 1, GyroY);
dtostrf((gyroData.gyroZ), 1, 1, GyroZ);
snprintf(_packetGYRO.Gyro, 24, "%s;%s;%s", GyroX, GyroY, GyroZ);
_radio.send(ID_EST_TERRENA, &_packetGYRO, sizeof(_packetGYRO));

dtostrf(magData.magX, 1, 1, MagX);
dtostrf(magData.magY, 1, 1, MagY);
dtostrf(magData.magZ, 1, 1, MagZ);
snprintf(_packetMAG.Mag, 21, "%s;%s;%s", MagX, MagY, MagZ);
_radio.send(ID_EST_TERRENA, &_packetMAG, sizeof(_packetMAG));

filter.updateIMU(gyroData.gyroX, gyroData.gyroY, gyroData.gyroZ,

accelData.accelX, accelData.accelY, accelData.accelZ);

dtostrf(filter.getQuatX(), 1, 2, quatX);
delay(1);
dtostrf(filter.getQuatY(), 1, 2, quatY);
 delay(1);
dtostrf(filter.getQuatZ(), 1, 2, quatZ);
delay(1);
dtostrf(filter.getQuatW(), 1, 2, quatW);
delay(1);
snprintf(_packetQUAT.Quat, 28, "%s;%s;%s;%s", quatW, quatX, quatY, quatZ);

_radio.send(ID_EST_TERRENA, &_packetQUAT, sizeof(_packetQUAT)); // Note how '&'

must be placed in front of the variable name.

void GPS_Loop() {
while (ss.available() > 0)
if (gps.encode(ss.read()))
displayInfo();

if (millis() > 5000 && gps.charsProcessed() < 10) {

Serial.println("No GPS detected: check wiring.");
while (true)
;
}
}

void displayInfo() {
Serial.println("gps loop engaged");
if (gps.location.isValid()) {
dtostrf(gps.location.lat(), 4, 6, Lat);
dtostrf(gps.location.lng(), 4, 6, Long);

snprintf(_packetGPS.Loc, 26, "%s;%s", Lat, Long);

//Enviar longitud y latitud

if(_radio.send(ID_EST_TERRENA, &_packetGPS, sizeof(_packetGPS))){
Serial.println("gps sent successfully");
} else{
Serial.println("error sending gps");
}
} else {
Serial.println("Unable to get valid gps signal");
}
}

char imu_calib[16] = "Keep IMU level.";

void CalibracionIMU() {
// Serial.println("FastIMU calibration & data example");
delay(1000);
Serial.println(imu_calib);
delay(2500);
IMU.calibrateAccelGyro(&calib);
IMU.calibrateMag(&calib);
Serial.println("Calibration done!");
Serial.println("Accel biases X/Y/Z: ");
Serial.print(calib.accelBias[0]);
Serial.print(", ");
Serial.print(calib.accelBias[1]);
Serial.print(", ");
Serial.println(calib.accelBias[2]);
 Serial.println("Gyro biases X/Y/Z: ");
Serial.print(calib.gyroBias[0]);
Serial.print(", ");
Serial.print(calib.gyroBias[1]);
Serial.print(", ");
Serial.println(calib.gyroBias[2]);
Serial.println("Magnetometer biases X/Y/Z: ");
Serial.print(calib.magBias[0]);
Serial.print(", ");
Serial.print(calib.magBias[1]);
Serial.print(", ");
Serial.println(calib.magBias[2]);

delay(1000);
IMU.init(calib, IMU_ADDRESS);
}

void BlinkError(uint32_t tiempo) {

pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
delay(tiempo);
digitalWrite(13, LOW);
delay(tiempo);
}