diff --git a/ArduPlane/climb_rate.pde b/ArduPlane/climb_rate.pde index 644be85a34..acf3762a7f 100644 --- a/ArduPlane/climb_rate.pde +++ b/ArduPlane/climb_rate.pde @@ -3,58 +3,58 @@ #if 0 // currently unused struct DataPoint { - unsigned long x; - long y; + unsigned long x; + long y; }; -DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from -unsigned char hindex; // Index in history for the current data point +DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from +unsigned char hindex; // Index in history for the current data point -unsigned long xoffset; -unsigned char n; +unsigned long xoffset; +unsigned char n; // Intermediate variables for regression calculation -long xi; -long yi; -long xiyi; -unsigned long xi2; +long xi; +long yi; +long xiyi; +unsigned long xi2; void add_altitude_data(unsigned long xl, long y) { - //Reset the regression if our X variable overflowed - if (xl < xoffset) - n = 0; + //Reset the regression if our X variable overflowed + if (xl < xoffset) + n = 0; - //To allow calculation of sum(xi*yi), make sure X hasn't exceeded 2^32/2^15/length - if (xl - xoffset > 131072/ALTITUDE_HISTORY_LENGTH) - n = 0; + //To allow calculation of sum(xi*yi), make sure X hasn't exceeded 2^32/2^15/length + if (xl - xoffset > 131072/ALTITUDE_HISTORY_LENGTH) + n = 0; - if (n == ALTITUDE_HISTORY_LENGTH) { - xi -= history[hindex].x; - yi -= history[hindex].y; - xiyi -= (long)history[hindex].x * history[hindex].y; - xi2 -= history[hindex].x * history[hindex].x; - } else { - if (n == 0) { - xoffset = xl; - xi = 0; - yi = 0; - xiyi = 0; - xi2 = 0; - } - n++; - } + if (n == ALTITUDE_HISTORY_LENGTH) { + xi -= history[hindex].x; + yi -= history[hindex].y; + xiyi -= (long)history[hindex].x * history[hindex].y; + xi2 -= history[hindex].x * history[hindex].x; + } else { + if (n == 0) { + xoffset = xl; + xi = 0; + yi = 0; + xiyi = 0; + xi2 = 0; + } + n++; + } - history[hindex].x = xl - xoffset; - history[hindex].y = y; + history[hindex].x = xl - xoffset; + history[hindex].y = y; - xi += history[hindex].x; - yi += history[hindex].y; - xiyi += (long)history[hindex].x * history[hindex].y; - xi2 += history[hindex].x * history[hindex].x; + xi += history[hindex].x; + yi += history[hindex].y; + xiyi += (long)history[hindex].x * history[hindex].y; + xi2 += history[hindex].x * history[hindex].x; - if (++hindex >= ALTITUDE_HISTORY_LENGTH) - hindex = 0; + if (++hindex >= ALTITUDE_HISTORY_LENGTH) + hindex = 0; } #endif