From f4576ec8f30297f556b20993de74e4167eba54ee Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 27 May 2019 11:49:12 +1000 Subject: [PATCH] AP_Compass: reduce firmware size for AP_Periph --- libraries/AP_Compass/AP_Compass.cpp | 14 +++++++++++++ libraries/AP_Compass/AP_Compass.h | 13 ++++++++++++ libraries/AP_Compass/AP_Compass_AK09916.cpp | 20 ++++++++++++------- libraries/AP_Compass/AP_Compass_Backend.cpp | 7 +++++-- .../AP_Compass/AP_Compass_Calibration.cpp | 4 ++++ libraries/AP_Compass/Compass_learn.cpp | 5 +++++ 6 files changed, 54 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 0a2dc539ed..c392dd02e2 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/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 diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index c6b502c24c..48ced14bfe 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/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; diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index 8457a00e16..92ac091401 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -25,6 +25,12 @@ #include #include +#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::OwnPtrread_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; } diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 7212efb88b..8fbcbf4318 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/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, diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index 6e51e93ede..75bf834375 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/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 diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp index dd576f28ba..d0b7122b7c 100644 --- a/libraries/AP_Compass/Compass_learn.cpp +++ b/libraries/AP_Compass/Compass_learn.cpp @@ -10,6 +10,8 @@ #include +#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 +