From c21cfb7b28051b6d9a33c6ba578a256ad90c8baa Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Thu, 10 Mar 2016 10:04:46 -0800
Subject: [PATCH] Fix ArduPilot compass cal

---
 src/AutoPilotPlugins/APM/APMCompassCal.cc | 12 +++++-------
 src/Vehicle/Vehicle.cc                    | 21 ++++++++++++++++++++-
 src/Vehicle/Vehicle.h                     |  1 +
 3 files changed, 26 insertions(+), 8 deletions(-)

diff --git a/src/AutoPilotPlugins/APM/APMCompassCal.cc b/src/AutoPilotPlugins/APM/APMCompassCal.cc
index 07fa826d66..787d4d5aa3 100644
--- a/src/AutoPilotPlugins/APM/APMCompassCal.cc
+++ b/src/AutoPilotPlugins/APM/APMCompassCal.cc
@@ -143,18 +143,16 @@ CalWorkerThread::calibrate_return CalWorkerThread::calibrate(void)
         free(worker_data.z[cur_mag]);
     }
 
-    AutoPilotPlugin* plugin = _vehicle->autopilotPlugin();
     if (result == calibrate_return_ok) {
         for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
             if (rgCompassAvailable[cur_mag]) {
                 _emitVehicleTextMessage(QString("[cal] mag #%1 off: x:%2 y:%3 z:%4").arg(cur_mag).arg(-sphere_x[cur_mag]).arg(-sphere_y[cur_mag]).arg(-sphere_z[cur_mag]));
 
-                const char* offsetParam = rgCompassParams[cur_mag][0];
-                plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_x[cur_mag]);
-                offsetParam = rgCompassParams[cur_mag][1];
-                plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_y[cur_mag]);
-                offsetParam = rgCompassParams[cur_mag][2];
-                plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_z[cur_mag]);
+                if (cur_mag != 2) {
+                    float sensorId;
+                    sensorId = cur_mag == 0 ? 2.0f : 5.0f;
+                    _vehicle->doCommandLong(0, MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS, sensorId, -sphere_x[cur_mag], -sphere_y[cur_mag], -sphere_z[cur_mag]);
+                }
             }
         }
     }
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 0884e89de5..1bc0345734 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -1485,12 +1485,31 @@ void Vehicle::emergencyStop(void)
     cmd.param7 = 0.0f;
     cmd.target_system = id();
     cmd.target_component = 0;
-
     mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &cmd);
 
     sendMessage(msg);
 }
 
+void Vehicle::doCommandLong(int component, MAV_CMD command, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
+{
+    mavlink_message_t       msg;
+    mavlink_command_long_t  cmd;
+
+    cmd.command = command;
+    cmd.confirmation = 0;
+    cmd.param1 = param1;
+    cmd.param2 = param2;
+    cmd.param3 = param3;
+    cmd.param4 = param4;
+    cmd.param5 = param5;
+    cmd.param6 = param6;
+    cmd.param7 = param7;
+    cmd.target_system = id();
+    cmd.target_component = component;
+    mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &cmd);
+
+    sendMessage(msg);
+}
 
 const char* VehicleGPSFactGroup::_hdopFactName =                "hdop";
 const char* VehicleGPSFactGroup::_vdopFactName =                "vdop";
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 9eaed890d8..560b4ba9c5 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -504,6 +504,7 @@ public:
     static const int cMaxRcChannels = 18;
 
     bool containsLink(LinkInterface* link) { return _links.contains(link); }
+    void doCommandLong(int component, MAV_CMD command, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
 
 public slots:
     void setLatitude(double latitude);
-- 
GitLab