145 lines
2.9 KiB
C++
145 lines
2.9 KiB
C++
#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 = 0.0f;
|
|
bool pressure_sensor_ready = false;
|
|
|
|
bool init_2ndi2c(){
|
|
if (!Wire1.begin(dp_sda, dp_scl))
|
|
{
|
|
Serial.println("Error starting I2C");
|
|
return false;
|
|
}
|
|
Wire1.setClock(400000);
|
|
return true;
|
|
}
|
|
bool 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");
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
bool init_pressure_sensor(void)
|
|
{
|
|
if (!bmp.begin(BMP085_ULTRAHIGHRES, &Wire1))
|
|
{
|
|
Serial.println("Error communicating with pressure sensor");
|
|
return false;
|
|
}
|
|
basicaltitude=bmp.readPressure();
|
|
if (basicaltitude <= 0.0f)
|
|
{
|
|
Serial.println("Invalid initial pressure");
|
|
return false;
|
|
}
|
|
for (float &sample : pressure_avg)
|
|
sample = basicaltitude;
|
|
pressure_sensor_ready = true;
|
|
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");
|
|
return true;
|
|
}
|
|
|
|
float get_pressure()
|
|
{
|
|
if (!pressure_sensor_ready)
|
|
return 0.0f;
|
|
|
|
float pressure;
|
|
pressure = bmp.readPressure();
|
|
if (pressure <= 0.0f)
|
|
return pressure_avg[9];
|
|
|
|
memmove(pressure_avg, &pressure_avg[1], sizeof(pressure_avg) - sizeof(pressure_avg[0]));
|
|
pressure_avg[9] = pressure;
|
|
return pressure;
|
|
}
|
|
|
|
float get_pressure_avg(void)
|
|
{
|
|
float total = 0.0f;
|
|
int i;
|
|
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 = 0.0f;
|
|
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 / 9);
|
|
}
|
|
|
|
float get_altitude(float pressure)
|
|
{
|
|
float altitude;
|
|
altitude = 44330 * (1.0 - pow(pressure / 101325, 0.1903));
|
|
return altitude;
|
|
}
|
|
|
|
float get_altitude_diff(void){
|
|
return get_altitude(pressure_avg[9]) - get_altitude(basicaltitude);
|
|
}
|