Copter: updates for compass API change

ArduCopter/GCS_Mavlink.pde

@@ -268,7 +268,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
 
 static void NOINLINE send_ahrs(mavlink_channel_t chan)
 {
-    Vector3f omega_I = ahrs.get_gyro_drift();
+    const Vector3f &omega_I = ahrs.get_gyro_drift();
     mavlink_msg_ahrs_send(
         chan,
         omega_I.x,
@@ -450,8 +450,9 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
 
 static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
 {
-    Vector3f accel = ins.get_accel();
+    const Vector3f &accel = ins.get_accel();
-    Vector3f gyro = ins.get_gyro();
+    const Vector3f &gyro = ins.get_gyro();
+    const Vector3f &mag = compass.get_field();
     mavlink_msg_raw_imu_send(
         chan,
         micros(),
@@ -461,9 +462,9 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
         gyro.x * 1000.0f,
         gyro.y * 1000.0f,
         gyro.z * 1000.0f,
-        compass.mag_x,
+        mag.x,
-        compass.mag_y,
+        mag.y,
-        compass.mag_z);
+        mag.z);
 }
 
 static void NOINLINE send_raw_imu2(mavlink_channel_t chan)

ArduCopter/Log.pde

@@ -351,13 +351,14 @@ struct PACKED log_Compass {
 // Write a Compass packet
 static void Log_Write_Compass()
 {
-    Vector3f mag_offsets = compass.get_offsets();
+    const Vector3f &mag_offsets = compass.get_offsets();
-    Vector3f mag_motor_offsets = compass.get_motor_offsets();
+    const Vector3f &mag_motor_offsets = compass.get_motor_offsets();
+    const Vector3f &mag = compass.get_field();
     struct log_Compass pkt = {
         LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
-        mag_x           : compass.mag_x,
+        mag_x           : (int16_t)mag.x,
-        mag_y           : compass.mag_y,
+        mag_y           : (int16_t)mag.y,
-        mag_z           : compass.mag_z,
+        mag_z           : (int16_t)mag.z,
         offset_x        : (int16_t)mag_offsets.x,
         offset_y        : (int16_t)mag_offsets.y,
         offset_z        : (int16_t)mag_offsets.z,

ArduCopter/motors.pde

@@ -273,7 +273,7 @@ static void pre_arm_checks(bool display_failure)
         }
 
         // check for unreasonable mag field length
-        float mag_field = pythagorous3(compass.mag_x, compass.mag_y, compass.mag_z);
+        float mag_field = compass.get_field().length();
         if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35) {
             if (display_failure) {
                 gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field"));

ArduCopter/setup.pde

@@ -189,9 +189,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
     }
 
     // store initial x,y,z compass values
-    compass_base.x = compass.mag_x;
+    compass_base = compass.get_field();
-    compass_base.y = compass.mag_y;
-    compass_base.z = compass.mag_z;
 
     // initialise motor compensation
     motor_compensation = Vector3f(0,0,0);
@@ -234,18 +232,14 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
 
             // if throttle is zero, update base x,y,z values
             if( throttle_pct == 0.0f ) {
-                compass_base.x = compass_base.x * 0.99f + (float)compass.mag_x * 0.01f;
+                compass_base = compass_base * 0.99f + compass.get_field() * 0.01f;
-                compass_base.y = compass_base.y * 0.99f + (float)compass.mag_y * 0.01f;
-                compass_base.z = compass_base.z * 0.99f + (float)compass.mag_z * 0.01f;
 
                 // causing printing to happen as soon as throttle is lifted
                 print_counter = 49;
             }else{
 
                 // calculate diff from compass base and scale with throttle
-                motor_impact.x = compass.mag_x - compass_base.x;
+                motor_impact = compass.get_field() - compass_base;
-                motor_impact.y = compass.mag_y - compass_base.y;
-                motor_impact.z = compass.mag_z - compass_base.z;
 
                 // throttle based compensation
                 if( comp_type == AP_COMPASS_MOT_COMP_THROTTLE ) {

ArduCopter/test.pde

@@ -142,15 +142,16 @@ test_compass(uint8_t argc, const Menu::arg *argv)
             counter++;
             if (counter>20) {
                 if (compass.healthy) {
-                    Vector3f maggy = compass.get_offsets();
+                    const Vector3f &mag_ofs = compass.get_offsets();
-                    cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
+                    const Vector3f &mag = compass.get_field();
-                                    (wrap_360_cd(ToDeg(heading) * 100)) /100,
+                    cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
-                                    (int)compass.mag_x,
+                                        (wrap_360_cd(ToDeg(heading) * 100)) /100,
-                                    (int)compass.mag_y,
+                                        mag.x,
-                                    (int)compass.mag_z,
+                                        mag.y,
-                                    maggy.x,
+                                        mag.z,
-                                    maggy.y,
+                                        mag_ofs.x,
-                                    maggy.z);
+                                        mag_ofs.y,
+                                        mag_ofs.z);
                 } else {
                     cliSerial->println_P(PSTR("compass not healthy"));
                 }