pitch roll problem

[pizza]

Hi guys I'm having a pitch roll understanding problem. attached is the image(pitchroll.jpg) of my perception.

should pitch/roll range from 0-360degrees, or +-180degrees?
well i used atan2(x,z) to obtain pitch, but due to the discontinuity of atan2 function, there will be a jump from -180 to 180 degrees. if i shifted the range to 360degrees, then they'll be a jump from 0 to 360degrees when you tilt in the negative direction.

the other image(tilt.jpg) which i googled online shifts pitch/roll to the 4 quadrants, hence the max of pitch and roll is 90degrees each. using this solution, for example if your vehicle rolls 110 degrees from the right, your acclerometer will interpret it as a roll of 70degrees.

sighs i'm so confused. hopefully i did not confuse you guys. how did you do your pitch and rolls?

[pizza]

also, if pitch = atan2(x,z) and roll = atan2(y,z)
when the sensorboard is pitched 90degrees (0 roll), the software will read 180++ degrees roll, because z axis is now pointed up and atan2 will add 180degrees to your roll. which is wrong..
vice versa, if sensorboard is rolled 90 degrees (and 0 pitch) the software will read 180degrees++pitch

this problem is similar to this guy's
http://forum.sparkfun.com/viewtopic.php ... 986#p98986

what is the correct calculation for pitch and roll?

[Mitch]

The "standard" is pitch +-90, roll +-180, and yaw +- 180 (sometimes 0-360). As you're finding, the euler angles don't cover everything because there are mathematical singularities at pitch +-90 corresponding to the north and south poles. The functions blow up as the divisor goes to zero. This needs to be addressed somehow if the vehicle will be in that attitude. Also, this is why quaternions are becoming so popular!

To get around part of the problem use

double g = sqrt( ax*ax + ay*ay + az*az );

euler[0] = atan2( ay, -az ); // Roll
euler[1] = asin( ax / -g ); // Pitch

[pizza]

thanks mitch.

yeah, atan2 goes undefined if both arguments read 0, when both the axis(corressponding to the arguments) are parallel to ground. so i guess euler[0] = atan2( ay, -az ); // Roll could pose a problem too, it is just that our accelerometers are noisy and are unlikely to read perfect 0 for both axes.

in fact, without quaternions i mean,
i do not think that pitch = atan2(ax,az) or roll= atan2( ay, -az ); is correct,
let's use this example: when pitching a uav at 45degrees and rolling at 0 degrees, then our y axis would remain parellel to ground while x axis would be at 45degrees. tan45 = 1 = x/z;
then, from this position, roll the uav by say 30 degrees, your x value remains unchanged, while z axis has changed its value due to the roll, so x/z=1 is no longer true, and your pitch would probably be at a wrong angle. please correct me if my perception is wrong.

so to consider all 3 vector components x y and z, what the freescale an3461 datasheet suggests is when calculating pitch, resolve the vector perpendicular to the X axis by combining the Y and Z components => vector = sqroot(Ysquare+Zsquare). this resolved vector in the yz plane is also gcos(pitch), while x is gsin(pitch),
then tan(pitch) = x/vector => pitch = atan2(x,vector) ..however this method limits pitch/roll calculation to 90degrees.. can't think of ways to get around this

if we use one axis i.e euler[1] = asin( ax / -g ); // Pitch, then it's also limited to 90degrees, but in fact limited to 45degrees because the sensitivity is only linear at +-45degrees.

[Tom]

In the gluon firmware code I wrote 2 functions to correctly map accelerometer readings to a correct pitch and roll angle:

Code: Select all

/*!
 *   Calculates roll using accelerometer input.
 *
 *   Makes sure the output is similar to the quaternion's output.
 */
inline double gravity_to_roll(double a_y, double a_z)
{
        double roll_acc = atan(a_y / a_z);
        if (a_z > 0.0)
        {
                if (a_y < 0.0) 
                        roll_acc =  roll_acc + 3.14159;
                else
                        roll_acc =  roll_acc - 3.14159;
        }       
        return roll_acc;       
}       


/*!
 *   Calculates pitch using accelerometer input.
 *
 *   Makes sure the output is similar to the quaternion's output.
 */
inline double gravity_to_pitch(double a_x, double a_z)
{
        double pitch_acc = -atan(a_x / a_z);

        if (a_z > 0.0)
                pitch_acc =  -pitch_acc;

        return pitch_acc;
}


This makes sure the quaternion to pitch/roll has the same "euler-angle-convention" as the accelerometer's euler angles.

[pizza]

thanks tom, does the equations need quaternions to work?
i tried your code, except converting to degrees..
pitch = atan(x,z) gives 45degrees when it is pitched 90degrees physically...
acc roll starts off at about -359degrees on ground, when rolled in positive direction it gives up to -315degrees, then jumped to 45degrees(when rolled 90degrees physically).. when in the negative direction, it gives 315degrees max then jumped to -45degrees(when rolled 90degrees physically).

Code: Select all

...
static const int accXzero = 1950;
static const int accYzero = 2150;
static const int accZzero = 1970;
...
//inside adc interrupt:
   raw_accX=(S12AD.ADDRA); //get values from adc registers
   raw_accY=(S12AD.ADDRB);
   raw_accZ=(S12AD.ADDRC);
   raw_gyroX=(S12AD.ADDRD);
   raw_gyroY=(S12AD.ADDRE);
   raw_gyroZ=(S12AD.ADDRF);
   
   accX = ((raw_accX-accXzero)*3.3/4095)/800*1000;
   accY = ((raw_accY-accYzero)*3.3/4095)/800*1000;
   accZ = ((raw_accZ-accZzero)*3.3/4095)/800*1000;

   acc_pitch = -atan(accX,accZ)*degree;
   acc_roll = atan(accY,accZ)*degree;
   
   if(accZ>0)
   {
      acc_pitch=-acc_pitch;
      if(accY<0)
         acc_roll+=360;
      else
         acc_roll-=360;   
   }
...


[Tom]

There must be something wring with your code!

atan(x, z) can only be 45° when x = y, which is impossible at a pitch of 90°.

Please write your acc x, y and z to screen, and check whether you get correct values.

acc z is maximal when turned upside down (pitch 180°)
acc x is maximal when pitched up 90°

[pizza]

thanks I have spotted my mistake! it shouldn't be atan(x,z) because atan() takes only one argument..so it should be atan(x/z)

however, pitch can go from 0 degrees to 90degrees, then as the device continued pitching, it falls back to 89,88,87...0..
roll falls from 360degrees to 270degrees then jumps to 90 degrees...

sorry i'm still not enlightened,
is this method supposed to work without using quaternions?

i did not use quaternions like in your code:

Code: Select all

       
double g_ax = sensor_data.acc_x;// - sensor_data.q * w / G;
double g_ay = sensor_data.acc_y - (sensor_data.r * u/* - sensor_data.p * w*/) / G;
double g_az = sensor_data.acc_z + (sensor_data.q * u / G);

roll_acc = gravity_to_roll(g_ay, g_az);
pitch_acc = gravity_to_pitch(g_ax, g_az);


[Tom]

The code didn't contain quaternions

p, q and r are rotation rates. They are used to calculated the dynamics (centripel force). In static conditions (on your bench), you can ignore them.