Fix defines around fixed-mag-cal-yaw

Tools/scripts/build_options.py

@@ -160,6 +160,8 @@ def __init__(self,
     Feature('Compass', 'QMC5883L', 'AP_COMPASS_QMC5883L_ENABLED', 'Enable QMC5883L compasses', 1, None),
     Feature('Compass', 'RM3100', 'AP_COMPASS_RM3100_ENABLED', 'Enable RM3100 compasses', 1, None),
     Feature('Compass', 'DRONECAN_COMPASS', 'AP_COMPASS_DRONECAN_ENABLED', 'Enable DroneCAN compasses', 0, None),
+    Feature('Compass', 'FixedYawCal', 'AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED', 'Enable Fixed-Yaw Compass Calibration', 1, None),  # noqa
+    Feature('Compass', 'CompassLearn', 'COMPASS_LEARN_ENABLED', 'Enable In-Flight Compass Learning', 1, "FixedYawCal"),
 
     Feature('Gimbal', 'MOUNT', 'HAL_MOUNT_ENABLED', 'Enable Mount', 0, None),
     Feature('Gimbal', 'ALEXMOS', 'HAL_MOUNT_ALEXMOS_ENABLED', 'Enable Alexmos Gimbal', 0, "MOUNT"),

Tools/scripts/extract_features.py

@@ -242,6 +242,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('FORCE_APJ_DEFAULT_PARAMETERS', 'AP_Param::param_defaults_data'),
             ('HAL_BUTTON_ENABLED', 'AP_Button::update'),
             ('HAL_LOGGING_ENABLED', 'AP_Logger::Init'),
+            ('AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED', 'AP_Compass::mag_cal_fixed_yaw'),
+            ('COMPASS_LEARN_ENABLED', 'CompassLearn::update'),
         ]
 
     def progress(self, msg):

libraries/AP_Compass/AP_Compass.h

@@ -39,7 +39,7 @@
 #define COMPASS_MOT_ENABLED 1
 #endif
 #ifndef COMPASS_LEARN_ENABLED
-#define COMPASS_LEARN_ENABLED 1
+#define COMPASS_LEARN_ENABLED AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
 #endif
 
 // define default compass calibration fitness and consistency checks
@@ -342,12 +342,14 @@ friend class AP_Compass_Backend;
 
     uint8_t get_filter_range() const { return uint8_t(_filter_range.get()); }
 
+#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
     /*
       fast compass calibration given vehicle position and yaw
      */
-    MAV_RESULT mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
-                                 float lat_deg, float lon_deg,
-                                 bool force_use=false);
+    bool mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
+                           float lat_deg, float lon_deg,
+                           bool force_use=false);
+#endif
 
 #if AP_COMPASS_MSP_ENABLED
     void handle_msp(const MSP::msp_compass_data_message_t &pkt);
@@ -405,12 +407,14 @@ friend class AP_Compass_Backend;
     // see if we already have probed a i2c driver by bus number and address
     bool _have_i2c_driver(uint8_t bus_num, uint8_t address) const;
 
+#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
     /*
       get mag field with the effects of offsets, diagonals and
       off-diagonals removed
     */
     bool get_uncorrected_field(uint8_t instance, Vector3f &field) const;
-
+#endif
+
 #if COMPASS_CAL_ENABLED
     //keep track of which calibrators have been saved
     RestrictIDTypeArray<bool, COMPASS_MAX_INSTANCES, Priority> _cal_saved;

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -504,7 +504,7 @@ bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const
 
   This assumes that the compass is correctly scaled in milliGauss
 */
-MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
+bool Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
                                       float lat_deg, float lon_deg, bool force_use)
 {
     _reset_compass_id();
@@ -514,7 +514,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
         if (!AP::ahrs().get_location(loc)) {
             if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) {
                 GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag: no position available");
-                return MAV_RESULT_FAILED;
+                return false;
             }
             loc = AP::gps().location();
         }
@@ -528,7 +528,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
     float inclination;
     if (!AP_Declination::get_mag_field_ef(lat_deg, lon_deg, intensity, declination, inclination)) {
         GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag: WMM table error");
-        return MAV_RESULT_FAILED;
+        return false;
     }
 
     // create a field vector and rotate to the required orientation
@@ -553,13 +553,13 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
         }
         if (!healthy(i)) {
             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: unhealthy\n", i);
-            return MAV_RESULT_FAILED;
+            return false;
         }
 
         Vector3f measurement;
         if (!get_uncorrected_field(i, measurement)) {
             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: bad uncorrected field", i);
-            return MAV_RESULT_FAILED;
+            return false;
         }
 
         Vector3f offsets = field - measurement;
@@ -572,7 +572,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
 #endif
     }
 
-    return MAV_RESULT_ACCEPTED;
+    return true;
 }
 
 #endif  // AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED

libraries/AP_Compass/AP_Compass_config.h

@@ -19,7 +19,7 @@
 #endif
 
 #ifndef AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
-#define AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED AP_GPS_ENABLED
+#define AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED AP_COMPASS_ENABLED && AP_GPS_ENABLED
 #endif
 
 #define COMPASS_MAX_SCALE_FACTOR 1.5

libraries/AP_Compass/Compass_learn.cpp

@@ -60,8 +60,8 @@ void CompassLearn::update(void)
         return;
     }
 
-    const auto result = compass.mag_cal_fixed_yaw(degrees(yaw_rad), (1U<<HAL_COMPASS_MAX_SENSORS)-1, 0, 0, true);
-    if (result == MAV_RESULT_ACCEPTED) {
+    const bool result = compass.mag_cal_fixed_yaw(degrees(yaw_rad), (1U<<HAL_COMPASS_MAX_SENSORS)-1, 0, 0, true);
+    if (result) {
         AP_Notify::flags.compass_cal_running = false;
         compass.set_learn_type(Compass::LEARN_NONE, true);
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CompassLearn: Finished");

libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h

@@ -130,6 +130,9 @@
 #ifndef COMPASS_CAL_ENABLED
 #define COMPASS_CAL_ENABLED 0
 #endif
+#ifndef AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
+#define AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED 0
+#endif
 #ifndef COMPASS_MOT_ENABLED
 #define COMPASS_MOT_ENABLED 0
 #endif

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -3832,19 +3832,24 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg)
 #endif
 
 
-#if COMPASS_CAL_ENABLED
+#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
 /*
   handle MAV_CMD_FIXED_MAG_CAL_YAW
  */
 MAV_RESULT GCS_MAVLINK::handle_command_fixed_mag_cal_yaw(const mavlink_command_int_t &packet)
 {
     Compass &compass = AP::compass();
-    return compass.mag_cal_fixed_yaw(packet.param1,
-                                     uint8_t(packet.param2),
-                                     packet.param3,
-                                     packet.param4);
+    if (!compass.mag_cal_fixed_yaw(packet.param1,
+                                   uint8_t(packet.param2),
+                                   packet.param3,
+                                   packet.param4)) {
+        return MAV_RESULT_FAILED;
+    }
+    return MAV_RESULT_ACCEPTED;
 }
+#endif  // AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
 
+#if COMPASS_CAL_ENABLED
 MAV_RESULT GCS_MAVLINK::handle_command_mag_cal(const mavlink_command_int_t &packet)
 {
     return AP::compass().handle_mag_cal_command(packet);
@@ -5226,9 +5231,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
     case MAV_CMD_COMPONENT_ARM_DISARM:
         return handle_command_component_arm_disarm(packet);
 
-#if COMPASS_CAL_ENABLED
+#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
     case MAV_CMD_FIXED_MAG_CAL_YAW:
         return handle_command_fixed_mag_cal_yaw(packet);
+#endif
+#if COMPASS_CAL_ENABLED
     case MAV_CMD_DO_START_MAG_CAL:
     case MAV_CMD_DO_ACCEPT_MAG_CAL:
     case MAV_CMD_DO_CANCEL_MAG_CAL: