# High Altitude Apogee Detection (100K ft)

### Help Support The Rocketry Forum:

#### AllDigital

##### Well-Known Member
Not necessarily on tilt. If the IM7180 is using the gravity reference vector to correct gyro drift (as most IMU chips do by default) then you can have an indeterminate error on the tilt reading.
My EM7180 does use the accelerometer and the magnetometer to correct the gyro and is using some variant of a madgwick filter (as far as I can tell). It has a configurable parameter to control accelerometer latency that is used to "mask shocks for more accurate inertial navigation". I have that parameter set, but not sure if that is helping or not. I've done about 18-20 flights with the EM7180 (>10K') and have never seen any anomalies, drift, or errors with my tilt values coming from the quaternion output. I've got a lot of raw data and onboard video to confirm.

Would the gravity reference be more of an issue if I exceeded the 16g limit on the accelerometer?

#### jderimig

Would the gravity reference be more of an issue if I exceeded the 16g limit on the accelerometer?
No, application of the gravity reference using the Madgwick filter is wrong for a rocket flight under any acceleration (because there is no gravity reference).

In a rocket flight there will be a z-acceleration from the rocket thrust and drag after motor burnout. This will be interpreted by the filter as the z-gravity reference which is profoundly incorrect. The likely effect would be an underestimation of the tilt. The gyro say the rocket is rotating but the faux gravity reference says it isn't. How the EM7180 code handles this conflict is unknown. Perhaps the EM7180 drops the gravity reference component when the acceleration is not -1g. However guessing on what the black box IMU is doing is not a proper way to develop a rocket IMU engine, in my humble opinion.

The proper configuration of the IMU processor is not to use the gravity reference in flight at all (infinite latency) and just simply integrate the gyro rates.

#### strud

##### Active Member
There are two 'near absolute' references you can use for the pitch and yaw gyro integration results, those being magnetic field vector and/or horizon detected using differential far infra-red sensors such as these (thermopiles) :
https://www.melexis.com/en/product/...ermopile-Sensor-High-Temperature-Measurements

I have data from a few flights comparing the magnetic field vector and the integrated rate gyro results, but that was a while ago and would like to utilise a Kalman filter to fuse all three, however roll would need to be 'under control' otherwise it would be near impossible.

Test flight of attitude & location control system May 29, WWW 2015

#### jderimig

There are two 'near absolute' references you can use for the pitch and yaw gyro integration results, those being magnetic field vector and/or horizon detected using differential far infra-red sensors such as these (thermopiles) :
The Madgwick paper cited above includes magnetic field compensation for the attitude filter. You can start there.

One implementation uses the earth magnetic field components in x and y as the earth frame x,y. The problem to get around in practice is the hard iron distortion or shift as the rocket leaves the earth and possible ferrous launch pad. Does the filter interpret that as a gyro drift or actual tilt? Do you turn on magnetic field compensation and zero after you left the pad?

For a rocket flight, today's gyro's drifts rates are very small relative to the duration of the event. The one on my desk drifts less than 1 degree in 2minutes.

#### strud

##### Active Member
For a rocket flight, today's gyro's drifts rates are very small relative to the duration of the event. The one on my desk drifts less than 1 degree in 2minutes.
Sounds great, are you happy to share the part number?

However, if you are trying to determine the attitude of the vehicle relative to the earth frame for the case of apogee detection (one possible metric) , the magnetic field vector is a good one. Although many have stated for VERY high flights (thin atmosphere) one cannot assume a horizontal attitude represents the apogee event.....

#### jderimig

Sounds great, are you happy to share the part number?

However, if you are trying to determine the attitude of the vehicle relative to the earth frame for the case of apogee detection (one possible metric) , the magnetic field vector is a good one. Although many have stated for VERY high flights (thin atmosphere) one cannot assume a horizontal attitude represents the apogee event.....
As you mentioned attitude is not a reliable metric for apogee detection. Jim Jarvis had a Balls flight where where the airframe tumbled for ~30K feet on the way up if I remember correctly. Rockets that go significantly above 100K are apt to tumble at some point.

The gyro on my desk is an ST L3GD20H. However it is getting long in the tooth and there are better ones. The ST ISM330 series outperforms that as well as the TDK (formerly Invensense) ICD-20601 looks very nice as well. However the latter 2 are 6dof units where the L3GS20H is just a 3 axis gyro.

#### AllDigital

##### Well-Known Member
Ok, I got data this weekend from a launch, but it just reinforced how bad my apogee detecting skills are using velocity.

Here is the background on the launch and the setup. I launched a 3” rocket on a 5 grain KNSB motor to 12,100’ AGL. Overall, a good flight and good recovery. The top speed was 1415 fps (994mph). Apogee was about 750 meters (2460’) from launch and landing was about 1.2km from launch. On board we have our own DIY flight computer with GPS (Ublox-M8), an EM7180/MPU9250 16G accelerometer, an ADXL375 200G accelerometer, BME280 baro, SD Card, 435Mhz TTY radio, and and ESP32 running the show. During the launch phase of flight data is logged at 20Hz to the SD card, but both accelerometers are sampling and integrating at 500Hz. The EM7180 is a “black box” co-processor that is filtering and integrating data off of the MPU9250 and outputs raw, linear (filtered w/gravity removed), and filtered quaternions. I am only pulling the accelerometer and quat data, as pulling the gyro and mag data requires more cycles. I also have two commercial altimeters (a Stratalogger and an RRC3) on board for deployment events.

For this flight, my launch detection was perfect and declared launch within 150ms and I pre-captured accelerometer data into integrated velocity during the confirm period. Apogee on this flight happened at 29.416 seconds, as confirmed by the barometer data and the commercial altimeter deployment (I record aft separation with hall sensors). On the other hand, my normal barometer apogee detection was woefully off at 25.164 seconds (4 seconds early and 230’ low), due to tilt indicators showing 180 degrees and baro seeing a single read <20’ from max (more on that below).

The objective of this flight was to capture accelerometer data and see how integrated velocity using the accelerometer z axis (less gravity) tracked to apogee. Here are a few major observations and questions:

1. Did we really flip over at 10,000 feet? In the first 15 seconds of flight our tilt indicator, coming from the EM7138 board (pitch/roll quat data), indicated we were drifting from 10 degrees to 40 degrees, but when we hit 10,000 feet the indicator says we went full 180 degrees upside down while climbing the last ten seconds and 2000 feet. This rocket is overstable, so it is plausible to me that it might have flipped over in high winds, but I’m also doubting the data from the EM7180 after feedback from John. The rocket on the pad is 3.5 caliber stable and when all the fuel is burned it is 5.75 stable, so a big gust at 10K feet (actually 13K ASL), might flip it over (?). That said, the raw Z data from both accelerometers did not seem to flip, only the “linear Z” data from the EM7180, which uses their kalman filters and feeds the quaternion output. Also, no camera on this flight, but previous flights with cameras correlate to the values I get from the tilt sensor.

2. Significantly Different Readings: We got significantly different readings between the two accelerometers. The EM7180 is mounted with Z up, so all the samples and processing logic are standard. The ADXL375 is mounted with the -Y up, so to get “Z” we multiply Y * -1, and to get “linear Z” (crude version G removed) we just subtract 1g (1000mg) from that. We are sampling/integrating and logging at different rates (500Hz vs 20Hz), so we also capture max, min, and average during the logging cycles. For this launch, we can see that the EM7180 with an MTU9250 maxed out at launch (16g), but the ADXL375 only reported a max of 11g. Also, the ADXL accel data looks like a big “W” during the initial motor burn. I figured it was just anomalous readings, but the average readings also exhibit the “W” shape (see photos). The max peak and the drop to negative look normal and consistent with the EM7180, but how to explain the W shape during burn?

3. Velocity Integration was way off: On this flight we were integrating velocity by using the “linear Z” (gravity removed Z value) for both accelerometers, using the formula v += (z accel) x (rate), where rate = (current microseconds - last microseconds) / 1000000. We also start integrating once z > .2g, before launch and we zero out any false starts. The pre-capture worked well. Velocity is being integrated at 500hz and values logged to SD at 20hz. For this launch, velocity started off looking good for both accelerometers. Velocity between the two peaked within the same milliseconds and started to decline, as expected. But, the ADXL declined rapidly and crossed zero at 9.4 seconds (almost 20 seconds before apogee), while the EM7180 was on a nice slope down when it turned back up to positive around 14 seconds (correlating with the reported 180 degree flip in #1 above). The EM7180 never reached negative. I recognize that the EM7180 data was already compromised when the launch maxed out the Z axis, but then it was compromised again with the rotation (or presumed rotation). Also of interest, the trend lines for the raw z of the EM7180 and the “linear Z” of the ADXL both look directionally correct, as they drop negative and come back to near zero at apogee.

So, I’m not sure if I actually flipped over the last 2000 feet or if I can trust the EM7180 for tilt (pitch/roll) data, but this is a better (sorry, longer) explanation of why I’ve had challenges in the past integrating velocity to get apogee. I’m not flying anything to 100K feet for now, but I’d like to develop a solution that doesn’t completely depend on the barometer now, so when we do it is in the bag. If the accelerometer alone won’t cut it, we can integrate early barometer data with time calculations, and/or GPS data at the top. My GPS has been reliably coming back before or at apogee, so that is encouraging.

Below are a few graphs and here is a link to a spreadsheet with raw data. One last note on the data, after we (think) we detect apogee, we switch to logging every second (1hz), so on this flight the resolution drops considerably in the last three seconds, due to the early apogee call from our tilt logic.

-Mike

Data export in Excel format: https://www.dropbox.com/s/zu1ddcx1g9nr3iz/21Nov Export.xlsx?dl=0

#### jderimig

In you excel file what are the units of the ADXL Y data?

Do you have a sim of your flight?
Motor used?
Rocket weight at launch?

This information should be use to cross validate your acceleration values, at least for the initial few seconds of flight.

Last edited:

#### jderimig

Your ADXL375 data looks bogus. Your acceleration curve should resemble your motor thrust curve. Unless you motor has severe combustion instability which would have made an unmistakable sound.

So we probably need to look at your code and possibly how you configured the ADXL375. Are you sure you don't have any high pass filters enabled on the ADXL375?

Last edited:

#### UhClem

##### Well-Known Member
but how to explain the W shape during burn?
It is almost certainly an artifact of your data processing. Exactly what I can't even guess at without seeing some code. (Details matter.)

#### mikec

##### Well-Known Member
It is almost certainly an artifact of your data processing.
Agreed, looks like bit wraparound, confusion about word width or scale factors, or something similar.

#### AllDigital

##### Well-Known Member
In you excel file what are the units of the ADXL Y data?
Do you have a sim of your flight?
Motor used?
Rocket weight at launch?
The units in the Excel file for all the accelerometer readings are in milli-G (1000 = 1G).
Here is a link to a folder with my Rocksim file and engine file.
The motor was my own EX motor, a 75mm 5 grain KNSB sugar motor in a simple bates configuration. I've flown this one about 5 times and static tested 4 times. All very clean and consistent. The burn profile for the motor is a standard bates peak in the middle.
The Sim and the actual flight were almost identical and everything looked super clean going off the pad. Nothing that would indicate anomalies in the flight or the motor.
The rocket weighs 21 lbs loaded on the rail and burns off 8.3 lbs of propellent in 2.9 seconds.

Your ADXL375 data looks bogus. Your acceleration curve should resemble your motor thrust curve. Unless you motor has severe combustion instability which would have made an unmistakable sound.
Agree. Even though the EM7180 maxed out at launch, it seemed to capture the expected burn profile, while the ADXL data looked bogus during thrust. There is no way the motor produced that crazy W shape that I can conceive, based on all the static testing and launching I've done with it.

So, I just re-reviewed the ADXL375 data sheet, my library, and code and I do not see anything obvious. This accelerometer is pretty stripped down, as it doesn't seem to have a lot of configurable filters or options. It does have a "shock detection" option that I am not using (turned off) and it references a filter in the logic block diagram, but offers no configuration for it. Here is the data sheet:

My code and the library are not doing much with the ADXL. I'm setting the rate, initializing the sensor on I2C, and I start reading x,y,z values. My code uses ADXL sample code to read the registers convert to x,y,z and then I am correcting for calibration in my code and then deriving "linear Z" with a simple LinZ = (Y * -1) - 1000, since it it sitting -Y up. I can post the code snippets if that would be helpful.

Thanks,

Mike

Motor burn profile:

#### mikec

##### Well-Known Member
Post your code snippets, I'm guessing it has something to do with how you're gluing the two bytes of each sample together. Are you using a multi-byte read to get all parts of each sample at the same time?

I assume you've looked at

#### jderimig

My code and the library are not doing much with the ADXL. I'm setting the rate, initializing the sensor on I2C, and I start reading x,y,z values. My code uses ADXL sample code to read the registers convert to x,y,z and then I am correcting for calibration in my code and then deriving "linear Z" with a simple LinZ = (Y * -1) - 1000, since it it sitting -Y up. I can post the code snippets if that would be helpful.
2. Code that coverts ADXL register values to acceleration
As a start.

#### UhClem

##### Well-Known Member
My code and the library are not doing much with the ADXL.
There can be a vast difference between what you think you are doing and what your code actually does.

But looking at the data I converted it from mG back to ADC counts (20counts/G) and it rather suspiciously never exceeds 256. This is why I like my data raw.

#### AllDigital

##### Well-Known Member
2. Code that coverts ADXL register values to acceleration
So I just did a deep review of the ADXL375 library, my code, and I confirmed all the registry settings post startup. They all look good. I can't find the smoking gun, but my guess, based on the "W Data" at launch is that something is wrong with the two's compliment conversion with the MSB and LSB registers, so when the sensor exceeds one register worth of output the values corrupt (maybe?). Here is the code that is doing the conversion. The confirmed settings are LSB mode (right justified) with sign extension. The x,y,z are stored in six registers (2 each) with the first being the LSB and the second the MSB. Here is the conversion code:

C:
      Accel.readRegisterRegion(buffer, 0x32, 6);
x = (((int)buffer[1]) << 8) | buffer[0];
y = (((int)buffer[3])<< 8) | buffer[2];
z = (((int)buffer[5]) << 8) | buffer[4];
if (bitRead(x, 15)) { // negative number
x = ~x ; //inverting the number
x = x+B01 ;//addinf the 1 to it
x = x & 0xff;//anding with 0xff to avoid any bit overflow
x = x * -1;
}
if (bitRead(y, 15)) { // negative number
y = ~y ; //inverting the number
y = y+B01 ;//addinf the 1 to it
y = y & 0xff;//anding with 0xff to avoid any bit overflow
y = y * -1;
}
if (bitRead(z, 15)) { // negative number
z = ~z ; //inverting the number
z = z+B01 ;//addinf the 1 to it
z = z & 0xff;//anding with 0xff to avoid any bit overflow
z = z * -1;
}
Here are my readings of all the ADXL375 registers post startup, confirming configuration:

Here is a link to a folder with the library I am using (ADXL375I2C.h and .app):
https://www.dropbox.com/sh/9ffsruvb9i1pcfq/AAAcbENHpDDl7rMoc2CVatL7a?dl=0

Here is all the code from my program related to the ADXL375, including the conversion to G's:

C-like:
//ADXL375 Accelerometer

int x, y, z;
Accel.settings.commInterface = I2C_MODE;
if (Accel.beginI2C() == false) { //Begin communication over I2C
working.error = 1;
} else {
}
Accel.startup();

// Important note:  Mounted with y- up, so LinZ = (y + 1000) * -1
uint8_t buffer[6];
uint8_t fifoCount = Accel.getFIFOBufferSize();
if (fifoCount > 0) {
x = (((int)buffer[1]) << 8) | buffer[0];
y = (((int)buffer[3])<< 8) | buffer[2];
z = (((int)buffer[5]) << 8) | buffer[4];
if (bitRead(x, 15)) { // negative number
x = ~x ; //inverting the number
x = x+B01 ;//addinf the 1 to it
x = x & 0xff;//anding with 0xff to avoid any bit overflow
x = x * -1;
}
if (bitRead(y, 15)) { // negative number
y = ~y ; //inverting the number
y = y+B01 ;//addinf the 1 to it
y = y & 0xff;//anding with 0xff to avoid any bit overflow
y = y * -1;
}
if (bitRead(z, 15)) { // negative number
z = ~z ; //inverting the number
z = z+B01 ;//addinf the 1 to it
z = z & 0xff;//anding with 0xff to avoid any bit overflow
z = z * -1;
}

//Calibration adjustment and scale (20.408 = 1G, so x49 = 1000mg)
adxl.x = ((float)x - 6.67) * 49;
adxl.y = ((float)y - 12.35) * 49;
adxl.z = ((float)z + 5.96) * 49;
// velocity stuff here
if (working.launch == 0 && adxl.linZ > 500) { //pre launch integration (only .5g for the big guy)
float rateD = 0.0;
rateD = 0.002;
} else {
rateD = (float)(micros() - adxl.last) / (float)1000000.0;
}
}
if (working.launch == 0 && adxl.v > 0 && adxl.linZ < 500) {  //clear any false starts
}
if (working.launch == 1) {
float rateD = 0.0;
rateD = 1/500;
} else {
rateD = (float)(micros() - adxl.last) / (float)1000000.0;
}

//detect apogee
}
} //end launch = 1

} //fifo count
} //adxl timer
Thanks, Mike

#### UhClem

##### Well-Known Member
There it is.

The data is in two's complement so the most you need to do is copy bit 15 to the higher positions. But that is a wasted effort. Better by far is to use the stdint.h type "int16_t". Type "int" can vary depending on the system but this will always be what you want: "x = (((int16_t)buffer[1]) << 8) | buffer[0];"

Worse of course is that bit wise and with 0xff throwing away all but the lower 8 bits of the result.

#### AllDigital

##### Well-Known Member
There it is.
Ah, Perfect. Thanks for the clarification. I need to brush up on my two's complement handling. That will cut a lot of code down to just a handful of lines. It also explains why I didn't see the anomaly in my "backyard tests", since I didn't get anywhere near the same G's.

So, that solves one mystery (the ADXL erroneous data). I'd still like to figure out if the EM7180 yaw, pitch,roll data is accurate, but I likely need another flight (with good ADXL to compare) and the camera running. On the next flight I'll also make sure I capture ALL the raw data - untouched by code.

#### mikec

##### Well-Known Member
If you declare x, etc, as int16_t then it's possible that no explicit cast is needed -- but given the various ways that C compilers work on machines with different integer lengths, you should test this by taking bytes of known value and insuring they convert to the values you expect on the platform you're running on.

The Sparkfun library has an explicit cast but it's in a different place than David's fragment -- *x = (int16_t)((((int)_buff[1]) << 8)|_buff[0]);. They probably both work because of type promotion but be careful.

Last edited:

#### AllDigital

##### Well-Known Member
Hi I'm back with more data. This weekend, I did a repeat of my launch from two weeks ago. I flew to 12,183 feet with the EM7180 and the ADXL375 sampling at 500hz, but logging to SD at 20hz. I also had my camera onboard during this flight. Here is what I found out:

1. The EM7180 I've been using for tilt sensing is unreliable after 14K feet ASL. The outputs from the black box quaternion filters that give me linear Z, pitch, and roll are corrupt 17 seconds into the flight. I thought this was due to an overstable rocket and tumble, but the camera on this trip verified it was straight and narrow. Also verified by the ADXL. I am 99% sure that their quat function uses an integrated barometer and I know the barometer is faulty after about 10K feet, as they only used one register instead of two to capture baro readings. I confirming this with the company/programmer, but for now the EM7180 solution is not useful to me. I like the idea of having a co-processor to free up CPU cycles, but it is time to develop my own tilt functions and ditch the black box.

2. The ADXL375 is now performing as expected, but the timing to apogee using integrated velocity was still way off. The ADXL velocity convergence with zero still had at least 5 seconds to go at apogee 25 seconds into flight. Aft separation at apogee was triggered by another altimeter, so the integration data is polluted after that. Any insight into what I might be doing wrong with velocity to be late to apogee would be appreciated.

Here is a link to the logged data in excel format. The accelerometer data is in milli-G (1000 = 1 G).

Here is a graph of the integrated velocity for both accelerometers. You can see the EM7180 goes haywire at 17 seconds, while the ADXL looks clean, but is late converging to zero at apogee. Apogee was confirmed by onboard barometer and by external (commercial) altimeter deployment event. The EM7180 also has a max of 16G, so the velocity peak and slope were lower, due to lost acceleration values.

never mind.

#### jderimig

There is about 1g discrepancy or offset from the EM raw Z acceleration and the ADXL Y acceleration. THe EM7180 values seemed to be on track to better predict apogee before it blew up.

#### UhClem

##### Well-Known Member
2. The ADXL375 is now performing as expected, but the timing to apogee using integrated velocity was still way off.
Of course it was as your code makes at least three errors:
1) The 1G offset must be measured just prior to first motion.
2) 20 counts/G is insufficient precision for the 1G offset.
3) Single precision floats have insufficient precision.

#### AllDigital

##### Well-Known Member
There is about 1g discrepancy or offset from the EM raw Z acceleration and the ADXL Y acceleration. THe EM7180 values seemed to be on track to better predict apogee before it blew up.
The EM7180 velocity data is integrated using "Linear Z" (gravity removed), but the EM7180 is using a MPU9250, so Z is capped at 16G. I assumed the missing data was why the EM7180 tracked earlier.

Here is a graph showing Linear Z comparison between the two. (blue = ADXL and Orange is the EM7180)

#### JimJarvis50

##### Well-Known Member
I suspect my response here won't be helpful to you, but in any case, your data reminds me of the issues with Raven altimeters a few years back. Many of them had a faulty accelerometer that could be calibrated at 1G, but was inaccurate at higher G's. They would typically result in a calculated apogee time that was early by 25% or so. I had one Raven that I knew did a good job of predicting accelerometer-based apogee, and I would just use a spin test to compare that altimeter against others in my stash. Sorting out the faulty ones was easy.

Jim

#### jderimig

The EM7180 velocity data is integrated using "Linear Z" (gravity removed), but the EM7180 is using a MPU9250, so Z is capped at 16G. I assumed the missing data was why the EM7180 tracked earlier.

Here is a graph showing Linear Z comparison between the two. (blue = ADXL and Orange is the EM7180)

View attachment 441614

#### AllDigital

##### Well-Known Member
Oh, I missed the "raw" part of your message. If I compare the raw Z from the EM7180 to the Y of the ADXL they seem to track pretty close. In this graph, all I did was invert the raw Y in the ADXL (because Y- is up). Where do you see the discrepancy between the two?

#### AllDigital

##### Well-Known Member
Of course it was as your code makes at least three errors:
1) The 1G offset must be measured just prior to first motion.
2) 20 counts/G is insufficient precision for the 1G offset.
3) Single precision floats have insufficient precision.

1) I start "pre integrating" velocity as soon as I see any value > .5G on the ADXL and then I zero it out if it is noise or a false alarm. I would start integrating at lower levels, but because the ADXL is a 200G accelerometer it is really noisy.
2) The ADXL375 doesn't have an option to provide lower G for higher resolution, so I am stuck with 20 counts per G. As such, it is noisy and low resolution.
3) Where do you see the single precision float? Here is the code snip for velocity integration. All should be standard 32bit floats.

rateD = (float)(micros() - adxl.last) / (float)1000000.0;

#### mikec

##### Well-Known Member
3) Where do you see the single precision float? ... All should be standard 32bit floats.
32-bit float is single-precision. I'm not sure I agree with David that this is insufficient, I used single in my code and it appeared to work OK, but I haven't done the analysis. I'm not sure what you're trying to achieve with the 1e6 scaling. Agreed that you're stuck with the 49 mg/LSB scale factor.

Code:
#define RATE 50
#define G 9.81f
float dt = 1.0f/RATE;
accel = val/256.0*G;
acceleration = accel - filtered_accel[2];
velocity += acceleration*dt;
If your code really needs to handle the case that the sample rate is variable, that adds another wrinkle because now timebase precision becomes an issue.

Last edited:

#### UhClem

##### Well-Known Member
It would appear that an example is in order:
Code:
/*
Simple demonstration of apogee detection via integrating an accelerometer.

Measures 1G offset up till launch detect. The temperature variation of modern
sensors is better than the ADXL50 but it still isn't zero. Or the same
between sensors. So it is vital to get an accurate measure of this. Any
offset errors will cause the velocity error to grow in direct proportion to
elapsed time.

While the sensitivity of the MPU9250 means that it doesn't benefit much from
averaging many samples together, it helps most accels. The ADXL375 for
example with its 20 counts/G needs it. The offset and the velocity are
in a fixed point format with 8 fractional bits.

Careful attention to the size of integers is in order. It bit me while working
on this. (See below.)

Conversion to engineering units (f/s or m/s) is not required. Use of floating
point is not recommended unless you use 64 bit floats. 32 bit floats have
insufficient precision. Adding a small number (-1G) to a large number
(velocity) is the problem. Remember that a floating point addition first
has to align the binary point which can shift the small number (or parts
of it) right out of the picture.
*/

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>

// For state machine
#define PRELAUNCH 1
#define FLIGHT 2
#define APOGEE 3

// For manipulating the fixed point format. (8 fractional digits)
#define TOFIXED(X)      (X << 8)
#define FIXEDTOFLOAT(X) (X / 256.0)

// Sample rate and sensor counts/G
#define SPS  4000
#define CPG  2048
#define BIAS (CPG/10)
/*
Read the data file returning the next sample.

Convert back to native MPU9250 integer form.
Sensitivity is 2048 counts/G.
*/

int16_t x,y,z;

void getSample(void)
{
char buf[512];
float tx, ty, tz;

if(fgets(buf, 512, stdin) == NULL)
exit(1);
sscanf(buf, "%f %f %f", &tz, &tx, &ty);

x = tx * -CPG;
y = ty * CPG;
z = tz * CPG;

}

/*
Track sensor offset.

The acceleration is averaged by adding up a number of samples. This sum
is periodically stored into a last in first out queue. The last entry
in this queue is used in the integration.

The critical thing is to make sure that there is enough time between
updates of the offset so that the initial acceleration doesn't leak into
the offset. I had to fiddle with things a bit here because of the
a three entry queue.
*/
int32_t aoffset;

void do_offset(int16_t accel)
{
static int sum = 0;
static int i = 0;
static int32_t offset[2];

// Don't forget to initialize with something reasonable for integration!
if(offset[1] == 0)
offset[0] = offset[1] = aoffset = TOFIXED(accel);

sum += accel;

if(++i == 2048)
{
aoffset   = offset[1];
offset[1] = offset[0];
offset[0] = sum >> 3;        // convert to the .8 fixed point format
sum = 0;
i = 0;
}
}

int main(int argc, char **argv)
{
int i;
int state;

/*
A 64 bit type is required here because of the high sample rate. I was
rather surprised that the default size for an integer on this Linux
box wasn't 64. So surprised that it took me a while to figure out why
velocity jumped from 10m/s to -10. :-)

Can be 32 bits with lower sample rates and sensor sensitivities. But
check!
*/
int64_t velocity = 0;

state = PRELAUNCH;
i = 0;

while(1)
{
getSample();
switch (state)
{
// Waiting for velocity to exceed 9.8 m/s.
case PRELAUNCH:
do_offset(x);
// Use a biased and floored integration
velocity += TOFIXED(x) - offset[2] - TOFIXED(BIAS);
i++;
if(velocity < 0)
{
velocity = 0;
i = 0;
}

if(velocity > TOFIXED(CPG * SPS))
{
state = FLIGHT;
/*
Remove bias from velocity. Note that the bias remains in the
data output prior to this point.
*/
velocity += i * TOFIXED(BIAS);

printf("launched!\n");
fprintf(stderr, "%f %f %f\n", FIXEDTOFLOAT(offset[0])/CPG,
FIXEDTOFLOAT(offset[1])/CPG,
FIXEDTOFLOAT(offset[2])/CPG);
}
break;
case FLIGHT:
velocity += TOFIXED(x) - offset[2];
if(velocity < 0)
state = APOGEE;
break;
case APOGEE:
printf("#apogee\n");
exit(0);
}
printf("%f %f\n", x/(float)CPG, 9.8*FIXEDTOFLOAT(velocity)/(SPS*CPG));

}
}