4545// driver
4646#include "vti_ps.h"
4747#include "timer.h"
48+ #ifdef FIXEDPOINT
49+ #include "dsp.h"
50+ #endif
4851
4952
5053// *************************************************************************************************
@@ -62,10 +65,18 @@ void twi_delay(void);
6265// *************************************************************************************************
6366// Global Variable section
6467
68+ #ifndef FIXEDPOINT
6569// VTI pressure (hPa) to altitude (m) conversion tables
6670const s16 h0 [17 ] = { -153 , 0 , 111 , 540 , 989 , 1457 , 1949 , 2466 , 3012 , 3591 , 4206 , 4865 , 5574 , 6344 , 7185 , 8117 , 9164 };
6771const u16 p0 [17 ] = { 1031 , 1013 , 1000 , 950 , 900 , 850 , 800 , 750 , 700 , 650 , 600 , 550 , 500 , 450 , 400 , 350 , 300 };
6872float p [17 ];
73+ #else
74+ // Storage for pressure to altitude conversions
75+ static s16 pLast ; // Last measured pressure in 4Pa units
76+ static s16 pRef ; // Reference pressure at sea level in 4Pa units
77+ static s16 hLast ; // Last altitude estimate in normalized units b/H0/2^15
78+ #endif
79+
6980
7081// Global flag for proper pressure sensor operation
7182u8 ps_ok ;
@@ -435,7 +446,6 @@ u16 ps_get_temp(void)
435446 return (kelvin );
436447}
437448
438-
439449// *************************************************************************************************
440450// @fn init_pressure_table
441451// @brief Init pressure table with constants
@@ -444,11 +454,67 @@ u16 ps_get_temp(void)
444454// *************************************************************************************************
445455void init_pressure_table (void )
446456{
457+ #ifndef FIXEDPOINT
447458 u8 i ;
448459
449460 for (i = 0 ; i < 17 ; i ++ ) p [i ] = p0 [i ];
461+ #else
462+ pLast = 101325 /4 ; // Last measured pressure in 4Pa units
463+ pRef = 101325 /4 ; // Reference pressure at sea level in 4Pa units
464+ hLast = 0 ;
465+ #endif
466+ }
467+
468+ #ifdef FIXEDPOINT
469+ // *************************************************************************************************
470+ // @fn conv_altitude_to_fraction
471+ // @brief Relative pressure deviation from reference pressure for given altitude estimate.
472+ // @param s16 hh Altitude estimate (in normalised units).
473+ // @return Calculated relative pressure deviation for this altitude
474+ // *************************************************************************************************
475+ s16 conv_altitude_to_fraction (s16 hh )
476+ {
477+ /*
478+ The fixed part of the function of altitude can be broken into tabulated ranges
479+ and/or interpolated according to a Taylor series expansion
480+ (1 - f) = (1 h/H0)^b
481+ = 1 - h*b/H0 + h^2*b*(b1)/2/H0^2 h^3*b8(b1)*(b-2)/6/H0^3 +
482+ At low altitudes h/H0 << 1, so this series tends to converge rapidly and is
483+ well-suited for fixed point implementation. With one or two additional terms
484+ the series converges accurately over the range of interest so there is no need
485+ for table interpolation. For the proposed fixed point implementation we rewrite
486+ this expression a bit into
487+ hh = b*h/H0
488+ (1 - f) = (1 h/H0)^b
489+ = 1 - hh*(1 hh*(b1)/2/b*(1 hh*(b2)/3/b*(...
490+ We stick to integer multiply and shift operations. Signed s16 values can contain
491+ values +/2^15 and unsigned u16 values 0..2^16. In C multiplication amounts to
492+ expanding to s32, integer multiply and scaling back by a proper shift operation.
493+
494+ Given the above equations the natural unit of hh as the first order correction is
495+ H0/b = 8434.48m. If we accept this as a maximum +/ range we can store s16 hh in
496+ units of (H0/b)/2^15 = 0,26m which keeps the resolution at less than a foot.
497+ */
498+ s16 f , hf ;
499+ // f = hh*(b 4)/5/b, correction relevant above 3.5km:
500+ // (Could be omitted, but it is relatively little work.)
501+ f = mult_scale16 (hh , 3132 );
502+ // f = hh*(b 3)/4/b*(1 - f), correction relevant above 1.3km:
503+ hf = mult_scale16 (hh , 7032 );
504+ f = hf - mult_scale15 (hf ,f );
505+ // f = hh*(b 2)/3/b*(1 - f), correction relevant above 300m:
506+ hf = mult_scale16 (hh , 13533 );
507+ f = hf - mult_scale15 (hf ,f );
508+ // f = hh*(b 1)/2/b*(1 - f), correction relevant above 30m:
509+ hf = mult_scale16 (hh , 26533 );
510+ f = hf - mult_scale15 (hf ,f );
511+ // f = hh*(1 - f), the linear part:
512+ f = hh - mult_scale15 (hh ,f );
513+ return f ;
450514}
451515
516+ #endif // FIXEDPOINT
517+
452518
453519// *************************************************************************************************
454520// @fn update_pressure_table
@@ -461,6 +527,7 @@ void init_pressure_table(void)
461527// *************************************************************************************************
462528void update_pressure_table (s16 href , u32 p_meas , u16 t_meas )
463529{
530+ #ifndef FIXEDPOINT
464531 const float Invt00 = 0.003470415 ;
465532 const float coefp = 0.00006 ;
466533 volatile float p_fact ;
@@ -495,9 +562,29 @@ void update_pressure_table(s16 href, u32 p_meas, u16 t_meas)
495562 {
496563 p [i ] = p0 [i ]* p_fact ;
497564 }
565+ #else
566+ // Note: a user-provided sea-level reference pressure in mbar as used by pilots
567+ // would be straightforward: href = 0; p_meas = (s32)mbar*100;
568+ // The altitude reading will be iteratively updated.
569+
570+ // Convert to 4Pa units:
571+ pLast = (s16 )((p_meas + 2 ) >> 2 );
572+ // Convert reference altitude to normalized units:
573+ if (sys .flag .use_metric_units ) { // user_altitude in m
574+ hLast = 4 * href - mult_scale16 (href , 7536 );
575+ } else { // user_altitude in ft
576+ hLast = href + mult_scale16 (href ,12068 );
577+ }
578+ s32 f = (s32 )0x8000 - conv_altitude_to_fraction (hLast );
579+ // pRef = p_meas*2^15/f:
580+ pRef = ((((s32 )pLast << 16 ) + f ) >> 1 ) / f ;
581+ // The long division is acceptable because it happens rarely.
582+ // The term + f) is for proper rounding.
583+ // The <<16 and >>1 operations correct for the 15bit scale of f.
584+ #endif
498585}
499586
500-
587+ #ifndef FIXEDPOINT
501588// *************************************************************************************************
502589// @fn conv_pa_to_meter
503590// @brief Convert pressure (Pa) to altitude (m) using a conversion table
@@ -551,3 +638,89 @@ s16 conv_pa_to_meter(u32 p_meas, u16 t_meas)
551638
552639 return (h );
553640}
641+ #else
642+ // *************************************************************************************************
643+ // @fn conv_pressure_to_altitude
644+ // @brief Calculates altitude from current pressure, and
645+ // stored reference pressure at sea level and previous altitude estimate.
646+ // Temperature info is ignored.
647+ // @param u32 p_meas Pressure (Pa)
648+ // @param u16 t_meas Temperature (10*°K) Ignored !!!
649+ // @return Estimated altitude in user-selected unit (m or ft)
650+ // (internally filtered, slightly sluggish).
651+ // *************************************************************************************************
652+ s16 conv_pa_to_altitude (u32 p_meas , u16 t_meas )
653+ {
654+ /*
655+ Assumption: fixed, linear T(h)
656+ T = T0 dTdh*h
657+ with
658+ T0 = 288.15K (15C)
659+ dTdh = 6.5mK/m
660+
661+ Basic differential equation:
662+ dh = -(R/G)*T(H)*dp/p
663+ Solution:
664+ H = H0*(1 (p/pRef)^a)
665+ with
666+ H0 = T0/dTdh = 44330.77m
667+ pRef = adjustable reference pressure at sea level (h=0).
668+ a = dTdH*R/G = 0.190263
669+ R = 287.052m^2/s^2/K
670+ G = 9.80665 (at medium latitude)
671+
672+ We assume T0 and the temperature profile to be fixed; the temperature reading
673+ of the watch is not very useful since it is strongly influenced by body heat,
674+ clothing, shelter, etc.
675+
676+ Straight evaluation of h(p) requires an unattractive long division p/pRef
677+ with pRef the adjustable reference pressure, and the Taylor expansion does
678+ not converge very quickly.
679+
680+ Evaluation of p(h) requires a more attractive multiplication by the
681+ user-adjustable reference pressure pRef:
682+ f =(1 h/H0)^b
683+ p = pRef*f
684+ with
685+ b = 1/a = G/(dTdH*R) = 5.255896
686+ In a very crude linear iteration the h value can be updated by
687+ delta_h = delta_p / dpdh
688+ The slope dpdh varies by about a factor two over the range of interest,
689+ but we can pick a fixed value on the safe side and accept that the updates
690+ are a bit more damped at higher altitudes.
691+
692+ The sensor provides 19bit absolute pressure in units of 0.25Pa, but that is more
693+ resolution than we can easily handle in the multiplications. We store measured
694+ pressure p, reference pressure pRef and calculated pressure as u16 in units of 4Pa.
695+
696+ In the units chosen for p (4Pa) and for hLast (see function conv_altitude_to_fraction),
697+ the slope dpdh is about -0.75 at sea level down to -0.375 at high altitudes. To avoid
698+ overshoot and instabilities we assume a bigger value and accept a minor amount of
699+ extra filtering delay at higher altitudes. The factor 1/0.75 is approximated by 1.
700+ */
701+ // Scale to 4Pa units:
702+ s16 p = (s16 )((p_meas + 2 ) >> 2 );
703+ // Predictor to speed up response to pressure changes:
704+ // hLast -= p - pLast; // Factor of about 1/0.75 would be better.
705+ // Store current pressure for next predictor:
706+ pLast = p ;
707+ // Calculate pressure ratio based on guessed altitude (serious DSP work):
708+ s16 f = conv_altitude_to_fraction (hLast );
709+ // Calculate pressure expected for guessed height
710+ u16 pCalculated = pRef - mult_scale15 (pRef ,f );
711+ // This calculation is correct within about 7Pa.
712+ // We still have to reverse the solution with a linearly improved guess:
713+ hLast -= p - pCalculated ;
714+ // Iteration gain factor of about 1/0.75 would result in faster convergence,
715+ // but even the big initial jump when the altimeter is switched on converges
716+ // in some 5 or 6 steps to about 1m accuracy.
717+
718+ if (sys .flag .use_metric_units ) {
719+ // Altitude in meters (correct within about 0.7m):
720+ return mult_scale16 (hLast , 16869 );
721+ } else {
722+ // Altitude in feet (correct within 1.5ft):
723+ return mult_scale15 (hLast , 27672 );
724+ }
725+ }
726+ #endif // FIXEDPOINT
0 commit comments