Pixhawk accelerometer and compass calibration though pymavlink

I am continuing my mavlink message journey and wanted to learn how the pixhawk or QGC does the calibration. After seeing this post here and @EliotBR 's response, I tried to calibrate the pixhawk by sending a command long message with the MAV_CMD_ACCELCAL_VEHICLE_POS command from the ardupilot dialect according to the mavlink website here.

But, when I tried this with my pixhawk, I couldn’t get the calibration to start. I looked at the command_ack messages to see what was being sent and I am getting a Mav_result response of 4, meaning the command is correct, but it failed nonetheless. What could be the reason for this?

The first command_ack message with the 0 result is from the disarm command by the way.

This is the mavlink command I sent:

master.mav.command_long_send(
    master.target_system, master.target_component,
    mavutil.mavlink.MAV_CMD_ACCELCAL_VEHICLE_POS, 0,
    0,  # Acceleration Calibration
    0, 0, 0, 0, 0, 0)

I sent these messages one by one, only changing the 0 with these values and then physically positioned my pixhawk.

mavutil.mavlink.ACCELCAL_VEHICLE_POS_LEVEL
mavutil.mavlink.ACCELCAL_VEHICLE_POS_LEFT
mavutil.mavlink.ACCELCAL_VEHICLE_POS_RIGHT
mavutil.mavlink.ACCELCAL_VEHICLE_POS_NOSEDOWN
mavutil.mavlink.ACCELCAL_VEHICLE_POS_NOSEUP
mavutil.mavlink.ACCELCAL_VEHICLE_POS_BACK

Hi @VioletCheese,

You’re definitely getting in quite deep here :slight_smile:

Given the code can be confusing to navigate, I’ll try to lay out my own steps when finding the functionality. Hopefully that can serve as an example of the reasoning process for anyone looking to find other similar information in the source :slight_smile:

Accelerometer Calibration

The MAV_CMD_ACCELCAL_VEHICLE_POS command says it’s “Used when doing accelerometer calibration.” That’s a bit ambiguous, but given it’s not working for you then the “when doing” likely implies the calibration has to be started with a different message, and the vehicle pos command is just for stepping through the positions during the calibration.

  1. From the QGC source file that was linked to in the other post,
  2. the APMSensorsComponentController.cc sets up the accelerometer calibration,
  3. then delegates it to the vehicle (_vehicle->startCalibration()).
  4. Looking in Vehicle.cc (where vehicle functionality is defined),
  5. the calibration is started using the MAV_CMD_PREFLIGHT_CALIBRATION command.

Compass Calibration

  1. For the compass calibration, the definition in APMSensorsComponentController.cc uses MAV_CMD_DO_CANCEL_MAG_CAL to determine whether the flight computer supports onboard compass calibration.
  2. Looking at the nearby commands in the message set comes up with the MAV_CMD_DO_START_MAG_CAL, which is presumably used to start magnetometer (compass) calibration (so we have something to look for).
  3. The calibrateCompass method (from step 1) hands off logic to the _mavCommandResult method in the same file,
  4. where you can see that MAV_CMD_DO_START_MAG_CAL is indeed used if onboard calibration is supported,
  5. which from a search of MAG_CAL in the ArduPilot codebase
  6. seems to be the case as of at least ArduSub 4.0 (probably can stop here, and just use that message).
  1. Where onboard calibration isn’t supported, it instead uses _compassCal.startCalibration().
  2. I wasn’t sure where that was defined, but it wasn’t in that file so
  3. I checked the corresponding header file, which #includes APMCompassCal.h.
  4. Looking in the corresponding source file,
  5. APMCompassCal.cc defines the function we’re after, as well as the various calculations that are used to perform a compass calibration externally.
  6. The code there is quite convoluted, so I’m not certain how it tracks the calibration state (if it even does - it seems like it might just communicate via string/text messages).
  1. Focusing back on the more promising approach, to confirm whether the MAG_CAL messages work I tried following the QGC approach from step 3. - sending a CANCEL_MAG_CAL command and seeing what kind of MAV_RESULT the command is acknowledged with (as per the Command Protocol):
    from pymavlink import mavutil
    
    master = mavutil.mavlink_connection(...)
    master.wait_heartbeat()
    
    command = mavutil.mavlink.MAV_CMD_DO_CANCEL_MAG_CAL
    master.command_long_send(master.target_system, master.target_component,
                             command, 0,
                             0, # all
                             0,0,0,0,0,0)
    message = master.recv_match(type='COMMAND_ACK', blocking=True)
    assert message.command == command, 'wrong ACK - try again'
    assert message.result != mavutil.mavlink.MAV_RESULT_UNSUPPORTED, \
           'onboard calibration not supported :('
    print('onboard calibration supported :-)')
    
  2. On ArduSub 4.0.3 (the latest stable) that worked fine, so onboard calibration should be available, and you shouldn’t need to worry about implementing external calibration :slight_smile:

That said, unless you’re trying to make a full QGC replacement it’s likely easier to calibrate sensors via QGC instead of directly over MAVLink, because it already has a useful interface, and related functionality in the same place (e.g. the “autopilot rotation” when about to calibrate accelerometer/compass).

1 Like

As an aside, the software team have suggested an alternative and potentially simpler approach to finding out this information (and its direct usage in Pymavlink) would be to look at mavproxy’s implementation instead. This file was the first result in a search for “calibration”.

Also,