Add an altimeter and VSI to the Airball display. We do not yet have a

way to change the altimeter setting.

host/display/aerodynamics.cpp

@@ -49,4 +49,25 @@ double q_to_tas(double q, double p, double t) {
   return sqrt(2.0 * q / dry_air_density(p, t));
 }
 
+static constexpr double kAltK = 1.313e-05;
+static constexpr double kAltN = 0.1903;
+static constexpr double kAltPb = 29.92126;
+
+// https://www.av8n.com/physics/altimetry.htm
+// The formulae are given in English units. To maintain a consistent API
+// throughout our system, the function inputs and outputs are in SI units. The
+// tradeoff is we do a few more unit conversions than needed, in return for a
+// hopefully less error-prone set of APIs.
+double pressure_to_altitude(double p, double qnh) {
+  double p_inHg = p / kPascalsPerInHg;
+  double pS_inHg = qnh / kPascalsPerInHg;
+  double h_feet =
+      (pow(pS_inHg, kAltN) / kAltK) *
+      (
+          pow(pS_inHg / kAltPb, kAltN) -
+          pow(p_inHg / kAltPb, kAltN)
+      );
+  return kMetersPerFoot * h_feet;
+}
+
 } // namespace airball

host/display/aerodynamics.h

@@ -62,6 +62,15 @@ double dry_air_density(double p, double t);
  */
 double q_to_tas(double q, double p, double t);
 
+/**
+ * Compute the altitude given the barometric pressure.
+ *
+ * @param p barometric pressure at this point.
+ * @param qnh barometer setting at this point (pressure at sea level).
+ * @return altitude.
+ */
+double pressure_to_altitude(double p, double qnh);
+
 } // namespace airball
 
 #endif // AIRBALL_AERODYNAMICS_H

host/display/airdata.cpp

@@ -25,6 +25,7 @@
 #include "airdata.h"
 
 #include <iostream>
+#include <cstring>
 
 #include "aerodynamics.h"
 #include "units.h"
@@ -78,6 +79,7 @@ double find_dpr_to_angle(InterpolationTable& table, double dpr) {
 
 Airdata::Airdata() {
   populate_table(dpr_to_angle);
+  memset(climb_rate_buffer_, 0, sizeof(climb_rate_buffer_));
 }
 
 void Airdata::update(const airdata_sample* d) {
@@ -88,6 +90,18 @@ void Airdata::update(const airdata_sample* d) {
       single_point_sphere_pressure_coefficient(total_angle);
   ias_ = q_to_ias(free_stream_q_);
   tas_ = q_to_tas(free_stream_q_, d->get_baro(), d->get_temperature());
+  double new_altitude = pressure_to_altitude(d->get_baro(), 101300 /* Pascals */);
+  for (int i = 1; i < kClimbRatePoints; i++) {
+    climb_rate_buffer_[i - 1] = climb_rate_buffer_[i];
+  }
+  climb_rate_buffer_[kClimbRatePoints - 1] = new_altitude - altitude_;
+  altitude_ = new_altitude;
+  climb_rate_ = 0;
+  for (int i = 0; i < kClimbRatePoints; i++) {
+    climb_rate_ += climb_rate_buffer_[i];
+  }
+  climb_rate_ /= (double) kClimbRatePoints;
+  climb_rate_ *= kSamplesPerMinute;
   valid_ = !isnan(alpha_) && !isnan(beta_);
 }
 

host/display/airdata.h

@@ -50,19 +50,32 @@ class Airdata {
   // Free stream dynamic pressure
   double free_stream_q() const {return free_stream_q_;}
 
+  // Current altitude (TODO: No barometer setting yet)
+  double altitude() const { return altitude_; }
+
+  // Current climb rate
+  double climb_rate() const { return climb_rate_; }
+
   // Returns true if the data is valid. If not, display a red X indicating system failure.
   bool valid() const {return valid_;}
 
   // Commands this model to update its contents based on the given sensor data.
   void update(const airdata_sample* d);
+
 private:
+  static constexpr int kClimbRatePoints = 5;
+  static constexpr int kSamplesPerMinute = 20 * 60;
+
   InterpolationTable dpr_to_angle;
   double ias_;
   double tas_;
   double alpha_;
   double beta_;
   double free_stream_q_;
+  double altitude_;
+  double climb_rate_;
   bool valid_;
+  double climb_rate_buffer_[kClimbRatePoints];
 };
 
 } // namespace airball