// 3€ Altimeter SOFEX Version
// benutzt die bmp180 bibliothek welche haupsächlich auf der BMP085 Bibliothek von Filipe Vieira basiert
// Der Altimeter misst die maximale Höhe und blinkt sie nach dem wiedereinsetzen der batterie aus
// blinkt aus in Metern, beginnend mit der kleinsten Ziffer, 0=10 mal blinken

// hier werden Bibliotheken geladen
#include <Wire.h> // I2C bibliothek für die Kommunikation mit dem Sensor
#include <BMP180.h> // Drucksensor
#include <EEPROM.h> // für das speichern zum Eeprom
#include <avr/power.h> // avr Stromsparfunktionen
#include <LowPower.h> // Stromsparoptionen für den ATMrega in den Standby zu schicken

//im folgenden werden Variablen definiert
//#define feetmode // uncomment if you want the altitude to be blinked out in feet 
int bmpPin = 3;                 // dieser Pin wird genutzt um den BMP180 an und aus zu schalten

BMP180 dps = BMP180();
long Pressure = 0;
int32_t temperature = 0;
unsigned long start=0;
unsigned long start2=0;
float baseline = 0; // Druck am Boden
const word launchalti=20; //20 m höhe für Starterkennung; or in feet if you chose feetmode
float maxalt=0;
int loopcount=0;
// variables for kalman
const float dt = 1 / 64; 
const float gain[3] = {0.16, 0.98, 2.88}; // kalman gain extern in python berechnet
float p_pred, v_pred, a_pred;
float p=0;
float a=0;
float v=0; // status Variable p= Höhe v=Geschwindigkeit a=Beschleunigung
float measurement=0;
word safedalt;
word downcount =100;
byte flightstate =0;


//hier werden einige Funktionen definiert die später verwendet werden
// sehr kurzer blinken um Strom zu sparen aber anzuzeigen dass der Altiemter eingeschaltet ist
void fastblink() { 
    digitalWrite(13, HIGH);
    delay(2);
    digitalWrite(13, LOW);
 }

//   blinkt eine einzelne Ziffer aus 0=10 mal blinken 
void blinknumber(int number) {
  if (number == 0) number=10;
  for (int g=0; g < number; g++) {
    digitalWrite(13, HIGH);
    delay(300);
    digitalWrite(13, LOW);
    delay(300);
  } 
  delay(2000); // wartet 2000ms bis zur nächsten Ziffer
}

// Blinkt die Höhe aus, funktioniert bis 9999m, da es nicht im Klaman loop ist intressiert uns die Geschwindigkeit nicht 
void blinkalti(int lastmaxp) { 
#if defined(feetmode) // blinks out feet if you want
  lastmaxp=lastmaxp*3.2808399;
#endif   
  pinMode(13, OUTPUT); // das ist die interne LED auf dem Pro mini
  digitalWrite(13, LOW);
  delay(500);
  blinknumber((lastmaxp)%10); // letzte Ziffer
  if (lastmaxp>9) blinknumber((lastmaxp/10)%10); // 10er
  if (lastmaxp>99) blinknumber((lastmaxp/100)%10); // 100er
  if (lastmaxp>999) blinknumber((lastmaxp/1000)%10); // 1000er
  }    
  


// Code um die Batteriespannung vor dem Start zu prüfen
const long InternalReferenceVoltage = 1062;  // Adjust this value to your board's specific internal BG voltage
// Code courtesy of "Coding Badly" and "Retrolefty" from the Arduino forum
// results are Vcc in V * 100
int getBandgap () 
  {
  // REFS0 : Selects AVcc external reference
  // MUX3 MUX2 MUX1 : Selects 1.1V (VBG)  
   ADMUX = bit (REFS0) | bit (MUX3) | bit (MUX2) | bit (MUX1);
   ADCSRA |= bit( ADSC );  // start conversion
   while (ADCSRA & bit (ADSC)) { }  // wait for conversion to complete
   int results = (((InternalReferenceVoltage * 1024) / ADC) + 5) / 10; 
   return results;
  } 
 




void setup(void) {
   Serial.begin(9600);
   pinMode(bmpPin, OUTPUT);  //
   digitalWrite(bmpPin, HIGH); // Versorgt den BMP180 über den digitalen pin 3, ermöglicht in abzuschalten, spart 0.2mA in Standbymode, Schützt in auch vor Verpolung
   power_spi_disable(); // schaltet spi aus um strom zu sparen (nicht benutzen falls man auf SD loggt)
   power_timer1_disable(); //schaltet timer aus um strom zu sparen 
   power_timer2_disable(); // timer0 wird gebraucht für millis
 
   Wire.begin();
   dps.getCalData();   // liest die Calibrierungsdaten des bmp180
   delay(1000); 
   dps.onetimepressure(&Pressure);
   Serial.print("Druck (Pa): ");
   Serial.println(Pressure);
   EEPROM.get(0, safedalt); // liest die letzte Maximalhöhe vom eeprom
   Serial.print("Maximale Höhe: ");
   Serial.println(safedalt);
   blinkalti(safedalt); // blinkt die letzte Maximalhähe aus die aus dem eeprom gelesen wurde
   if (getBandgap()<240) {for (int i=0; i<100; i++) {fastblink(); delay(100);}}; /// low voltage warning
   delay(30000); // wartet 30 Sekunden um einem Zeit zu geben den Altimeter einzubauen
   for (int i = 0; i < 200; i++) { // mittelt 200 mal den druck am Boden
      dps.onetimepressure(&Pressure);
      baseline = baseline + Pressure;
   }
   baseline = baseline /200;
   dps.getAltitude(&Pressure, &baseline, &measurement);
   }


// Klaman // Geschwindigkeitsvorteil mit bitshifting ist vernachlässigbar auf dem Atmega328P
void kalman() {
  //Vorhersage vom Modell und letzten Werten
  p_pred = p + dt * v + dt * dt * 0.5 * a;
  v_pred = v + dt * a;
  a_pred = a;
  //korrektur der Vorhersage mit dem Messwerten
  p = p_pred + gain[0] * (measurement - p_pred);
  v = v_pred + gain[1] * (measurement - p_pred);
  a = a_pred + gain[2] * (measurement - p_pred);
}


void loop(void){
     while (micros() - start <= 15615) {} // wartet 1/64 s um ein festes Intervall für den Kalman zu haben can auf 70Hz erhöht werden
     start=micros(); // läuft über nach 70 minutes
     dps.startTemperature();
     delayMicroseconds(4500); // 4.5ms maximale Zeit die eine Temperaturmessung braucht
     dps.readTemperature();
     dps.startPressure(); 
     start2=micros(); // die Befehle sind getrennt um die Wartezeiten ausnutzen zu können // flass man nicht auf SD Karte loggt ist das nicht wirkilich nötig

 switch (flightstate) { // wechselt den flugstatus 
    case 0: //pad
      if (p> launchalti) flightstate=1; // Starterkennung falls der Klaman über launchalti ist
      if (loopcount==320 ) {loopcount=0; fastblink();}  //blinkt alle 5 Sekunden um Bereitschaft zu signalisieren // nur auf dem Pad um Strom zu sparen
      break;
    case 1: //gestartet
      if (p>maxalt) {maxalt=p;} // überprüft ob aktuelle Höhe höher der maximalen ist // Die Filterung ist recht gut, falls man spikes erwartet(Überschall) kann man die Geschwindigkeit überprüfen
      if (p<maxalt && downcount>0) {downcount--;} // detektiert den Gipfelpunkt als sinken der Höhe
      if (downcount<=0) {flightstate=2;} // Gipfelpunkt erkannt
      break;
    case 2: //Gipfelpüunkt  
      EEPROM.put(0,(word)maxalt);   // schreibt die Maximalhöhe in den eeprom wird in metern ausgegeben, cm sind nicht sinvoll // erzeugt 5ms Verzögerung was den nächsten kalman wert falsch macht, da es nach dem Gipfel Punkt ist, ist mir das egal. // one could correct this by a different kalman gain or skipping the temperature in the next cycle
      delay(500);
      pinMode(bmpPin, INPUT);  // schaltet den BMP180 ab
      LowPower.powerDown(SLEEP_FOREVER, ADC_OFF, BOD_OFF);  // schickt den Atmega in den Standby
      break;
  }
loopcount++;
while (micros() - start2 < 4500) {} ; // 4.5ms maximale Zeit die Eine Druckmessung braucht im ultra low power mode ohne oversampling
dps.readPressure(&Pressure);  // liest den Druck
dps.getAltitude(&Pressure, &baseline, &measurement);// Konvertiert aktuellen Druck und Durck am Boden in die Höhe
kalman();    // neue Höhe vom Kalmanfilter
}


