migration repo

lib/sailplane/airspeed.cpp

@@ -0,0 +1,119 @@
+#include "../windentelemetry.h"
+#include "airspeed.h"
+#include <ms4525do.h>
+#include <airdata.h>
+#include <units.h>
+#include <Wire.h>
+#include <Adafruit_BMP085.h>
+
+extern TwoWire Wire1;
+bfs::Ms4525do pres;
+Adafruit_BMP085 bmp;
+
+float pressure_avg[10];
+float basicaltitude;
+void init_2ndi2c(){
+  Wire1.begin(dp_sda, dp_scl);
+  Wire1.setClock(400000);
+}
+void init_airspeed_sensor(void)
+{
+
+  /*
+   * I2C address of 0x28, on bus 0, with a -1 to +1 PSI range
+   */
+  pres.Config(&Wire1, 0x28, 1.0f, -1.0f);
+  /* Starting communication with the pressure transducer */
+  if (!pres.Begin())
+  {
+    Serial.println("Error communicating with sensor");
+    while (1)
+    {
+    }
+  }
+}
+
+int get_airspeed_kmh(void)
+{
+  int vel_kmh = round(bfs::convvel(get_airspeed(), bfs::LinVelUnit::MPS, bfs::LinVelUnit::KPH));
+  return vel_kmh;
+}
+float get_airspeed(void)
+{
+  if (pres.Read())
+  {
+    float ms = bfs::Ias_mps(pres.pres_pa());
+
+    return ms;
+  }
+  else
+  {
+    return 0;
+  }
+}
+void init_pressure_sensor(void)
+{ 
+  bmp.begin(BMP085_ULTRAHIGHRES, &Wire1);
+  basicaltitude=bmp.readPressure();
+  Serial.print("Pressure = ");
+  Serial.print(basicaltitude);
+  Serial.println(" Pa");
+
+  // Calculate altitude assuming 'standard' barometric
+  // pressure of 1013.25 millibar = 101325 Pascal
+  Serial.print("Altitude = ");
+  Serial.print(get_altitude(basicaltitude));
+  Serial.println(" meters");
+}
+
+float get_pressure()
+{
+  float pressure;
+  pressure = bmp.readPressure();
+  memcpy(pressure_avg, &pressure_avg[1], sizeof(pressure_avg) - sizeof(int));
+  pressure_avg[9] = pressure;
+  return pressure;
+}
+
+float get_pressure_avg(void)
+{
+  int i;
+  float total;
+  for (i = 0; i <10; i++)
+  {
+    total = total + pressure_avg[i];
+  }
+  return (total / 10);
+}
+float get_vario(void)
+{
+  float vario;
+  vario = get_altitude(pressure_avg[9]) - get_altitude(pressure_avg[8]);
+  return vario;
+}
+
+float get_vario_avg(void)
+{
+  float vario;
+
+  int i;
+  float total;
+  for (i = 0; i <9; i++)
+  {
+    int n=i+1;
+    vario = get_altitude(pressure_avg[n]) - get_altitude(pressure_avg[i]);
+    total = total + vario;
+  }
+  return (total / 10);
+}
+
+float get_altitude(float pressure)
+{
+  float altitude;
+  altitude = 44330 * (1.0 - pow(pressure / 101325, 0.1903));
+  return altitude;
+}
+
+float get_altitude_diff(void){
+  return bmp.readPressure() - basicaltitude;
+}