Ronald Niederhagen / Mbed 2 deprecated Vario_101_407mini

Initial RC1

Dependencies:   QEI mbed

avs.cpp@0:5c7d59862c77, 2017-12-18 (annotated)

Committer:
bomilkar
Date:
Mon Dec 18 16:55:37 2017 +0000
Revision:
0:5c7d59862c77
Initial RC 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bomilkar 0:5c7d59862c77 1 #include "mbed.h"
bomilkar 0:5c7d59862c77 2 #include <math.h>
bomilkar 0:5c7d59862c77 3 #include "avs.h"
bomilkar 0:5c7d59862c77 4
bomilkar 0:5c7d59862c77 5 fpct avs_c::dyn_press(fpct P, fpct S)
bomilkar 0:5c7d59862c77 6 // Given the Pitot pressure P (hPa) and Static pressure S (hPa)
bomilkar 0:5c7d59862c77 7 // returns the dynamic pressure in Pascal (100 * hPa = Pascal)
bomilkar 0:5c7d59862c77 8 // Values less than 20 Pa are truncated to 0
bomilkar 0:5c7d59862c77 9 {
bomilkar 0:5c7d59862c77 10 const fpct x = fabs(P-S) * 100.0f;
bomilkar 0:5c7d59862c77 11 if (x > 20.0f) return (x);
bomilkar 0:5c7d59862c77 12 else return (0.0f);
bomilkar 0:5c7d59862c77 13 }
bomilkar 0:5c7d59862c77 14
bomilkar 0:5c7d59862c77 15 fpct avs_c::sealevel(fpct P, fpct A)
bomilkar 0:5c7d59862c77 16 // Given a pressure P (mb) taken at a specific altitude (meters),
bomilkar 0:5c7d59862c77 17 // return the equivalent pressure (mb) at sea level.
bomilkar 0:5c7d59862c77 18 // This produces pressure readings that can be used for weather measurements.
bomilkar 0:5c7d59862c77 19 {
bomilkar 0:5c7d59862c77 20 return (P / pow((fpct)1 - (A / (fpct)44330.0), (fpct)5.255));
bomilkar 0:5c7d59862c77 21 }
bomilkar 0:5c7d59862c77 22
bomilkar 0:5c7d59862c77 23
bomilkar 0:5c7d59862c77 24 fpct avs_c::altitude(fpct P, fpct P0)
bomilkar 0:5c7d59862c77 25 // Given a pressure measurement P (mb) and the pressure at a baseline P0 (mb),
bomilkar 0:5c7d59862c77 26 // return altitude (meters) above baseline.
bomilkar 0:5c7d59862c77 27 {
bomilkar 0:5c7d59862c77 28 return ((fpct)44330.0 * ((fpct)1 - pow(P / P0, (fpct)1 / (fpct)5.255)));
bomilkar 0:5c7d59862c77 29 // approximation if pow() is too expensive: return (P * -9.9011f + 9900.0f);
bomilkar 0:5c7d59862c77 30 }
bomilkar 0:5c7d59862c77 31
bomilkar 0:5c7d59862c77 32 fpct avs_c::bernulli_ias(fpct pitot, fpct stat, bool clip)
bomilkar 0:5c7d59862c77 33 // Given a pressure measurement pitot (hPa) and stat (hPa),
bomilkar 0:5c7d59862c77 34 // return IAS (km/h).
bomilkar 0:5c7d59862c77 35 {
bomilkar 0:5c7d59862c77 36 const fpct d0 = (fpct)1.225; // kg/m^3
bomilkar 0:5c7d59862c77 37 fpct s = ((fpct)3.6 * sqrt((fpct)200.0 * fabs(pitot-stat) / d0));
bomilkar 0:5c7d59862c77 38 if (clip && (s < 20.0f)) return ((fpct)0.0);
bomilkar 0:5c7d59862c77 39 else return s;
bomilkar 0:5c7d59862c77 40 }
bomilkar 0:5c7d59862c77 41
bomilkar 0:5c7d59862c77 42 fpct avs_c::bernulli_tas(fpct pitot, fpct stat, bool clip)
bomilkar 0:5c7d59862c77 43 // Given a pressure measurement pitot (hPa) and stat (hPa),
bomilkar 0:5c7d59862c77 44 // return TAS (km/h).
bomilkar 0:5c7d59862c77 45 {
bomilkar 0:5c7d59862c77 46 const fpct d0 = (fpct)1.225; // kg/m^3
bomilkar 0:5c7d59862c77 47 const fpct p0 = 1013.25; // hPa
bomilkar 0:5c7d59862c77 48
bomilkar 0:5c7d59862c77 49 fpct s = ((fpct)3.6 * sqrt(p0/stat) * sqrt((fpct)200.0 * fabs(pitot-stat) / d0));
bomilkar 0:5c7d59862c77 50 if (clip && (s < 20.0f)) return ((fpct)0.0);
bomilkar 0:5c7d59862c77 51 else return s;
bomilkar 0:5c7d59862c77 52 }
bomilkar 0:5c7d59862c77 53
bomilkar 0:5c7d59862c77 54 int avs_c::nmea_checksum(char *s)
bomilkar 0:5c7d59862c77 55 {
bomilkar 0:5c7d59862c77 56 int my_xor = 0;
bomilkar 0:5c7d59862c77 57 for (; *s != '\0'; s++) {
bomilkar 0:5c7d59862c77 58 my_xor ^= *s; // calculate checksum
bomilkar 0:5c7d59862c77 59 }
bomilkar 0:5c7d59862c77 60 return (my_xor);
bomilkar 0:5c7d59862c77 61 }
bomilkar 0:5c7d59862c77 62
bomilkar 0:5c7d59862c77 63 /*
bomilkar 0:5c7d59862c77 64 E: TE vario in m/s
bomilkar 0:5c7d59862c77 65 Example: $POV,E,2.15*14
bomilkar 0:5c7d59862c77 66
bomilkar 0:5c7d59862c77 67 S: true airspeed in km/h
bomilkar 0:5c7d59862c77 68 Example: $POV,S,123.45*05
bomilkar 0:5c7d59862c77 69
bomilkar 0:5c7d59862c77 70 P: static pressure in hPa
bomilkar 0:5c7d59862c77 71 Example: $POV,P,1018.35*39
bomilkar 0:5c7d59862c77 72
bomilkar 0:5c7d59862c77 73 Q: dynamic pressure in Pa
bomilkar 0:5c7d59862c77 74 Example: $POV,Q,23.3*04
bomilkar 0:5c7d59862c77 75
bomilkar 0:5c7d59862c77 76 R: total pressure in hPa
bomilkar 0:5c7d59862c77 77 Example: $POV,R,1025.17*35
bomilkar 0:5c7d59862c77 78 */
bomilkar 0:5c7d59862c77 79 #ifndef TESTBENCH
bomilkar 0:5c7d59862c77 80 void avs_c::nmea_out(char *buf, fpct xv, fpct stat, fpct dynp, fpct xtas, fpct totp)
bomilkar 0:5c7d59862c77 81 {
bomilkar 0:5c7d59862c77 82 char nmea_string[100];
bomilkar 0:5c7d59862c77 83
bomilkar 0:5c7d59862c77 84 sprintf(nmea_string, "$POV,E,%04.2f,S,%04.2f,P,%04.2f,Q,%04.2f,R,%04.2f",
bomilkar 0:5c7d59862c77 85 xv,xtas,stat,dynp,totp);
bomilkar 0:5c7d59862c77 86
bomilkar 0:5c7d59862c77 87 sprintf(buf,"%s*%02X", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
bomilkar 0:5c7d59862c77 88 }
bomilkar 0:5c7d59862c77 89
bomilkar 0:5c7d59862c77 90 void avs_c::nmea_out(Serial *channel, fpct xv, fpct stat, fpct dynp, fpct xtas, fpct totp)
bomilkar 0:5c7d59862c77 91 {
bomilkar 0:5c7d59862c77 92 char nmea_string[100];
bomilkar 0:5c7d59862c77 93
bomilkar 0:5c7d59862c77 94 sprintf(nmea_string, "$POV,E,%.2f,S,%.2f,P,%.2f,Q,%.2f,R,%.2f",
bomilkar 0:5c7d59862c77 95 xv,xtas,stat,dynp,totp);
bomilkar 0:5c7d59862c77 96
bomilkar 0:5c7d59862c77 97 channel->printf("%s*%02X\n", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
bomilkar 0:5c7d59862c77 98 }
bomilkar 0:5c7d59862c77 99
bomilkar 0:5c7d59862c77 100 void avs_c::nmea_out(char *buf, fpct xv, fpct stat, fpct dynp, fpct xtas)
bomilkar 0:5c7d59862c77 101 {
bomilkar 0:5c7d59862c77 102 char nmea_string[100];
bomilkar 0:5c7d59862c77 103
bomilkar 0:5c7d59862c77 104 sprintf(nmea_string, "$POV,E,%04.2f,S,%04.2f,P,%04.2f,Q,%04.2f",
bomilkar 0:5c7d59862c77 105 xv,xtas,stat,dynp);
bomilkar 0:5c7d59862c77 106
bomilkar 0:5c7d59862c77 107 sprintf(buf,"%s*%02X", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
bomilkar 0:5c7d59862c77 108 }
bomilkar 0:5c7d59862c77 109
bomilkar 0:5c7d59862c77 110 void avs_c::nmea_out(Serial *channel, fpct xv, fpct stat, fpct dynp, fpct xtas)
bomilkar 0:5c7d59862c77 111 {
bomilkar 0:5c7d59862c77 112 char nmea_string[100];
bomilkar 0:5c7d59862c77 113
bomilkar 0:5c7d59862c77 114 sprintf(nmea_string, "$POV,E,%.2f,S,%.2f,P,%.2f,Q,%.2f",
bomilkar 0:5c7d59862c77 115 xv,xtas,stat,dynp);
bomilkar 0:5c7d59862c77 116
bomilkar 0:5c7d59862c77 117 channel->printf("%s*%02X\n", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
bomilkar 0:5c7d59862c77 118 }
bomilkar 0:5c7d59862c77 119
bomilkar 0:5c7d59862c77 120 void avs_c::nmea_out(char *buf, fpct xv, fpct stat, fpct dynp)
bomilkar 0:5c7d59862c77 121 {
bomilkar 0:5c7d59862c77 122 char nmea_string[100];
bomilkar 0:5c7d59862c77 123
bomilkar 0:5c7d59862c77 124 sprintf(nmea_string, "$POV,E,%04.2f,P,%04.2f,Q,%04.2f",
bomilkar 0:5c7d59862c77 125 xv,stat,dynp);
bomilkar 0:5c7d59862c77 126
bomilkar 0:5c7d59862c77 127 sprintf(buf,"%s*%02X", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
bomilkar 0:5c7d59862c77 128 }
bomilkar 0:5c7d59862c77 129
bomilkar 0:5c7d59862c77 130 void avs_c::nmea_out(Serial *channel, fpct xv, fpct stat, fpct dynp)
bomilkar 0:5c7d59862c77 131 {
bomilkar 0:5c7d59862c77 132 char nmea_string[100];
bomilkar 0:5c7d59862c77 133
bomilkar 0:5c7d59862c77 134 sprintf(nmea_string, "$POV,E,%.2f,P,%.2f,Q,%.2f",
bomilkar 0:5c7d59862c77 135 xv,stat,dynp);
bomilkar 0:5c7d59862c77 136
bomilkar 0:5c7d59862c77 137 channel->printf("%s*%02X\n", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
bomilkar 0:5c7d59862c77 138 }
bomilkar 0:5c7d59862c77 139
bomilkar 0:5c7d59862c77 140 void avs_c::nmea_alert(Serial *channel, Severity lev, char * message)
bomilkar 0:5c7d59862c77 141 {
bomilkar 0:5c7d59862c77 142 char nmea_string[100], al_char;
bomilkar 0:5c7d59862c77 143
bomilkar 0:5c7d59862c77 144 switch (lev)
bomilkar 0:5c7d59862c77 145 {
bomilkar 0:5c7d59862c77 146 case warning : al_char = 'W'; break;
bomilkar 0:5c7d59862c77 147 case err : al_char = 'E'; break;
bomilkar 0:5c7d59862c77 148 case failure : al_char = 'F'; break;
bomilkar 0:5c7d59862c77 149 default : al_char = 'I'; break;
bomilkar 0:5c7d59862c77 150 }
bomilkar 0:5c7d59862c77 151
bomilkar 0:5c7d59862c77 152 sprintf(nmea_string, "$POV,C,AL,%c,%s",al_char,message);
bomilkar 0:5c7d59862c77 153
bomilkar 0:5c7d59862c77 154 channel->printf("%s*%02X\n", nmea_string,nmea_checksum(nmea_string+1)); // with checksum
bomilkar 0:5c7d59862c77 155 }
bomilkar 0:5c7d59862c77 156 #endif
bomilkar 0:5c7d59862c77 157
bomilkar 0:5c7d59862c77 158