=============== Control Utility 1: ==============

......... Calculate Pitch, Roll and Yaw .........

One of the key functions to control the flight of a drone is the Compass function. The function in Sec.4 named "calc_pitch_roll_yaw.ino" is the implementation of a tilt compensated compass as discussed in [Ref.7]. This function calculates the Pitch-, Roll- and Yaw-, values used as input to the relevant PID controller.
The implemented Compass function will be discussed below. On behalf of this discussion the function is split in the following 3 Steps:
STEP 1: Sensordata Reading.
STEP 2: Sensordata Processing and Calibration.
STEP 3: Pitch, Roll and Yaw calculations.


STEP 1: Sensordata Reading.
In this first step the raw x,y and z values of the accelerometer, gyro and magnetometer are read.
The reading of the MPU6050 accelerometer and gyro data is done with a for-loop. After each call to the sensor reading function "sensor_read_stuff.ino" a low-pass filter is applied to the data. With a weight-factor the filter calculates a weighted average involving previous readings. In this way the values are smoothed at the expense of a small delay. The filtered x, y, z results of accel.and gyro are accumulated over the for-loop and stored. The stored gyro data are corrected with the drift as determined in Sec4 file "Gyro_Drift_Calcul.ino" .
In case of the magnetometer MAG3110, one of the calibration steps is to correct the readings with the offset values determined at the calibration. The MAG3110 has an option [Ref.10] to apply this correction on the sensor-chip. This option is invoked as part of the sensor configuration function "configure_mag3110_magnetometer.ino" . In CTRL_REG2 bit 5 (RAW) is set to 0. The coordinate axis offset is corrected by user set register values,
Like with the accelerometer and gyro, in a for-loop the offset corrected magnetometer values are read with a function call to "sensor_read_stuff.ino" ., accumulated in the loop and finally stored.
It should be noted, sofar all x, y and z readings are bit-values.

STEP 2: Sensordata Processing and Calibration.
In this second step the stored x, y and z Bit-values of Accelerometer, Gyro and Magnetometer are processed. The sensor calibration data as discussed and derived in Section 2 for the different sensors will be used in this data processing.
For consitency of the results the sensor orientation as shown in "Fig.3 Sensor Orientation of Wooden Cube." The arrow labeled "Nose" in Fig.3 marks the sensor orientation on the drone body.
Accelerometer: The accumulated x, y and z Bitvalues from Step-1 are averaged, and converted to g-values. The appropriate conversion factor from Bit-value to physical value can be found in the MPU6050 Data-Sheet [Ref.11]. Finally the data are calibrated with the shift-vector and calibration matrices stored in Sec4 file "calibration_data_mpu6050.ino"
Gyro: The accumulated x, y and z Bitvalues from Step-1 are averaged, and converted to angular rates in deg/sec. The conversion factor can be found in the MPU6050 Data-Sheet [Ref.11]. It should be noted that the measured angular rates of the gyro represent the angular velocity about the body-fixed (sensor) frame. This rate is not the same as the rate of change of the Euler angles!!
Magnetometer: The accumulated x, y and z Bitvalues from Section-1 are averaged. These averaged Bitvalues are then calibrated with Shift-vector and calibration matrix stored in Sec4 file "calibration_data_mag3110.ino"
Conversion of the magnetometer data to physical values is not required. The data are used in quotients only.

STEP 3: Pitch, Roll and Yaw calculations.
Based on the calibrated accelerometer x, y, z data the G-Value, the AccRoll and AccPitch are calculated. In [Ref.7] the theory and formula's behind these calculations can be found.
- In [Ref.7] the formula for the tilt compensated magnetic yaw is derived. With this formula and the calibrated magnetometer and accelerometer measurements from above Section 2 the value of MagYaw is calculated. This MagYaw angle is relative to magnetic north and measured in degrees.
_ Calculation of GyroPitch, GyroRoll and GyroYaw is not straight forward. As mentioned above, the measured gyro angular rates about the sensor body frame are not the same as the rate of change of the Euler angles GyroPitchdot, GyroRolldot and GyroYawdot. This rather complicated relation is derived in [Ref.9].
GyroPitch, GyroRoll and GyroYaw values are finally calculated by integration of the angular rate values with the looptime variable "Loop_T_secs". The time integration requires an initial condition. For this the calibrated initial Acceleromter and Magnometer results are used.


Discussion of Pitch, Roll and Yaw results. At each call to the function, the measured G-value is calculated from the calibrated accelerometer readings. A reading is the sum of gravity and acceleration. When the calculated value only slightly differs from a threshold Gvalue measured at rest, it is concluded that the x, y and z components are not impaired (parasitic) by acceleration. This means, low vibrations during the flight, and or a stationary uniform motion during the flight. It is reasonable to expect the flight character of an autonomous drone to be more stationary than rough. Motor vibrations need to be minimized by damping. In addition to the magnetometer MagYaw values, the accelerometer values AccRoll and AccPitch can be used for flight-control. The Gyro values GyroPitch and GyroRoll are set equal to AccPitch and AccRoll. These values are used as new initial values for the gyro time integration process. In case the threshold value is exceeded, flight-control is taken over by the Gyro readings. To indicate this change, the white LED starts blinking. Despite the fact that the gyro readings are corrected for drift, as measured with sec4 function "Gyro_Drift_Calcul.ino" , drift is still found in the 3 gyro channels. Some web-sites cure the gyro drift with a cloudy form of sensorfusion. A small portion of accelerator measurements is "injected" in the drifting gyro data, These sites claime the fusion process cures the gyro drift. The author of this site decided rather than using sensor-fusion to focus on vibration dampening the accelerometer.
There are two causes that may affect the G-value:
- Vibrations
- Drone Acceleration

In below pictures the rubber adapters for the flight-control unit , and a flexible motor mount are shown.

Drone-Built3-Fig.1: Damping Flight-Control Unit.

Drone-Built3-Fig.2: Motor Vibration Damping.

Next to flexible motor-mounts, further reduction of motor vibrations is achieved by balancing the propellors.

Drone-Built3-Fig.3: Prop Balancing.

Note: To avoid Gimballock, the pitch has to stay below +90 deg and above -90deg.
MagYaw (and Gyro_Yaw) are CW limited to the ranges 0 to 180 degrees and CCW from 0 to -180deg.