Diff: avs.cpp

Revision:

0:5c7d59862c77

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/avs.cpp	Mon Dec 18 16:55:37 2017 +0000
@@ -0,0 +1,158 @@
+#include "mbed.h"
+#include <math.h>
+#include "avs.h"
+
+fpct avs_c::dyn_press(fpct P, fpct S)
+// Given the Pitot pressure P (hPa) and Static pressure S (hPa)
+// returns the dynamic pressure in Pascal (100 * hPa = Pascal)
+// Values less than 20 Pa are truncated to 0
+{
+  const fpct x = fabs(P-S) * 100.0f;
+  if (x > 20.0f) return (x);
+  else return (0.0f);
+}
+
+fpct avs_c::sealevel(fpct P, fpct A)
+// Given a pressure P (mb) taken at a specific altitude (meters),
+// return the equivalent pressure (mb) at sea level.
+// This produces pressure readings that can be used for weather measurements.
+{
+  return (P / pow((fpct)1 - (A / (fpct)44330.0), (fpct)5.255));
+}
+
+
+fpct avs_c::altitude(fpct P, fpct P0)
+// Given a pressure measurement P (mb) and the pressure at a baseline P0 (mb),
+// return altitude (meters) above baseline.
+{
+  return ((fpct)44330.0 * ((fpct)1 - pow(P / P0, (fpct)1 / (fpct)5.255)));
+  // approximation if pow() is too expensive: return (P * -9.9011f + 9900.0f);
+}
+
+fpct avs_c::bernulli_ias(fpct pitot, fpct stat, bool clip)
+// Given a pressure measurement pitot (hPa) and stat (hPa),
+// return IAS (km/h).
+{
+	const fpct d0 = (fpct)1.225; // kg/m^3
+	fpct s = ((fpct)3.6 * sqrt((fpct)200.0 * fabs(pitot-stat) / d0));
+	if (clip && (s < 20.0f)) return ((fpct)0.0);
+	else return s;
+}
+
+fpct avs_c::bernulli_tas(fpct pitot, fpct stat, bool clip)
+// Given a pressure measurement pitot (hPa) and stat (hPa),
+// return TAS (km/h).
+{
+	const fpct d0 = (fpct)1.225; // kg/m^3
+	const fpct p0 = 1013.25; // hPa
+
+	fpct s = ((fpct)3.6 * sqrt(p0/stat) * sqrt((fpct)200.0 * fabs(pitot-stat) / d0));
+	if (clip && (s < 20.0f)) return ((fpct)0.0);
+	else return s;
+}
+
+int avs_c::nmea_checksum(char *s)
+{
+  int my_xor = 0;
+  for (; *s != '\0'; s++) {
+    my_xor ^= *s;  // calculate checksum
+  }
+  return (my_xor);
+}
+
+/*
+  E: TE vario in m/s
+  Example: $POV,E,2.15*14
+  
+  S: true airspeed in km/h
+  Example: $POV,S,123.45*05
+  
+  P: static pressure in hPa
+  Example: $POV,P,1018.35*39
+  
+  Q: dynamic pressure in Pa
+  Example: $POV,Q,23.3*04
+  
+  R: total pressure in hPa
+  Example: $POV,R,1025.17*35
+*/
+#ifndef TESTBENCH
+void avs_c::nmea_out(char *buf, fpct xv, fpct stat, fpct dynp, fpct xtas, fpct totp)
+{
+  char nmea_string[100];
+
+  sprintf(nmea_string, "$POV,E,%04.2f,S,%04.2f,P,%04.2f,Q,%04.2f,R,%04.2f",
+  xv,xtas,stat,dynp,totp);
+
+  sprintf(buf,"%s*%02X", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
+}
+
+void avs_c::nmea_out(Serial *channel, fpct xv, fpct stat, fpct dynp, fpct xtas, fpct totp)
+{
+  char nmea_string[100];
+
+  sprintf(nmea_string, "$POV,E,%.2f,S,%.2f,P,%.2f,Q,%.2f,R,%.2f",
+  xv,xtas,stat,dynp,totp);
+
+  channel->printf("%s*%02X\n", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
+}
+
+void avs_c::nmea_out(char *buf, fpct xv, fpct stat, fpct dynp, fpct xtas)
+{
+  char nmea_string[100];
+
+  sprintf(nmea_string, "$POV,E,%04.2f,S,%04.2f,P,%04.2f,Q,%04.2f",
+  xv,xtas,stat,dynp);
+
+  sprintf(buf,"%s*%02X", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
+}
+
+void avs_c::nmea_out(Serial *channel, fpct xv, fpct stat, fpct dynp, fpct xtas)
+{
+  char nmea_string[100];
+
+  sprintf(nmea_string, "$POV,E,%.2f,S,%.2f,P,%.2f,Q,%.2f",
+  xv,xtas,stat,dynp);
+
+  channel->printf("%s*%02X\n", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
+}
+
+void avs_c::nmea_out(char *buf, fpct xv, fpct stat, fpct dynp)
+{
+  char nmea_string[100];
+
+  sprintf(nmea_string, "$POV,E,%04.2f,P,%04.2f,Q,%04.2f",
+  xv,stat,dynp);
+
+  sprintf(buf,"%s*%02X", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
+}
+
+void avs_c::nmea_out(Serial *channel, fpct xv, fpct stat, fpct dynp)
+{
+  char nmea_string[100];
+
+  sprintf(nmea_string, "$POV,E,%.2f,P,%.2f,Q,%.2f",
+  xv,stat,dynp);
+
+  channel->printf("%s*%02X\n", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
+}
+
+void avs_c::nmea_alert(Serial *channel, Severity lev, char * message)
+{
+  char nmea_string[100], al_char;
+  
+  switch (lev) 
+  {
+  	case warning : al_char = 'W'; break;
+  	case err : al_char = 'E'; break;
+  	case failure : al_char = 'F'; break;
+  	default : al_char = 'I'; break;
+  }
+
+  sprintf(nmea_string, "$POV,C,AL,%c,%s",al_char,message);
+
+  channel->printf("%s*%02X\n", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
+}
+#endif
+
+