Quantcast

Next Generation Tilt Sensor/ Attitude Monitor

The Rocketry Forum

Help Support The Rocketry Forum:

UhClem

Well-Known Member
Joined
Feb 6, 2009
Messages
1,665
Reaction score
179
The MPU9250 board DOES have a magnetometer, but if you think about it you don't really need it. You aren't trying to figure out which direction your rocket is pointing, just what the tilt is. Using the accelerometer to give you the tilt on the pad and the gyros to correct it in flight is the easiest way to do this, and it's not dependent on the velocity or acceleration of your rocket. For that matter, you don't need to worry about the z-axis gyro either, it doesn't affect the tilt.
You only need the accels for launch detect.

You don't want the rocket to fly straight up but to stay pointed in the same direction that it was on the pad. This way you can adjust its trajectory based on winds so that it has a better chance of landing where you want it to. Plus going straight up messes with the best practices of angling the rocket away from the flight line.

All the gyro axes are required because any rotation around the longitudinal axis changes the orientation of the other two.What was providing tilt in the north-south plane a moment ago could now be east-west.

I have a little project on the bench at the moment to record MPU-9250 data at high speed. Once I get that code working I will dust off what I remember about quaternions.
 

cerving

Owner, Eggtimer Rocketry
TRF Sponsor
TRF Supporter
Joined
Feb 3, 2012
Messages
3,957
Reaction score
1,200
All the gyro axes are required because any rotation around the longitudinal axis changes the orientation of the other two.What was providing tilt in the north-south plane a moment ago could now be east-west.
I will submit that if you only care about the amount of tilt and not the orientation, then you don't need the Z axis rotation. Yes, the Z axis rotation will change the ORIENTATION as the X and Y values change (think of a 3D spiral), but it won't change the TILT (the angle of Z to vertical). If you were lucky enough to have no X/Y rotation at all, the Z rotation will simply cause the rocket to spin around the Z axis.
 

mikec

Well-Known Member
Joined
May 9, 2009
Messages
2,437
Reaction score
347
As I continue to dig into the MPU-6050 documentation, I find that it should be possible to put the DMP in a mode where it is internally integrating the gyros at 200 Hz but not trying to do anything with the accelerometer, which should be the best of both worlds for how to operate post-launch. I don't think this mode is supported in the Rowberg library as yet, but it should be possible to add. BTW, there's a more complete library for the MPU-9250 ( https://github.com/sparkfun/MPU-9250_Breakout ).
 
Last edited:

VernK

Member/Vendor
TRF Sponsor
TRF Supporter
Joined
Oct 1, 2014
Messages
132
Reaction score
73
Location
Boise, Idaho
I will submit that if you only care about the amount of tilt and not the orientation, then you don't need the Z axis rotation. Yes, the Z axis rotation will change the ORIENTATION as the X and Y values change (think of a 3D spiral), but it won't change the TILT (the angle of Z to vertical). If you were lucky enough to have no X/Y rotation at all, the Z rotation will simply cause the rocket to spin around the Z axis.
I think you do need all three axis because the tilt you care about is the orientation with respect to the earth. For example, imagine a rocket that pitches +5 degrees on its pitch axis. Then it rolls 180 degrees on its longitudinal axis. Then it pitches another +5 degrees on its pitch axis. (Which is now oriented in the opposite direction from before.). The pitch gyro by itself would indicate a total of 10 degrees of pitch where as the total tilt with respect to the earth is zero degrees.
 
Last edited:

ekchess

Member
Joined
Sep 18, 2009
Messages
11
Reaction score
0
The MPU9250 board DOES have a magnetometer, but if you think about it you don't really need it. You aren't trying to figure out which direction your rocket is pointing, just what the tilt is. Using the accelerometer to give you the tilt on the pad and the gyros to correct it in flight is the easiest way to do this, and it's not dependent on the velocity or acceleration of your rocket. For that matter, you don't need to worry about the z-axis gyro either, it doesn't affect the tilt.
Thanks Cris. I downloaded the open-sourced code from Altus Metrum, in which they tackle this problem as you described for use with their Telemega, but they are using a full quaternion. The code is very complex, and has so many interacting functions in their .c files that it is difficult for me to follow. I'll keep at this, though, as the problem is worth solving. The most interesting functions are in the altos/src/kernel/ao_sample.c files, but you also need the functions in the ao_quaternion.c file and some others.
 

ekchess

Member
Joined
Sep 18, 2009
Messages
11
Reaction score
0
As I continue to dig into the MPU-6050 documentation, I find that it should be possible to put the DMP in a mode where it is internally integrating the gyros at 200 Hz but not trying to do anything with the accelerometer, which should be the best of both worlds for how to operate post-launch. I don't think this mode is supported in the Rowberg library as yet, but it should be possible to add. BTW, there's a more complete library for the MPU-9250 ( https://github.com/sparkfun/MPU-9250_Breakout ).
Fabulous! Good luck. I agree that the Rowberg library is limited with respect to gyro use--he really pushed using the accel functions, and that makes sense for folks who are not concerned about high accelerations (like folks whos applications mirror the the teapot/rabbit AHRS demo). Please keep us informed of your progress.
 

jderimig

Sponsor
TRF Sponsor
Joined
Jan 23, 2009
Messages
3,204
Reaction score
617
That's my concern, that the DMP is using the accelerometer inputs under thrust to "correct" gyro drift as though thrust was gravity -- if true that would defeat the whole purpose. I can't tell from the fairly minimal DMP docs I've been able to find online so far exactly how it works.
The DMP has an option to not use the accelerometers to compute the quaternions, just the gyros. (As MikeC has mentioned in an earlier post).
 

mikec

Well-Known Member
Joined
May 9, 2009
Messages
2,437
Reaction score
347
I downloaded the open-sourced code from Altus Metrum, in which they tackle this problem as you described for use with their Telemega, but they are using a full quaternion. The code is very complex, and has so many interacting functions in their .c files that it is difficult for me to follow.
There's lots of open-source software from the drone community that works with gyros, and I might recommend some of that over the Altus Metrum code, which I find pretty complex and hard to follow. http://wiki.paparazziuav.org/w/images/e/e5/DCMDraft2.pdf is some early work in this area, though it uses direction cosine matrices instead of quaternions (basically equivalent.) But keep in mind that it works on aircraft, where the sensed gravity vector means something when the plane is in level flight, whereas rockets are never in level flight.

But I think your code is fundamentally OK, it's just that we want to reference everything to the pre-launch gravity direction and not use the accelerometers post-launch.
 

JimJarvis50

Well-Known Member
Joined
Jan 18, 2009
Messages
2,433
Reaction score
669
There's lots of open-source software from the drone community that works with gyros, and I might recommend some of that over the Altus Metrum code, which I find pretty complex and hard to follow. http://wiki.paparazziuav.org/w/images/e/e5/DCMDraft2.pdf is some early work in this area, though it uses direction cosine matrices instead of quaternions (basically equivalent.) But keep in mind that it works on aircraft, where the sensed gravity vector means something when the plane is in level flight, whereas rockets are never in level flight.

But I think your code is fundamentally OK, it's just that we want to reference everything to the pre-launch gravity direction and not use the accelerometers post-launch.
I wish I knew a bit more about the math and programming associated with these systems. It would really help me in my development of a vertical stabilization system, which depends entirely on the accurate measurement of tilt. Fortunately for me, my Guru, William Premerlani, knows a lot about those things. He developed the cosine matrix theory you referenced in the link and worked with Frank Hermes to develop the Tiltometer, which unfortunately is no longer being made. Bill has also developed flight software and hardware primarily for autopilot systems for drones and helicopters, and now rockets.

For the last couple of years, I've been working with Bill, Frank and others to adapt the flight hardware he has developed to produce a vertical stabilization system for rockets. I think Bill has produced a number of innovations and we have had many lessons learned. Just as an example, Bill has developed a way to reduce the gyro offsets on all three axes to essentially zero right up to the moment the button is pushed. This is combined with a launch detect algorithm that then decouples the gyros and the accelerometers during the flight. The trick to this is to optimize the offset correction rate so that there is enough time to reduce the offset, but not enough time to change the offsets in the time it takes to detect launch. One of several examples.

With respect to lessons learned, I think we have conceded that for the type of gyros we are using (MPU-6000), it is simply not possible to obtain accurate tilt measurements if the rocket is rotating. I don't understand the technical basis for this, but what I do know is that rotation in the vertical axis imparts a "reading" in the two horizontal axis gyros. Rotating back to the starting point undoes this, but not exactly. As a result, after perhaps 10 rotations, the tilt reading becomes meaningless. Consequently, my focus on the application of the technology is to try to eliminate as much roll as possible.

I'm not exactly sure why I am posting this except to perhaps encourage a little more cross talk in the development of these systems. The capability and available hardware is something the hobby really needs.

Jim
 

mikec

Well-Known Member
Joined
May 9, 2009
Messages
2,437
Reaction score
347
With respect to lessons learned, I think we have conceded that for the type of gyros we are using (MPU-6000), it is simply not possible to obtain accurate tilt measurements if the rocket is rotating. I don't understand the technical basis for this, but what I do know is that rotation in the vertical axis imparts a "reading" in the two horizontal axis gyros.
Interesting. There could be two reasons for this. One is that the hardware just has some gyro or accelerometer/gyro crosstalk. That wouldn't be too surprising since these things are really designed for applications like device orientation for gaming, fitness trackers, 3D mice, etc, where the gravity vector is usually available in some form to correct gyro drift. Rockets are obviously a very stressing case of high acceleration and no gravity vector.

The other possibility is that the devices would work but we're just not using them right. This is entirely possible, because the math is really pretty complicated and the device documentation is IMHO appallingly poor. I have a fair amount of flight data using an older generation Sparkfun Razor IMU with separate gyro (ITG-3200) and accelerometer (ADXL345) chips and I thought it was getting the tilt right with rocket rotation, but I'll go back and look at that in more detail. I have some flights where I have both tilt data and onboard video and that provides a good rotation cross-check.

Back to this thread a bit: I spent some time looking at the Rowland library, but since it's based on an old version of the DMP for which I can't find any documentation, I think modifying it in any way is basically hopeless, at least for me. There is a good library from Sparkfun for the newer version of the DMP that is based on the MPU-9150 but it could probably be backported to the MPU-6050 so I think that will be what I will look at next. One possible thing that would help these applications is magnetometer data fusion, since the Earth's magnetic field should be a stable reference frame.
 

JimJarvis50

Well-Known Member
Joined
Jan 18, 2009
Messages
2,433
Reaction score
669
Interesting. There could be two reasons for this. One is that the hardware just has some gyro or accelerometer/gyro crosstalk. That wouldn't be too surprising since these things are really designed for applications like device orientation for gaming, fitness trackers, 3D mice, etc, where the gravity vector is usually available in some form to correct gyro drift. Rockets are obviously a very stressing case of high acceleration and no gravity vector.

The other possibility is that the devices would work but we're just not using them right. This is entirely possible, because the math is really pretty complicated and the device documentation is IMHO appallingly poor. I have a fair amount of flight data using an older generation Sparkfun Razor IMU with separate gyro (ITG-3200) and accelerometer (ADXL345) chips and I thought it was getting the tilt right with rocket rotation, but I'll go back and look at that in more detail. I have some flights where I have both tilt data and onboard video and that provides a good rotation cross-check.

Back to this thread a bit: I spent some time looking at the Rowland library, but since it's based on an old version of the DMP for which I can't find any documentation, I think modifying it in any way is basically hopeless, at least for me. There is a good library from Sparkfun for the newer version of the DMP that is based on the MPU-9150 but it could probably be backported to the MPU-6050 so I think that will be what I will look at next. One possible thing that would help these applications is magnetometer data fusion, since the Earth's magnetic field should be a stable reference frame.
I should clarify that the testing I mentioned was done with the accelerometers off. Obviously, if they are on and correcting, there is no problem (except that you can't fly that way in a rocket). I think cross talk is the issue with perhaps thousands of degrees of rotation in the vertical axis against only a few degrees in the horizontal. Any rotation accumulates an error.

Jim
 

UhClem

Well-Known Member
Joined
Feb 6, 2009
Messages
1,665
Reaction score
179
Interesting. There could be two reasons for this. One is that the hardware just has some gyro or accelerometer/gyro crosstalk.
The reason isn't a mystery as it is right there in the data sheet: cross axis sensitivity: 2% typical, no maximum given.

You could compensate for this if the sensitivity is stable or at least predictable and you can measure it. I nice rate table would be nice but you should be able to find a 200 d/s table pretty easily.
 

jderimig

Sponsor
TRF Sponsor
Joined
Jan 23, 2009
Messages
3,204
Reaction score
617
The reason isn't a mystery as it is right there in the data sheet: cross axis sensitivity: 2% typical, no maximum given.

You could compensate for this if the sensitivity is stable or at least predictable and you can measure it. I nice rate table would be nice but you should be able to find a 200 d/s table pretty easily.
^ Yup. I have several 198 d/s tables (I had to blow the dust off them) that I used to create a correction lookup table for the ST gyro chip I am playing with.
 

mikec

Well-Known Member
Joined
May 9, 2009
Messages
2,437
Reaction score
347
I nice rate table would be nice but you should be able to find a 200 d/s table pretty easily.
Pardon my ignorance, but I can't parse this. What's a d/s table?
 

JimJarvis50

Well-Known Member
Joined
Jan 18, 2009
Messages
2,433
Reaction score
669
Thanks Cris. I downloaded the open-sourced code from Altus Metrum, in which they tackle this problem as you described for use with their Telemega, but they are using a full quaternion.
Can you direct me to where this is located?

Thanks.

Jim
 

keithp

Well-Known Member
Joined
Aug 20, 2010
Messages
387
Reaction score
7
As noted upstream, IMHO this code is not very comprehensible.
The rotation tracking is a tiny portion of the overall flight state tracking code in src/kernel/ao_sample.c. There are two pieces, the first uses the accelerometer in the MPU to compute the rotation on the ground. The quaternion tracking the current total rotation in all three axes is stored in 'ao_rotation':

Code:
	ao_quaternion_init_vector(&orient,
				  (ao_ground_accel_across - ao_config.accel_zero_across),
				  (ao_ground_accel_through - ao_config.accel_zero_through),
				  (ao_ground_accel_along - ao_config.accel_zero_along));

	ao_quaternion_normalize(&orient,
				&orient);

	/* Here's up */

	struct ao_quaternion	up = { .r = 0, .x = 0, .y = 0, .z = 1 };

	if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
		up.z = -1;

	/* Compute rotation to get from up to our current orientation, set
	 * that as the current rotation vector
	 */
	ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient);
The other bit of code takes the gyro data and stirs that into ao_rotation. The first bit takes the raw sensor values, subtracts out the calibration data recorded on the ground (which assumes that the rocket is stationary on the pad). Those values are just the sum of 512 samples of the sensor, hence the weird shifting and dividing. Finally, the result is multiplied by the time step so we get rotation amount from rotation speed:

Code:
	float	x = ao_mpu6000_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt;
	float	y = ao_mpu6000_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt;
	float	z = ao_mpu6000_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt;
Next, we convert the rotation amount into a quaternion and apply that to the current rotation:

Code:
	ao_quaternion_init_half_euler(&rot, x, y, z);
	ao_quaternion_multiply(&ao_rotation, &rot, &ao_rotation);

	/* And normalize to make sure it remains a unit vector */
	ao_quaternion_normalize(&ao_rotation, &ao_rotation);
The last bit has a huge comment, because it's the 'tricky' part (at least for me). This extracts just the tilt angle from the overall ao_rotation quaternion by measuring the angle from a quaternion pointing vertically. Oddly, that doesn't take a full quaternion multiply as we only want one of the four values. So, the comment describes how that shortcut works. Of course, the result is actually the cosine of the tilt angle, because quaternions store ratios, not angles, so we run that back through acosf to get the angle. Finally, that's converted from radians to degrees.

Code:
	/* Compute pitch angle from vertical by taking the pad
	 * orientation vector and rotating it by the current total
	 * rotation value. That will be a unit vector pointing along
	 * the airframe axis. The Z value will be the cosine of the
	 * change in the angle from vertical since boost.
	 *
	 * rot = ao_rotation * vertical * ao_rotation°
	 * rot = ao_rotation * (0,0,0,1) * ao_rotation°
	 *     = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z
	 *
	 *     = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r)
	 *     = a.z² - a.y² - a.x² + a.r²
	 *
	 * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation°
	 *     = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z
	 *
	 *     = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r)
	 *     = -a.z² + a.y² + a.x² - a.r²
	 */

	float rotz;
	rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r;

	ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI);
That's the whole algorithm. Just like the Kalman filter, it took a few weeks to understand the math well enough to write it all down, but it's only a few lines of code in the final implementation.

What I want to eventually do is take this value and the magnetic sensor data and fuse those in another Kalman filter so I can get long-term stable rotation data, instead of having it slowly lose accuracy during the flight.
 

UhClem

Well-Known Member
Joined
Feb 6, 2009
Messages
1,665
Reaction score
179
There's lots of open-source software from the drone community that works with gyros, and I might recommend some of that over the Altus Metrum code, which I find pretty complex and hard to follow.
A quick summary of what is going on. But I am by no means an expert on quaternions. Every time I think about them my brain hurts.

Quaternions describe rotations of 3D vectors and you can combine two rotations into one simply by multiplying the quaternions together. So the integration step is easy. Just multiply the current rotation by the small rotation given by the gyro measurement.

NASA Technical Memorandum 4775 goes through a lot of math to get to the same point. It is interesting reading though. It describes a project where they lift a UAV by balloon. When released at altitude it uses a small rocket motor to bring it from its initial nose down attitude to level flight.

This is where you can make an optimization. Converting those gyro measurements into a quaternion requires both a sine and cosine. Depending on the platform that can be expensive in terms of code space and/or time. If the rotation is small enough, you can use the small angle approximation where cos(x) = 1 and sin(x) = x. That is what I do since my data is collected at 32KSPS. Altos is running at 100SPS and a 2000d/s range so it tries another approximation. I wouldn't worry about it.

The maximum possible rotation in Altos is 20 degrees per sample. I suspect that if you have rotation anywhere near the 2000 degrees/sec sensor maximum you are going to quickly blow past your tilt threshold. Errors from the small angle approximation aren't likely to matter much.

One nice feature of this is that it is norm preserving. (Vector length = 1) Errors will creep in that denormalize the quaternion over time so you will want to normalize it occasionally but there is no need to waste time doing it for every sample. Once a second seemed to do just as well with my 32KSPS data.

So keep your starting quaternion set to zero rotation until you detect launch and then start integrating. Launch detect criteria should be such that the rocket is still on the rod/rail!

Measuring the angle is pretty simple as well. Use the quaternion to rotate a vector from your starting orientation to the current orientation and then calculate the angle between those two vectors.

This can be simplified a lot if the starting vector is something simple like [1 0 0]. Then you can skip the full strength quaternion multiply and dot products.

Note that you don't even need an arccos function here unless you absolutely require angles in degrees. If you store your threshold for disabling outputs as cosines of the angles, you can skip that. So you don't need anything more complicated than: add, multiply, and square root. Fixed or floating point is your choice. Speed is important as the quaternion multiply requires 16 multiplications.

If this is integrated within an altimeter, you only need to check the angle when you are about to fire an output conditioned on it. No need to waste time doing it every stinkin' sample.

I dug up the code I wrote when trying this on Glen Overby's data and whipped up a quick plot using some 32KSPS data. I tried to do three 90 degree rotations isolated to a single axis. The failure to return to zero could be because of a flaw in my code or non-linearity/cross axis sensitivity in the sensor.

tilt.png
 

UhClem

Well-Known Member
Joined
Feb 6, 2009
Messages
1,665
Reaction score
179
I captured a little data using my rate table (Denon DP-45F) and the cross axis sensitivity was interesting. Because I expected that the sensitivity might vary depending on the direction of rotation, I flipped the logger over so as to rotate it in both directions around the Y axis.

The results showed that it did indeed vary quite a lot. See the attached plots.

The data was run through a 1000 point moving average filter in order to tamp down the sensor noise a bit.

With rotation of -200 degrees/second both the X and Z gyros showed not much sensitivity with a shift of less than 1/2 degree/second in output.

But with a positive 200 degree/second rotation things were different. The X gyro shifted by more than 3 degrees/second and the shift was negative just like it was in the first case. You would kind of expect the sign to change.

The Z axis did exhibit the expected sign change but the magnitude was unexpected as it was around 10 degrees/second. (5%) I would say that was out of specification except that the data sheet just gives a typical value of 2% with no indication of a maximum.

I also expect that this might vary with temperature but I do not have a temperature controlled chamber to set over my rate table. The best I will be able to do is toss the logger into the freezer for a while. So it will be warming up throughout the test.

cross1.png


cross2.png
 

UhClem

Well-Known Member
Joined
Feb 6, 2009
Messages
1,665
Reaction score
179
It turns out that the cross axis sensitivity is not as horrible as I thought.

I used the mount I had built for early flight testing and while that is adequate for that purpose, it has flaws for rate table testing. It is just a simple ply sled with centering rings at either end to hold it in place in a payload bay. But I noticed that one of the rings isn't as well aligned as the other. The result being that when standing on end, such as when I did the rate test, it has a noticeable tilt one way and not the other.

I unmounted the board and flipped it over. If the problem really was cross axis sensitivity then it would continue being bad for positive rates.

It wasn't. The results were about the opposite from the first test.

I am not sure how I am going to compensate for the misalignment with the rotation. Maybe the accels will help by measuring tilt...
 

SparkyVTFlyer

Senior Member
TRF Supporter
Joined
Feb 8, 2009
Messages
189
Reaction score
20
Location
Yorktown, VA
I've also been working on something similar. At NARAM last summer I built and demo'd an ATMega328 (Arduino Uno) based logging altimeter capable of dual deploy and 2-stage functions (separation & ignition). I developed my own simple algorithm to estimate the angle off vertical. The unit is capable of recording the following at 400 samples/sec to an external SD card:

3-axis 16G accelerometer (LSM303)
3-axis 200G accelerometer w/ 16-bit ADC (ADXL377)
3-axis 2000dps gyro (L3GD20)
baro pressure sensor (BMP180) (20 samples/sec)
Integrated velocity, altitude, and off-vertical
Status of all flight events
Mach immune staging and deployment

The algorithm to integrate angle off vertical was the hardest part. I attempted quaternions but couldn't get them to work. I tried traditional quaternions and differential quaternions to no avail. Mostly through luck, I came up with a simple algorithm that only required 10 floating point operations per cycle. I sped it up by using small-angle approximations in combination with the angle-addition trig identity and fixed-point math. It won't work for complex rotations, but for simple rocket trajectories it works well. I have about a two dozen flights with the algorithm now and its fairly accurate. Onboard video shows it to be typically within 2-5 degrees of the actual off-vertical value at apogee.

I might give quaternions another try using Keith's code from his post. It took me a while to understand quaternions but couldn't get them to work in practice. I used my code to build a test unit for a friend. Here is a youtube video of it working. Yes, I know its not a difficult test of the functionality.

https://youtu.be/mih46qINaT4

I've found that the cross-axis sensitivity is definitely a problem. I ran some bench tests to collect data and used a simple linear regression model to back-out the cross-axis "spillover." It works pretty well and tamped-down the cycling that was present for the flights with significant roll rates.

I attached a graphic of the algorithm output. This was an EX M-powered flight to about 9,000 ft and just shy of supersonic. The algorithm estimate matches the onboard video very well. In fact, its no more than 2 degrees off at apogee.

Rotation HyperLOC1600.jpg

Here's the bit of code (but doesn't have the linear regression model, that will probably be hardware dependent):

long gdt = 0L;
long ang_x = 0L;
long ang_y = 0L;
long ang_z = 0L;
float ang_y0;
float ang_z0;
int angX = 0;
int angY;
int angZ;
float cosX = 1.0;
float sinX = 0.0;
float PrevCosX = 1.0;
float PrevSinX = 0.0;
int gyroBiasX = 14;
int gyroBiasY = 35;
int gyroBiasZ = 3;
const long oneDeg = 14285714L;//oneDeg = mln/0.070
const long oneTenthDeg = 1428571L;//oneTenthDeg = oneDeg/10
int counterSign = 1;
boolean calcOffVert = false;
int offVert = 0;

//Eliminate gyro bias
gyro_event.gyro.x -= gyroBiasX;
gyro_event.gyro.y -= gyroBiasY;
gyro_event.gyro.z -= gyroBiasZ;

//Calculate new X angle from gyro data
ang_x += gyro_event.gyro.x * gdt;
//Overflow X data and recompute as needed
while (abs(ang_x) > oneDeg) {
if (ang_x > 0) {counterSign = 1;}
else {counterSign = -1;}
ang_x -= counterSign * oneDeg;
angX += counterSign;
//cos(A+B) = cosAcosB - sinAsinB A=big B=small cos(1deg)=0.999847695
cosX = PrevCosX * 0.999847695 - PrevSinX * (counterSign * 0.017452406);
//sin(A+B) = sinAcosB + cosAsinB A=big B=small
sinX = PrevSinX * 0.999847695 + PrevCosX * (counterSign * 0.017452406);
PrevCosX = cosX;
PrevSinX = sinX;}

//Compute the new y and z angles
ang_z += cosX * gyro_event.gyro.z * gdt;
ang_z += sinX * gyro_event.gyro.y * gdt;
ang_y += cosX * gyro_event.gyro.y * gdt;
ang_y -= sinX * gyro_event.gyro.z * gdt;
//Overflow Y data and recompute as needed
while (abs(ang_y) > oneTenthDeg) {
if (ang_y > 0) {counterSign = 1;}
else {counterSign = -1;}
ang_y -= counterSign * oneTenthDeg;
angY += counterSign;
calcOffVert = true;}
//Overflow Z data and recompute as needed
while (abs(ang_z) > oneTenthDeg) {
if (ang_z > 0) {counterSign = 1;}
else {counterSign = -1;}
ang_z -= counterSign * oneTenthDeg;
angZ += counterSign;
calcOffVert = true;}
 

cerving

Owner, Eggtimer Rocketry
TRF Sponsor
TRF Supporter
Joined
Feb 3, 2012
Messages
3,957
Reaction score
1,200
Very nice. BTW, the BMP180 is discontinued and basically unavailable, you're going to want to try another baro soon. I went with the TE/Meas-Spec MS5637 to replace the BMP180's in the Eggtimer altimeters. Unfortunately the code isn't compatible, but I'll send you the code snippet if you're interested. (Don't use MS5607/5611 code, it "looks" the same but it's not.) It's a bit faster than the BMP180, but the temperature sensor takes longer to stabilize.
 

plugger

Well-Known Member
Joined
May 1, 2009
Messages
459
Reaction score
156
I am adding 2 clustomatics from Whooshtronics (great product:smile:, also discontinued:cry:). Since plans call for starting up to 6 motors, and I have some concerns about the amperage limitations of the solid state relays, this will move the high amperage ignition circuit from the companion board to the Clustomatic, as well as meter out proper current to each individual igniter and not allow one short from stealing all the electrons.
Agreed. PK's uMAD was another product from Wooshtronics that is incredibly useful. It's crazy to think that his Clustomatics were $25AUD less than 5 years ago. Talk about VALUE!
 

SparkyVTFlyer

Senior Member
TRF Supporter
Joined
Feb 8, 2009
Messages
189
Reaction score
20
Location
Yorktown, VA
Here are some visuals highlighting the cross-axis sensitivity problem. This might be part of the reason I can't get quaternions to work correctly. Now that I know how to correct for it, I'll give them another shot.

The first graph shows my algorithm without the cross-axis correction. You can see the cycling when the roll rate gets high. The second graph corrects this effect with a simple linear regression model. Notice the big difference between the two. This matches really well with the onboard video. In particular, at apogee the video show the rocket pointed directly at the horizon (90 degrees), which is exactly what the algorithm is estimating. This was a 2-stage I-to-I flight that topped out around 6,000 ft.

Original Rotation Data.jpgCorrected Rotation Data.jpg
 

ekchess

Member
Joined
Sep 18, 2009
Messages
11
Reaction score
0
Sparky, With respect to your code, you have define gdt = 0L. Since that doesn't get updated anywhere in code, the computed angles zero out. Can you let us know what value gdt should take (I'm assuming it has to do with frequency of gyro readout and will be dependent on the gyro settings and hardware used in the circuit)? I implemented the code as written with an MPU6050 and printed out the values of angX, angY, and andZ, and found them to be consistently 0. With my current hardware/firmware settings, the gyro is being sampled at 8000Hz (obviously not using all the data when printing is occurring). I can slow that down to 1000Hz by increasing the filtering. Thanks for following the thread! I haven't been able to implement Keith's quaternion-based method as yet, but I'm still working on it. Your method seems much simpler.
 

SparkyVTFlyer

Senior Member
TRF Supporter
Joined
Feb 8, 2009
Messages
189
Reaction score
20
Location
Yorktown, VA
Sorry, I forgot that part. The full flight computer code is around 800 lines. gdt is the between samples time in microseconds.

gdt = currentClockTime - previousClockTime

Sorry my code isn't exactly clear as to what's going on. Its a little tricky because I did the math to make it run on integers and not floats. I also sacrificed resolution to make it run faster. You will need to update the values for oneTenthDeg and oneDeg to be your gain value x 1,000,000. For mine, the gain is set to 2000 dps, which is 0.07 degree per lsb (least significant bit). Yours will be different depending on your gyro.

I just looked up the MPU6050. Your settings should be:

oneDeg = 16400000L;
oneTenthDeg = 1640000L;

The angX, angY, and angZ values are the current orientation of the rocket which you want to output. angX is in degrees and angY, angZ are in tenths of a degree. Divide these by 10 in the output to get the estimate. These values only get updated when the rocket's rotation exceed the oneDeg and oneTenthDeg thresholds. ang_x, ang_y, and ang_z keep the pieces of the angle between the thresholds. Precision is maintained, but resolution is sacrificed.

The algorithm requires the cosine and sine of the angX rotation. Again, to make it go faster, I update these only when the thresholds oneDeg is exceeded. Since I only update these values when it has moved by exactly one degree of rotation, then I can employ the trig angle-addition identity very efficiently.

The heart of the algorithm is here:
ang_z += cosX * gyro_event.gyro.z * gdt;
ang_z += sinX * gyro_event.gyro.y * gdt;
ang_y += cosX * gyro_event.gyro.y * gdt;
ang_y -= sinX * gyro_event.gyro.z * gdt;

Don't be fooled by its simplicity. It took me months to stumble upon it. It then took me twice as long to understand the math behind it. It looks like Euler's formula for 2-D rotations, but its not. Its actually employing a hidden series of arctan and tangent calculations. As long as the update rate is fast enough, I eliminate the tangent calculations with the small-angle approximation tan(x) ~= x.

If you are a NAR member, you can login and download my report from NARAM-58. Its not very well written, so if you don't understand it, it's just my crappy writing.

Like I said before, this ONLY works with relatively simple rotations restricted to roughly one plane in 3D. If you swing the unit all over the place in many directions, then it will be wildly inaccurate. However, if you spin it around the vertical axis (like a rocket spinning in flight) and tilt it in any ONE direction (like a rocket tilting over at apogee), then it will be accurate. You can omit any cross-axis correction and it will still be relatively accurate, but you will see some cycling when the unit is spinning fast.
 
Last edited:
Top