Sensor calibration

Theory

Accelerometer and Magnetometer calibration is critical to the performance of the attitute and headinge estimation algorithm and can be performed using no special hardware. The gyroscope doesn’t have to be calibrated, because we are estimating its bias online. For the magnetometer, it is very important that the calibration be performed in the fully assembled vehicle, with all systems powered on. This is called hard-iron calibration and will allow us to compensate for any constant parasitic magnetic fields generated by the vehicle. The accelerometer calibration on the other hand should be performned only with the Pixhawk board, before it is integrated in the vehicle.

The calibration process consists of finding a set of neutrals and scale factors for each sensor, such as

More details about the theory can be found at Paparazzi Wiki. Now we jump right to the accelerometer.

Accelerometer calibration

The principle of the calibration is the following: an accelerometer, on a vehicle at rest, measures a constant vector (the opposite of gravity) in the earth frame, expressed in the vehicle frame. Written down this looks like:

DCM is a rotation matrix that converts between earth frame and body frame. It will change when we change the orientation of the vehicle. Nevertheless, a rotation conserves the norm of a vector, so we cab obtain the following scalar equation that doesn’t depend on the vehicle orientation:

Where 9.81 is earths average gravity value. We can then record an important number of measurements in different orientations and find the set of scale factors and neutrals giving the norm closest to the average gravity value. Again, more details can be found at Paparazzi Wiki.

Preparation

It is very important to align the gravity vector with each of the accelerometer axis (not the body axis of the airfame). Even a small imperfection in the calibration can lead to a few degree offset in the orientation estimate, so be very careful and follow the instructions precisely.

If you have the Pixhawk mounted in the Iris (which you probably have):

  1. Download these two files from Paparazzi autopilot project: calibrate.py and calibration_utils.py and save them to smaccmpilot-stm32f4/src/ivory-px4-hw/test-client/calibration/. (We cannot include these files in the SMACCMPilot repositories since they are GPL-licensed). You may need to install the Python scipy and matplotlib modules for these scripts to work, e.g., pip install scipy matplotlib.
  2. Apply the patch file for calibration_utils.py:

    $ cd smaccmpilot-stm32f4/src/ivory-px4-hw/test-client/calibration
    $ patch < calibration_utils.patch
  3. Open Iris airfame
  4. Unplug all connectors from the Pixhawk
  5. Unscrew the top of the Pixhawk
  6. Unscrew the PCB inside (NOTE: remember position and orientation of the servo connectors in the back of the board before you unplug them)
  7. Place the Pixhawk PCB into a separate enclosure and secure it with at least two screws
  8. Plug in a micro USB cable into the USB port on Pixhawk, and a FTDI USB-to-serial cable to the TELEM1 port

Procedure

First we have to uplouad all_sensors_test firmware

  1. Make sure that smaccmpilot-stm32f4/src/ivory-px4-hw/calibration.conf file has all mpu6000 accelerometer offsets to zero and all axis scales to 1.0 (or check git diff calibration.conf to make sure there are no local changes)
  2. cd smaccmpilot-build/smaccmpilot-stm32f4/src/ivory-px4-hw
  3. make clean; make platform-fmu24/px4-all-sensors-test-gen
  4. cd platform-fmu24/px4-all-sensors-test
  5. python px_uploader.py --port /dev/ttyACM0 image.px4 (on Linux) For OS X use your own port name.

Now we have uploaded the firmware that outputs raw sensors readings (you must have the default calibration.conf). During the calibration, we will move the pixhawk so each of the 6 sides is facing downwards at some point, as shown in the image below:

  1. cd smaccmpilot-stm32f4/src/ivory-px4-hw/test-client
  2. python sensorsmonitor-calibration.py /dev/ttyUSB0 to check that you are getting data
  3. you should see something like

    205.343 66 IMU_ACCEL_RAW 72 -790 -4210
    205.364 66 IMU_MAG_RAW -2216 -2639 7920
    205.363 66 IMU_ACCEL_RAW 64 -781 -4217
    205.384 66 IMU_MAG_RAW -2199 -2574 8012
    205.383 66 IMU_ACCEL_RAW 68 -787 -4229
    205.404 66 IMU_MAG_RAW -2191 -2610 7959
    205.403 66 IMU_ACCEL_RAW 65 -788 -4219
    205.424 66 IMU_MAG_RAW -2169 -2595 7959
  4. now lets start recording data python sensorsmonitor-calibration.py /dev/ttyUSB0 > accelCal1.data (replace accelCal1.data with your own file name).
  5. immediately after you start recording, move the Pixhawk into each of the positions shown above and keep it there in a steady state for 6-10 seconds.
  6. once finished, stop recording (Ctrl+C)
  7. run calibration script on recorded data ./calibration/calibrate.py -s ACCEL -p -v accelCal1.data
  8. you will see something like

    reading file accelCal1.data for aircraft 66 and sensor ACCEL
    found 1167 records
    Using noise threshold of 409.562238209 for filtering.
    remaining 671 after filtering
    initial guess : avg 9.7076822523 std 0.0881058170084
    optimized guess : avg 9.80994055284 std 0.0241489791359

    The avg value should be almost exactly 1g, while the std should be very small (as shown above). If that is not the case, you have to repeat the calibration.

    <define name="ACCEL_X_NEUTRAL" value="49"/>
    <define name="ACCEL_Y_NEUTRAL" value="-106"/>
    <define name="ACCEL_Z_NEUTRAL" value="-139"/>
    <define name="ACCEL_X_SENS" value="2.45582430418" integer="16"/>
    <define name="ACCEL_Y_SENS" value="2.44793341773" integer="16"/>
    <define name="ACCEL_Z_SENS" value="2.41664395911" integer="16"/>
    And a plot like this one: Accelerometer calibration results
  9. In the top part you see a plot of our raw accel readings (notice the y-axis scale). We are trying to scale those measurements to be between -1g and +1g. The two plots in the right show the norm of accel, which as you might have guessed by now should be exactly 9.8 m/s^2. If that is not the case, you have to repeat the calibration.
  10. copy the accelerometer offsets and scales into your smaccmpilot-stm32f4/src/smaccm-flight/calibration.conf file - below [calibration.mpu6000.accelerometer] section. The X_NEUTRAL value goes to x_offset and so on, and X_SENS goes to x_scale (and so on)
  11. cd smaccmpilot-stm32f4/src/smaccm-flight
  12. make clean; make flight_echronos to build the flight image
  13. make upload_flight_echronos to upload the image
  14. place the Pixhawk on a flat surface and make sure it stays flat
  15. start the GCS and verify that the artificial horizon is also almost perfectly flat, something like this: If that is not the case, you have to repeat the calibration.
  16. check http://localhost:8080/sensors_accel.html and verify that you are reading -9.8 m/s^2 in the z-axis and almost zeros in the other axis, something like this:
  17. If you are happy with the results, mount the Pixhawk back to the Iris to get it ready for the magnetometer calibration.

Magnetometer

The goal of the magnetometer calibration is to offset and scale the readings in such a way that the magnetometer outputs a normalized magnetic vector. Why? Because we are using a normalized earh magnetic field vector for heading esitmation.

Preparation

First of all it is important to know that all ferromagnetic materials near the mag distort the measurements. So preferably you do the mag calibration with the mag/autopilot mounted in your frame and as far away from metal and magnets as possible.

Procedure

First we have to uplouad all_sensors_ext_mag_test firmware. This firmware is different for the external (HMC5883L) magnetometer. If you want to calibrate for the internal (LSM303D) magnetometer (default), you can skip to step 9.

  1. Make sure that smaccmpilot-stm32f4/src/ivory-px4-hw/calibration.conf file has magnetometer offsets to zero and magnetometer axis scales to 1.0 (or check git diff calibration.conf to make sure there are no local changes)
  2. cd smaccmpilot-build/smaccmpilot-stm32f4/src/ivory-px4-hw
  3. make clean; make platform-fmu24/px4-all-sensors-ext-mag-test-gen
  4. cd platform-fmu24/px4-all-sensors-test
  5. python px_uploader.py --port /dev/ttyACM0 image.px4 (on Linux) For OS X use your own port name.

For the hard-iron calibration, we need to move the magnetometer with all axis around a sphere. Think about it as if you wanted to paint inside of a sphere with the point of the Iris (x-axis). HINT: sequentially align the Iris as if you were doing the accelerometer calibration, and then in each position rotate the Iris slowly all the way around its vertical axis and back. It might need some practice, but fortuately we have to do this calibration only once.

  1. cd smaccmpilot-stm32f4/src/ivory-px4-hw/test-client
  2. python sensorsmonitor-calibration.py /dev/ttyUSB0 to check that you are getting data
  3. you should see something like

    205.343 66 IMU_ACCEL_RAW 72 -790 -4210
    205.364 66 IMU_MAG_RAW -2216 -2639 7920
    205.363 66 IMU_ACCEL_RAW 64 -781 -4217
    205.384 66 IMU_MAG_RAW -2199 -2574 8012
    205.383 66 IMU_ACCEL_RAW 68 -787 -4229
    205.404 66 IMU_MAG_RAW -2191 -2610 7959
    205.403 66 IMU_ACCEL_RAW 65 -788 -4219
    205.424 66 IMU_MAG_RAW -2169 -2595 7959
  4. now lets start recording data python sensorsmonitor-calibration.py /dev/ttyUSB0 > magCal1.data (replace magCal1.data with your own file name).
  5. immediately after you start recording, start with the hard-iron calibration (see above how to do so).
  6. once finished, stop recording (Ctrl+C)
  7. run calibration script on recorded data ./calibration/calibrate.py -s MAG -p -v magCal1.data
  8. you will see something like

    Using aircraft id 66
    reading file magCal1.data for aircraft 66 and sensor MAG
    found 3985 records
    Using noise threshold of 3651.05502967 for filtering.
    remaining 3965 after filtering
    initial guess : avg 0.966739941734 std 0.102544232524
    optimized guess : avg 0.997178254747 std 0.0530451037009

    The avg should be as close to 1.0 as possible, and std as small as possible too.

    <define name="MAG_X_NEUTRAL" value="397"/>
    <define name="MAG_Y_NEUTRAL" value="-281"/>
    <define name="MAG_Z_NEUTRAL" value="756"/>
    <define name="MAG_X_SENS" value="0.388798741374" integer="16"/>
    <define name="MAG_Y_SENS" value="0.382649033587" integer="16"/>
    <define name="MAG_Z_SENS" value="0.396648366431" integer="16"/>
    and plots like this:
  9. Notice the two plots at the bottom right: we want the norm of the mag vector to be 1. Hard Iron Calibration (left-uncalibrated) (right-fitted to a sphere) Here we see the measurement points we took - if the sphere is not covered consistently or have holes, it means that you didn’t paint the whole sphere. Note that something like this: is most likely also fine.
  10. copy the magnetometer offsets and scales into your smaccmpilot-stm32f4/src/smaccm-flight/calibration.conf file - below [calibration.hmc5883l.magnetometer] (external mag) or [calibration.lsm303d.magnetometer] (internal mag). The X_NEUTRAL value goes to x_offset, X_SENS goes to x_scale, and so on.
  11. cd smaccmpilot-stm32f4/src/smaccm-flight
  12. make clean; make flight_echronos to build the flight image
  13. make upload_flight_echronos to upload the image
  14. point the Iris towards North (ideally) or to some known heading
  15. start the GCS and verify that the heading is stable and points to approximately correct azimuth (NOTE the real vs. estimated heading might differ depending on your latitude). If that is not the case, you have to repeat the calibration.
  16. check http://localhost:8080/sensors_mag.html and verify that your readings are between -1 and 1 no matter what the orientation of the Iris is
  17. check http://localhost:8080/sensors.html and see if your yaw is stable. Try to move the Iris left and right and see if you get back to the original heading (at least approximately).
  18. If you are happy with the results, you are good to fly.