AP_Compass: reduce firmware size for AP_Periph

libraries/AP_Compass/AP_Compass.cpp

@@ -435,9 +435,11 @@ const AP_Param::GroupInfo Compass::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("OFFS_MAX", 31, Compass, _offset_max, AP_COMPASS_OFFSETS_MAX_DEFAULT),
 
+#if COMPASS_MOT_ENABLED
     // @Group: PMOT
     // @Path: Compass_PerMotor.cpp
     AP_SUBGROUPINFO(_per_motor, "PMOT", 32, Compass, Compass_PerMotor),
+#endif
 
     // @Param: TYPEMASK
     // @DisplayName: Compass disable driver type mask
@@ -533,13 +535,19 @@ void Compass::init()
     // check that we are actually working before passing the compass
     // through to ARHS to use.
     if (!read()) {
+#ifndef HAL_BUILD_AP_PERIPH
         _enabled = false;
         hal.console->printf("Compass initialisation failed\n");
+#endif
+#ifndef HAL_NO_LOGGING
         AP::logger().Write_Error(LogErrorSubsystem::COMPASS, LogErrorCode::FAILED_TO_INITIALISE);
+#endif
         return;
     }
 
+#ifndef HAL_BUILD_AP_PERIPH
     AP::ahrs().set_compass(this);
+#endif
 }
 
 //  Register a new compass instance
@@ -966,9 +974,11 @@ void Compass::_detect_backends(void)
 bool
 Compass::read(void)
 {
+#ifndef HAL_BUILD_AP_PERIPH
     if (!_initial_location_set) {
         try_set_initial_location();
     }
+#endif
     for (uint8_t i=0; i< _backend_count; i++) {
         // call read on each of the backend. This call updates field[i]
         _backends[i]->read();
@@ -977,6 +987,7 @@ Compass::read(void)
     for (uint8_t i=0; i < COMPASS_MAX_INSTANCES; i++) {
         _state[i].healthy = (time - _state[i].last_update_ms < 500);
     }
+#if COMPASS_LEARN_ENABLED
     if (_learn == LEARN_INFLIGHT && !learn_allocated) {
         learn_allocated = true;
         learn = new CompassLearn(*this);
@@ -989,6 +1000,9 @@ Compass::read(void)
         AP::logger().Write_Compass();
     }
     return ret;
+#else
+    return healthy();
+#endif
 }
 
 uint8_t

libraries/AP_Compass/AP_Compass.h

@@ -31,6 +31,9 @@
 #endif
 #endif
 
+#define COMPASS_CAL_ENABLED !defined(HAL_BUILD_AP_PERIPH)
+#define COMPASS_MOT_ENABLED !defined(HAL_BUILD_AP_PERIPH)
+#define COMPASS_LEARN_ENABLED !defined(HAL_BUILD_AP_PERIPH)
 
 // define default compass calibration fitness and consistency checks
 #define AP_COMPASS_CALIBRATION_FITNESS_DEFAULT 16.0f
@@ -124,6 +127,7 @@ public:
     // compass calibrator interface
     void cal_update();
 
+#if COMPASS_MOT_ENABLED
     // per-motor calibration access
     void per_motor_calibration_start(void) {
         _per_motor.calibration_start();
@@ -134,6 +138,7 @@ public:
     void per_motor_calibration_end(void) {
         _per_motor.calibration_end();
     }
+#endif
     
     void start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false);
 
@@ -242,10 +247,12 @@ public:
         }
     }
 
+#if COMPASS_MOT_ENABLED
     /// Set the battery voltage for per-motor compensation
     void set_voltage(float voltage) {
         _per_motor.set_voltage(voltage);
     }
+#endif
     
     /// Returns True if the compasses have been configured (i.e. offsets saved)
     ///
@@ -339,9 +346,11 @@ private:
     // 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 COMPASS_CAL_ENABLED
     //keep track of which calibrators have been saved
     bool _cal_saved[COMPASS_MAX_INSTANCES];
     bool _cal_autosave;
+#endif
 
     //autoreboot after compass calibration
     bool _compass_cal_autoreboot;
@@ -458,10 +467,14 @@ private:
 
     AP_Int16 _offset_max;
 
+#if COMPASS_CAL_ENABLED
     CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
+#endif
 
+#if COMPASS_MOT_ENABLED
     // per-motor compass compensation
     Compass_PerMotor _per_motor{*this};
+#endif
     
     // if we want HIL only
     bool _hil_mode:1;

libraries/AP_Compass/AP_Compass_AK09916.cpp

@@ -25,6 +25,12 @@
 #include <AP_InertialSensor/AP_InertialSensor_Invensensev2.h>
 #include <GCS_MAVLink/GCS.h>
 
+#ifdef HAL_NO_GCS
+#define GCS_SEND_TEXT(severity, format, args...)
+#else
+#define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args)
+#endif
+
 #define REG_COMPANY_ID      0x00
 #define REG_DEVICE_ID       0x01
 #define REG_ST1             0x10
@@ -152,7 +158,7 @@ AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtr<AP_HAL::I2
     if (dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2)) {
         // a device is replying on the AK09916 I2C address, don't
         // load the ICM20948
-        gcs().send_text(MAV_SEVERITY_INFO, "ICM20948: AK09916 bus conflict\n");
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ICM20948: AK09916 bus conflict\n");
         goto fail;
     }
 
@@ -191,32 +197,32 @@ bool AP_Compass_AK09916::init()
     AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
 
     if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Unable to get bus semaphore\n");
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"AK09916: Unable to get bus semaphore\n");
         return false;
     }
 
     if (!_bus->configure()) {
-        gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Could not configure the bus\n");
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"AK09916: Could not configure the bus\n");
         goto fail;
     }
 
     if (!_reset()) {
-        gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Reset Failed\n");
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"AK09916: Reset Failed\n");
         goto fail;
     }
 
     if (!_check_id()) {
-        gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Wrong id\n");
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"AK09916: Wrong id\n");
         goto fail;
     }
 
     if (!_setup_mode()) {
-        gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Could not setup mode\n");
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"AK09916: Could not setup mode\n");
         goto fail;
     }
 
     if (!_bus->start_measurements()) {
-        gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Could not start measurements\n");
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"AK09916: Could not start measurements\n");
         goto fail;
     }
 

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -39,8 +39,9 @@ void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance
     // EKF and DCM would end up consuming compass data at the full
     // sensor rate. We want them to consume only the filtered fields
     state.last_update_ms = AP_HAL::millis();
-
+#if COMPASS_CAL_ENABLED
     _compass._calibrator[instance].new_sample(mag);
+#endif
 }
 
 void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
@@ -54,7 +55,6 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
     const Vector3f &offsets = state.offset.get();
     const Vector3f &diagonals = state.diagonals.get();
     const Vector3f &offdiagonals = state.offdiagonals.get();
-    const Vector3f &mot = state.motor_compensation.get();
 
     // add in the basic offsets
     mag += offsets;
@@ -68,6 +68,8 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
 
     mag = mat * mag;
 
+#if COMPASS_MOT_ENABLED
+    const Vector3f &mot = state.motor_compensation.get();
     /*
       calculate motor-power based compensation
       note that _motor_offset[] is kept even if compensation is not
@@ -94,6 +96,7 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
       on final field outputs, not on the raw outputs
     */
     mag += state.motor_offset;
+#endif // COMPASS_MOT_ENABLED
 }
 
 void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance,

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -6,6 +6,8 @@
 
 extern AP_HAL::HAL& hal;
 
+#if COMPASS_CAL_ENABLED
+
 void
 Compass::cal_update()
 {
@@ -368,3 +370,5 @@ MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
     
     return result;
 }
+
+#endif // COMPASS_CAL_ENABLED

libraries/AP_Compass/Compass_learn.cpp

@@ -10,6 +10,8 @@
 
 #include <stdio.h>
 
+#if COMPASS_LEARN_ENABLED
+
 extern const AP_HAL::HAL &hal;
 
 // constructor
@@ -229,3 +231,6 @@ void CompassLearn::process_sample(const struct sample &s)
     worst_error = worstv;
     best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors));
 }
+
+#endif // COMPASS_LEARN_ENABLED
+