It’s been almost a year since my last post as the project was on a pause for a while.

In this this post I will cover the most important piece of the quadcopter – position sensor. This sensor calculates Roll/Pitch/Yaw angles using Android’s Rotation Vector Sensor and is crucial part in it’s self-stabilization and control.

What I was trying to achieve:

- Calculate Roll/Pitch/Yaw angles
- The device is rotated in such a way to let the camera face forward. This means XYZ axes needed to be reassigned to get correct reading of roll/pitch/yaw
- It’s not always possible to perfectly align the device with ground and north pole, so the absolute angles will be different from 0. I needed a way to reset angles to 0 and thus calculate relative instead of absolute values

As I mentioned already the implementation is based on **Rotation Vector** Sensor. It is a virtual sensor which combines data from accelerometer and gyro to calculate device position. So it takes best of the two worlds – speed from the gyro, and no gyro drift.

### Basic implementation

Let’s now calculate Euler angles from the rotation vecto**r:**

private float[] rMatrix = new float[9]; /** * @param result the array of Euler angles in the order: yaw, roll, pitch * @param rVector the rotation vector */ public void calculateAngles(float[] result, float[] rVector){ //caculate rotation matrix from rotation vector first SensorManager.getRotationMatrixFromVector(rMatrix, rVector); //calculate Euler angles now SensorManager.getOrientation(rMatrix, result); //The results are in radians, need to convert it to degrees convertToDegrees(result); } private void convertToDegrees(float[] vector){ for (int i = 0; i < vector.length; i++){ vector[i] = Math.round(Math.toDegrees(vector[i])); } }

As I mentioned above the elements of the result vector come in order: yaw, roll, pitch.

### Change Device Orientation

But what if you need to change orientation of the device, such as flip it so the camera face forward like in my case:

It can result in changing the axes along which you calculate the rotation. Luckily it’s very easy to fix, with Android API. You just need to use method `SensorManager.remapCoordinateSystem(inR, NEW_AXIS_X, NEW_AXIS_Y, outR);`

Now we can modify code from above to apply different orientation.

private float[] rMatrix = new float[9]; private float[] tempRMatrix = new float[9]; /** * @param result the array of Euler angles in the order: yaw, roll, pitch * @param rVector the rotation vector */ public void calculateAngles(float[] result, float[] rVector){ //caculate temp rotation matrix from rotation vector first SensorManager.getRotationMatrixFromVector(tempRMatrix, rVector); //translate rotation matrix according to the new orientation of the device SensorManager.remapCoordinateSystem(tempRMatrix, remapX, remapY, rMatrix); //calculate Euler angles now SensorManager.getOrientation(rMatrix, result); //Now we can convert it to degrees convertToDegrees(result); }

The results you will get

- Yaw in the range of [-180,180]
- Roll in the range of [-90,90]
- Pitch in the range of [-180,180]

### Translate Angles from Device’s to Quadcopter’s Coordinate System

It’s not always possible to place device in such a way that it’s coordinate system is perfectly aligned with the quadcopter’s one. So what we need to do is just to translate one into another. But first I want to focus on translating angles (any integer) to [-180, 180] range and then calculating delta.

First let’s look how we can translate any angle in degrees to [-180,180]. This means angle of 340 degrees will become -20 and -181 will become 179.

/** * Translates given angle (degrees) into [-180,180] range * @param angle the given angle to translate * @return the translated angle */ public static int translateAgle(int angle){ if (angle == 0) return 0; int d = angle/180; if (d%2 == 0){ return angle%180; } int signum = Math.abs(angle)/angle; return angle%180 - signum*180; }

Now let’s say we have two readings of the Pitch angle: *Pitch_1* [-179] and *Pitch_2* [179]. We need to find the smallest correction to apply to *Pitch_1* to get *Pitch_2*, let’s call this correction *delta*. Positive delta means you are going clockwise, negative – countercloskwise. For example the actual delta for *Pitch_2 and Pitch_1* (i.e. *Pitch_2 – Pitch_1*)is -2, but not 358.

It’s very easy now to calculate *delta* using *translateAgle()* method:

/** * Calculates minimal angle difference (left - right) between two angles * @param left the left angle * @param right the right angle * @return the delta */ public static int angleDiff(int left, int right){ left = translateAgle(left); right = translateAgle(right); return translateAgle(left - right); }

Let’s put all the pieces together and translate the angles in Quadcopter Coordinate System. Though I want to mention the solution with calculating deltas is not ideal and doesn’t work well when quadcopter CS is significantly rotated vs device’s CS (because the Roll we get is in the range [-90,90] so you will get incorrect results getting closer to extremes). The better approach is to use rotation matrix multiplication, however I didn’t have much time to play with it, and my current solution works just well for fine tuning, just what I needed.

private float[] rMatrix = new float[9]; private float[] tempRMatrix = new float[9]; /** * @param result the array of Euler angles in the order: yaw, roll, pitch * @param rVector the rotation vector * @param referenceAngles the Euler angles of the reference position */ public void calculateAngles(float[] result, float[] rVector, int[] referenceAngles){ //caculate temp rotation matrix from rotation vector first SensorManager.getRotationMatrixFromVector(tempRMatrix, rVector); //translate rotation matrix according to the new orientation of the device SensorManager.remapCoordinateSystem(tempRMatrix, remapX, remapY, rMatrix); //calculate Euler angles now SensorManager.getOrientation(rMatrix, result); //Now we can convert it to degrees applyDeltaAndCovert(result, referenceAngles); } /** * @param result the array of Euler angles which need to be translated * @param reference the array of Euler angles of reference coordinate system */ private void applyDeltaAndConvert(float[] result, float[] reference){ for (int i = 0; i < result.length; i++) { //convert from radians to degree int cur = (int)Math.round(Math.toDegrees(result[i])); int ref = reference[i]; result[i] = angleDiff(cur, ref); } }

The full implementation can be found here.