[SOLVED]BCM2835 i2c timing crashes my quadcopter?


21 posts
by Hove » Fri Jun 20, 2014 1:49 pm
I use i2c connected to an accelerometer to guide my quadcopter in flight. The z-axis (vertical) occasionally returns a value ~0g, rather than the ~1g as expected. This completely messes up calculation of Euler angles (i.e. divide by 0 -> huge corrective angles), flipping the quad over onto her head and at the same time upsetting the ESC microcontrollers which reset themselves. The power surge caused by the excessive compensation angle also reboots the RaspberryPi, and possibly reduces the life of the LiPo battery too!

There is no indication to my code that such an error has happened.

I'm speculating this relates to the recently mentioned i2c bug in the BCM2835 i2c timer hardware. Is there a known fix or workaround to this?

Of course, it could equally be a bug in the MPU6050 firmware, but that would affect thousands of other quads, and so be common knowledge.

If no fix exists, I'll just make a dirty hack to replace the dodgy value with the previous good one if the difference between the two is significant.

Hove.
Last edited by Hove on Mon Jun 23, 2014 11:56 am, edited 1 time in total.
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by jeanleflambeur » Fri Jun 20, 2014 2:30 pm
You could check the delta since the last reading, divide it by Dt and if it's bigger than a limit - say 20g - ignore the reading and use the last one instead.
If only the z axis is broken then reorient the sensor so z points front instead of up. The z is way more important than front/right in your case.
Or you could read each axis separately instead of all at once.
Posts: 148
Joined: Mon Jun 16, 2014 6:07 am
by Hove » Fri Jun 20, 2014 3:30 pm
jeanleflambeur wrote:You could check the delta since the last reading, divide it by Dt and if it's bigger than a limit - say 20g - ignore the reading and use the last one instead.
If only the z axis is broken then reorient the sensor so z points front instead of up. The z is way more important than front/right in your case.
Or you could read each axis separately instead of all at once.


The spikes are so large (-1g) compared to normal running (0.8 - 1.2g), it's simple enough to just use the previous value. I don't need to worry about dt - the samples are evenly spaces at about 200Hz.

I'm not sure whether the z-axis is broken of whether it's the known BCM2835 i2c hardware bug which causes this - hence the post here.

I can't move the z-axis easily - it's on a breakout attached to a breadboard attached to the raspberry Pi attached to the quad frame - all very firmly. Rotating it 90 would ruin the firm seating of the breakout on the breadboard.

Here's what the spikes look like!

Image
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by jeanleflambeur » Fri Jun 20, 2014 4:20 pm
Do the X&Y show anything when Z spikes?
Do you get any i2c errors? What speed do you use i2c at? If it's 400khz try to lower it too 100khz and see if you still get the error.

Can you try to read the axis independently? Just pass the address of each register and then read 2 bytes instead of passing the address of X and reading 6 bytes.
Posts: 148
Joined: Mon Jun 16, 2014 6:07 am
by Hove » Fri Jun 20, 2014 5:16 pm
jeanleflambeur wrote:Do the X&Y show anything when Z spikes?


No, it seems to only be the z-axis

jeanleflambeur wrote:Do you get any i2c errors? What speed do you use i2c at? If it's 400khz try to lower it too 100khz and see if you still get the error.

I am running i2c at 400kHz - I'll try dropping back to 100and see if there's a difference


Yes, a few per 1000 reads - if I get one, I loop around reading again until I get a good reading. The number of i2c errors does not match the number of spikes.

Can you try to read the axis independently? Just pass the address of each register and then read 2 bytes instead of passing the address of X and reading 6 bytes.


I can, but there's a big performance hit - I read 7 shorts in one go, rather than 14 1 byte reads. Huge performance improvement which is critical as my code is Python.

Thanks.
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by jeanleflambeur » Fri Jun 20, 2014 5:43 pm
I assume you use the accelerometer to calculate the attitude of the quad and not for dead reckoning, right? If this is the case you could average the accelerometer readings. This would make the spike irrelevant and also eliminate any other unwanted high frequencies.
Posts: 148
Joined: Mon Jun 16, 2014 6:07 am
by Hove » Fri Jun 20, 2014 7:05 pm
Yup, that's what I'm already doing.

I've just run a test flight with the 'ignore bad readings and reuse the last one' and got exactly the same behaviour - the quad flips onto its head. So the 'flipping' problem is probably not to do with the spikes.

Oh well, thinking cap back on. Thanks anyway.
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by jeanleflambeur » Fri Jun 20, 2014 7:58 pm
If you're using euler angles on the stability PIDs you have to take care to avoid the discontinuities around 0 and 360, or -180 and 180. I had the same problem with my stability code as I was interpolating 2 angles incorrectly.
Now I use this code that takes the shortest path between 2 angles:
Code: Select all
   template <typename T, class Policy> angle<T> lerp(angle<T> const& a, angle<T> const& b, float t)
   {
      t = detail::lerp_check_mu<Policy>(t);
      auto start = a.radians;
      auto end = b.radians;
      auto difference = abs(end - start);
      if (difference > angle<T>::pi.radians)
      {
         if (end > start)
         {
            start += angle<T>::_2pi.radians;
         }
         else
         {
            end += angle<T>::_2pi.radians;
         }
      }
      return angle<T>(start + ((end - start) * t));
   }


In the latest code I'm using I completely avoid eulers and use the dot of the up vector with the front for the pitch PID and the dot of up and right for the roll PID. Like this I don't have any singularities when computing the eulers and no discontinuities as well.
The up front and right vectors are computed from the accelerometer (up), compass (front) and cross(front, up) for the right.
Posts: 148
Joined: Mon Jun 16, 2014 6:07 am
by Hove » Sat Jun 21, 2014 6:05 am
My discontinuities are at +/- 180. Since all hell breaks loose if the tilt angle goes anywhere near +/-90 degrees, 180 degree discontinuities are the least of my worries in that area! :-)

Jean - do you have a blog / site I can find out more about your quad - it sounds like yours is working well.
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by jeanleflambeur » Sat Jun 21, 2014 7:11 am
My blog is https://jeanleflambeur.wordpress.com/ but the rpi quad is not there yet..
The quad I'm working on flies in a simulation right now. I'm using bullet physics to simulate the quad - motors, air drag, etc - and test the PIDs and various flight modes in this simulation.
The initial version was developed for AVR on the crius aiop board. 3 weeks ago I switched to the Pi, but I'm still using the crius as a rate pid controller and as a sensor board. The PI will do navigation, stability pids and video streaming through wifi.

I'll publish the code as soon as I get back from vacation.
Posts: 148
Joined: Mon Jun 16, 2014 6:07 am
by hampi » Sat Jun 21, 2014 9:16 am
I cannot comment on the accelerometer, but I have not seen any "hardware bug" in i2c implementation of BCM2835 while testing it with my PiPIC at 20 kHz clock frequency. It does not support clock strecthing, so if you really need the clock stretching then you can call that a "bug". The code that I have used for testing the i2c dataflow is here

https://github.com/oh7bf/PiPIC/blob/mas ... ipictest.c

Basically the test is done by writing 4 random bytes or 32 random bits to the PiPIC and reading them back. The written bytes are compared to the received ones. The write-read cycle is repeated in a loop and hundred thousand cycles does not result to any error.

One thing you need to be careful about is that there is no other program trying to use the i2c bus at the same time. If this is the case you will probably need to implement some kind of file locking and wait that the lock is released before attempting to write to the i2c.
Posts: 223
Joined: Fri May 31, 2013 11:29 am
by Hove » Sat Jun 21, 2014 9:45 am
hampi wrote: <snip>It does not support clock strecthing, so if you really need the clock stretching then you can call that a "bug". .


That's the 'bug' that I've heard of elsewhere on the forum which can cause trouble apparently.

I actually now don't think this is i2c related, nor the root cause of the crashes I've been seeing, but I've put in a hack to reuse the previous accelerometer value if a duff value is read. I've also reduce i2c bus spead from 400kHz to the default 100kHz, and I've reduced the MPU6050 sampling rate to 250Hz from 333Hz to give the code more time to read the data. This is all completely speculative, but hopefully fix the crashes. Hopefully I can try later today.
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by Hove » Mon Jun 23, 2014 12:04 pm
Turns out the problem with my quad has nothing to do with the i2c connections.

I have no idea of the cause of the spikes - I now just detect them, and reuse the previous value should the current value be significantly out of range. A hack, but it'll do for now.

The problem causing the crashes was due to significant power bursts required to change the quad from (say) hovering state to ascending state. These were sufficient to drop the voltage of the LiPo battery from 12v to below 10v. This is outside the range of the DC-DC converter I use to get 5v from the LiPo, so the Pi rebooted, and the quad lost control.

I already had code to soften the transition between flight 'states', so all I need to do was improve this such that the 'softening' was proportional to the size of change of state. A quick few test flights this morning had no crashes.
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by Hove » Tue Jun 24, 2014 5:49 pm
jeanleflambeur wrote:I assume you use the accelerometer to calculate the attitude of the quad and not for dead reckoning, right? If this is the case you could average the accelerometer readings. This would make the spike irrelevant and also eliminate any other unwanted high frequencies.


I thought it worth picking up on this tangential question...

I am using the accelerometer for short term dead reckoning as well as long term attitude.

As my quad is autonomous, I don't include a human in the feedback loop to sort out drift. GPS resolution is too large; there are commercial motion detectors out there with cm resolution, but they are > £100; there is a possible solution using the RaspiCam as it now throws out the vectors used by the MPEG encoder to match motion between frames. The latter would be a fun winter project, but at the moment, I want to try to achieve a zero drift hover for say 10s - 30s by integrated accelerometer readings along. This is proving "challenging" at the moment though beyond a second or two!
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by jeanleflambeur » Wed Jun 25, 2014 9:21 am
Hove wrote:I thought it worth picking up on this tangential question...

I am using the accelerometer for short term dead reckoning as well as long term attitude.

As my quad is autonomous, I don't include a human in the feedback loop to sort out drift. GPS resolution is too large; there are commercial motion detectors out there with cm resolution, but they are > £100; there is a possible solution using the RaspiCam as it now throws out the vectors used by the MPEG encoder to match motion between frames. The latter would be a fun winter project, but at the moment, I want to try to achieve a zero drift hover for say 10s - 30s by integrated accelerometer readings along. This is proving "challenging" at the moment though beyond a second or two!


I'm also struggling this - as part of my goal is to have as much precision in the position as possible.
The biggest issue I've has was with the accelerometer calibration. I've tried a few ways to compute the bias and scale but failed to get anything consistent. It worked with one orientation of the sensor but flipping it upside-down or on its side gave me bad readings. In the end I took the calibration code from Arducopter and managed to get a solid calibration.
I still have drift - don't remember exactly how much but significant to be useless long-term - so my next approach was to use a complimentary filter between the accelerometer for high frequency and gps for very low frequency. I didn't do the test yet as in the meantime I switched from the AVR to the RPI and had to port some of the code.
Here's what I did so far:
- 500Hz gyro and accelerometer pooling (MPU6000 i2c).
- 188Hz DLPF, 1000DPS gyro rate, 4G max accel
- Arducopter accelerometer calibration without temperature compensation yet
- quaternion integration for the gyro
- complimentary filter between the gyro quat and a quat computed from the gravity and compass
- dead reckoning is done at 300Hz.

I can provide code for all the above if you want.
Posts: 148
Joined: Mon Jun 16, 2014 6:07 am
by Hove » Wed Jun 25, 2014 11:54 am
jeanleflambeur wrote:The biggest issue I've has was with the accelerometer calibration. I've tried a few ways to compute the bias and scale but failed to get anything consistent. It worked with one orientation of the sensor but flipping it upside-down or on its side gave me bad readings. <snip>


The accelerometer and gyro output changes depending on temperature. Gyro calibration can be calibrated prior to each flight so not a problem. For accel calibration, I ended up taking many samples across a 10 degree to 30 degree temperature range, with rotation at each temperature. I also got bad readings, so deleted these and extrapolated from good readings either side. Then I dropped these into Excel to create a trent of gain / offset against temperature - I think it's pretty good, but can only be done in spring when overnight temperatures drop to 10 degrees but during the day are above 20. So I can't do it again until the Autumn. It was also very tedious.

jeanleflambeur wrote: - 500Hz gyro and accelerometer pooling (MPU6000 i2c).


I'm at 333Hz

jeanleflambeur wrote: - 188Hz DLPF, 1000DPS gyro rate, 4G max accel


I'm at 10Hz DLPF (too much accelerometer noise otherwise for me), +/- 250 dps gyro, +/- 2g accelerometer. Doesn't 188Hz DLPF allow vast amounts of accelerometer noise? That's my reason for 10Hz.

jeanleflambeur wrote: - quaternion integration for the gyro


Ooh, interesting - I just integrate the gyro by summing each axis at 200Hz - sounds like I'd missed something here to convert pre-integrated values to earth axis frame before integrating much like I already do with accelerometer readings before summing to get velocity.

jeanleflambeur wrote: - complimentary filter between the gyro quat and a quat computed from the gravity and compass


Complementary filter between integrated gyro and Euler anglers from acclerometer - I have no compass to make sure orientation is stable, but then again I don't believe there's a way to fix yaw on a quad if the compass detects it anyway, is there?

jeanleflambeur wrote: - dead reckoning is done at 300Hz
.

About 200Hz for me - as fast as the Python code can run. Using Python more to see if it's possible, and so far it's OK

jeanleflambeur wrote:I can provide code for all the above if you want.


I think we're actually pretty similar actually by the sounds of it.
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by jeanleflambeur » Wed Jun 25, 2014 7:58 pm
Hove wrote:
jeanleflambeur wrote:The biggest issue I've has was with the accelerometer calibration. I've tried a few ways to compute the bias and scale but failed to get anything consistent. It worked with one orientation of the sensor but flipping it upside-down or on its side gave me bad readings. <snip>


The accelerometer and gyro output changes depending on temperature. Gyro calibration can be calibrated prior to each flight so not a problem. For accel calibration, I ended up taking many samples across a 10 degree to 30 degree temperature range, with rotation at each temperature. I also got bad readings, so deleted these and extrapolated from good readings either side. Then I dropped these into Excel to create a trent of gain / offset against temperature - I think it's pretty good, but can only be done in spring when overnight temperatures drop to 10 degrees but during the day are above 20. So I can't do it again until the Autumn. It was also very tedious.

I'll add temp compensation as I've now seen how much the accel is influenced by this. I take it that 2 calibrations at the temperature extremes and linear interpolation between them is enough, right?

Hove wrote:
jeanleflambeur wrote: - 500Hz gyro and accelerometer pooling (MPU6000 i2c).


I'm at 333Hz

In all honesty I didn't notice any clear difference between 500 and 250HZ gyro rates. No scientific test though.

Hove wrote:
jeanleflambeur wrote: - 188Hz DLPF, 1000DPS gyro rate, 4G max accel


I'm at 10Hz DLPF (too much accelerometer noise otherwise for me), +/- 250 dps gyro, +/- 2g accelerometer. Doesn't 188Hz DLPF allow vast amounts of accelerometer noise? That's my reason for 10Hz.

I got a lot of gyro saturation with anything lower than 1000DPS. I was doing free-fall tests to check how fast the quad can recover and any gyro saturation in these cases resulted in longer recovery times (as the complimentary filter took longer to fix the saturation). Also - noticed no gain due to the extra precision.
I figured my (old) motor+props will generate around 150hz noise but now I realize that I need to take into account the vibration due to resonance of the frame and all motors. So your 10Hz sound like a much better idea. I don't remember why I ended up with the 188Hz though...

Hove wrote:
jeanleflambeur wrote: - quaternion integration for the gyro


Ooh, interesting - I just integrate the gyro by summing each axis at 200Hz - sounds like I'd missed something here to convert pre-integrated values to earth axis frame before integrating much like I already do with accelerometer readings before summing to get velocity.

It's based on this paper and I saw a huge improvement compared to euler accumulation.
http://www.scribd.com/doc/29754518/A-Ef ... sor-Arrays


Hove wrote:
jeanleflambeur wrote: - complimentary filter between the gyro quat and a quat computed from the gravity and compass


Complementary filter between integrated gyro and Euler anglers from acclerometer - I have no compass to make sure orientation is stable, but then again I don't believe there's a way to fix yaw on a quad if the compass detects it anyway, is there?

I don't see any way to do dead-reckoning without a compass. You need the precise local frame of reference to be able to filter out gravity from the accel readings. Otherwise all your linear motion will include some gravity resulting in drift.
Unless you can keep the quads yaw perfectly constant you will have imprecise acceleration and cross axis readings.
There is a simple way to get an accurate quaternion from the accelerometer and compass provided they are not collinear. You can build a lookat matrix from these 2 vectors and their cross product and then a quaternion from the matrix. This quaternion is perfect for the complimentary filter with the integrated gyro quat.
Code:
Code: Select all
m_earth_frame_up = math::normalized<float, math::safe>(data.acceleration); //acceleration points opposite of gravity - so up
m_earth_frame_front = math::normalized<float, math::safe>(compass_data.direction);
m_earth_frame_right = math::normalized<float, math::safe>(math::cross(m_earth_frame_front, m_earth_frame_up));
m_earth_frame_front = math::cross(m_earth_frame_right, m_earth_frame_up); //make sure front is also perpendicular to up and right

math::mat3f mat(m_earth_frame_front, m_earth_frame_right, m_earth_frame_up); //this is the world to local matrix - we'll invert it below
m_local_to_world_quat_noisy.set_from_mat3(mat);
m_local_to_world_quat_noisy.invert();



So far the biggest problem - except calibration - has been getting a reliable and stable quaternion from the sensors to cancel out gravity. On the AVR it also had to be fast - so a quaternions were a perfect fit. Compared to a DCM, they cannot lose orthogonality and they are trivial to invert. Normalizing a quat is also faster and simpler than a matrix.

Having said this - I still need to accumulate the gyro rate for the rate PID.
Posts: 148
Joined: Mon Jun 16, 2014 6:07 am
by Hove » Fri Jun 27, 2014 8:56 am
I was able to do ready-reckoning from integrating the acceleration because I was working in the Earth frame, so vertical acceleration just needed g subtracting from it. So I didn't understand your comment until yesterday...

Yesterday, I converted all the code to work in the quad frame to fix all sorts of tricky problems caused by working in the Earth frame. And at that point, gravity is distributed across all the quad accelerometer axes, and as you say, ready reckoning isn't obviously possible. But, I've just realized this morning that I can rotate g from earth to quad axes, and can then subtract it from each accelerometer reading before integrating. Haven't tried it yet though - bit scared as it's a radical rework of the code moving the the quad frame!

Interesting that you had 150Hz noise - my first peak (from an audio analyser app on my iPad) is around 30Hz, hence my need to keep dlpf down to 10Hz.

Temperature calibration between 2 points shouuld be fine. I did it with lots of intermediate points as I wasn't sure how linear it was, but it's a very approximation to linear, so 2 points should be fine.

I wish I knew what quaternions are - everyone raves about them, but my ignorance is forcing me to stay with matrices! Even my matrix maths had rotted badly in the last 25 years since school. I think I need to go back to maths school.
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by jeanleflambeur » Fri Jun 27, 2014 2:28 pm
Hove wrote:I was able to do ready-reckoning from integrating the acceleration because I was working in the Earth frame, so vertical acceleration just needed g subtracting from it. So I didn't understand your comment until yesterday...

Yesterday, I converted all the code to work in the quad frame to fix all sorts of tricky problems caused by working in the Earth frame. And at that point, gravity is distributed across all the quad accelerometer axes, and as you say, ready reckoning isn't obviously possible. But, I've just realized this morning that I can rotate g from earth to quad axes, and can then subtract it from each accelerometer reading before integrating. Haven't tried it yet though - bit scared as it's a radical rework of the code moving the the quad frame!

What you're getting from the accelerometer is [earth's gravity + all linear acceleration] projected onto the sensor's axes - so in the quad frame.
You want the linear acceleration part of the sensor. So:
total_acc_q = gravity_q + linear_acc_q; // _q means in quad frame.This is what the sensor is seeing
=>
linear_acc_q = total_acc_q - gravity_q;
But gravity_q is:
gravity_q = gravity_e * matrix_e_q;
Where:
- gravity_e is gravity in earth frame - (0, 0, 1) - this is in my coord system where Z points upwards. Gravity acceleration points up.
- matrix_e_q is a rotation matrix that can bring a vector from earth frame to quad frame. It's literally the quad axes - its front, right and up vectors arranged in matrix form.
So:
gravity_q = (0, 0, 1) * matrix_e_q;
So we just need to find out matrix_e_q. But:
matrix_e_q = matrix_q_e ^ (-1)
Where matrix_q_e is literally earths axes arranged in matrix form. So we need 3 perpendicular vector to define earths frame.
You only have one now - acceleration at rest (gravity). You need another one - I use compass - and then you can fully define earth's frame.
I use this method:
When the quadcopter is at rest and on a flat surface, the accelerometers up axis should read the gravity vector in quad space (so gravity_q) - but since the quad is at rest and on a flat surface, gravity_q == gravity_e (I assume a calibrated sensor).
With a compass I have another vector in quad frame - compass_q. I assume compass_e == compass_q since I don't care about true magnetic north and with their cross product (gravity_e x compass_e) I get my 3rd vector. It's the code I posted few posts above.
With these 3 normalized vectors I have earths frame and I can keep it up-to-date using the gyro, the compass and the complimentary filter.
Once you have matrix_q_e, you also have matrix_e_q by inverting it. With these 2 you can move back and forth between earth and quad frame as you wish:
vector_q = vector_e * matrix_e_q;
and
vector_e = vector_q * matrix_q_e;



Hove wrote:Interesting that you had 150Hz noise - my first peak (from an audio analyser app on my iPad) is around 30Hz, hence my need to keep dlpf down to 10Hz.

Must have something resonating. Your cutoff seems much better as the usable frequency out of the accelerometer should never exceed ~10Hz.

Hove wrote:Temperature calibration between 2 points shouuld be fine. I did it with lots of intermediate points as I wasn't sure how linear it was, but it's a very approximation to linear, so 2 points should be fine.

I wish I knew what quaternions are - everyone raves about them, but my ignorance is forcing me to stay with matrices! Even my matrix maths had rotted badly in the last 25 years since school. I think I need to go back to maths school.


I treat quaternions a bit like black boxes - although they seem to be simple enough.
They represent a rotation - you can think about them as a different representation of an angle-axis pair.
Their advantages are:
- smaller than a matrix (4 floats vs 9 floats)
- faster multiplication (16 mul + 12 adds vs 27 mul + 18 adds)
- way faster inversion which is always defined (3 divisions vs a LOT of math)
- they can be interpolated while staying normalized- this is the main advantage.

I can send you some quaternion code or you could take a look at the Eigen library. Not sure if there are python ports though.

Cheers
Posts: 148
Joined: Mon Jun 16, 2014 6:07 am
by Hove » Fri Jun 27, 2014 3:24 pm
[quote="jeanleflambeur"]
What you're getting from the accelerometer is [earth's gravity + all linear acceleration] projected onto the sensor's axes - so in the quad frame.
You want the linear acceleration part of the sensor. So:
total_acc_q = gravity_q + linear_acc_q; // _q means in quad frame.This is what the sensor is seeing
=>
linear_acc_q = total_acc_q - gravity_q;
But gravity_q is:
gravity_q = gravity_e * matrix_e_q;

...<snip>
</quote>

I have matrix_e_q. I also don't really care about earth X & Y axes currently so mine are not earth, but aligned with the quad X and Y axes as viewed from above (i.e no yaw processing), so I think I'm safe to use q_gravity = matrix_e_q * (0, 0, g) where g just comes from before liftoff by qax^2 + qay^2 + qaz^2 = g (qa? = quad accelerometer reading in ? axis)

With q_gravity in hand, I can subtract it off q accelerometer to get the remnants of acceleration to intergrate to get q axis velocity. Feed that into the PID with a velocity target also defined by q_target = matrix_e_q * e_target and it all should work barring any bugs I introduce while turning the code into Phoebe's world!
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK
by Hove » Fri Jun 27, 2014 4:10 pm
Sorry Jean, I missed the point: q_forward + q_yaw can't be fixed by just q_reverse unless q_raw = 0; otherwise q_left or q_right will be needed to compenate and keep the quad in the same place, even if it's actual orientation against the earth doesn't matter.

I'm only planning on short term flights so hopefully integrated gyro will do for a few seconds, but as you say, only a compass can really allow ready-reckoning long term.

Cheers.
www.pistuffing.co.uk - Raspberry Pi and other stuffing!
User avatar
Posts: 1122
Joined: Sun Oct 21, 2012 6:55 pm
Location: Cotswolds, UK