diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index 871606d22d3..ec5d2ab037b 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -252,16 +252,15 @@ bool AP_Compass_AK09916::init() /* register the compass instance in the frontend */ _bus->set_device_type(_devtype); - if (!register_compass(_bus->get_bus_id(), _compass_instance)) { + if (!register_compass(_bus->get_bus_id())) { goto fail; } - set_dev_id(_compass_instance, _bus->get_bus_id()); if (_force_external) { - set_external(_compass_instance, true); + set_external(true); } - set_rotation(_compass_instance, _rotation); + set_rotation(_rotation); bus_sem->give(); @@ -280,7 +279,7 @@ void AP_Compass_AK09916::read() return; } - drain_accumulated_samples(_compass_instance); + drain_accumulated_samples(); } void AP_Compass_AK09916::_make_adc_sensitivity_adjustment(Vector3f& field) const @@ -324,7 +323,7 @@ void AP_Compass_AK09916::_update() _make_adc_sensitivity_adjustment(raw_field); raw_field *= AK09916_MILLIGAUSS_SCALE; - accumulate_sample(raw_field, _compass_instance, 10); + accumulate_sample(raw_field, 10); check_registers: _bus->check_next_register(); diff --git a/libraries/AP_Compass/AP_Compass_AK09916.h b/libraries/AP_Compass/AP_Compass_AK09916.h index 5c80cff9ef6..817c45297c5 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.h +++ b/libraries/AP_Compass/AP_Compass_AK09916.h @@ -101,7 +101,6 @@ private: AP_AK09916_BusDriver *_bus; bool _force_external; - uint8_t _compass_instance; bool _initialized; enum Rotation _rotation; enum AP_Compass_Backend::DevTypes _devtype; diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index c4dc7e66f88..a17b2fbdca8 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -162,12 +162,11 @@ bool AP_Compass_AK8963::init() /* register the compass instance in the frontend */ _bus->set_device_type(DEVTYPE_AK8963); - if (!register_compass(_bus->get_bus_id(), _compass_instance)) { + if (!register_compass(_bus->get_bus_id())) { goto fail; } - set_dev_id(_compass_instance, _bus->get_bus_id()); - set_rotation(_compass_instance, _rotation); + set_rotation(_rotation); bus_sem->give(); _bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void)); @@ -185,7 +184,7 @@ void AP_Compass_AK8963::read() return; } - drain_accumulated_samples(_compass_instance); + drain_accumulated_samples(); } void AP_Compass_AK8963::_make_adc_sensitivity_adjustment(Vector3f& field) const @@ -227,7 +226,7 @@ void AP_Compass_AK8963::_update() _make_adc_sensitivity_adjustment(raw_field); raw_field *= AK8963_MILLIGAUSS_SCALE; - accumulate_sample(raw_field, _compass_instance, 10); + accumulate_sample(raw_field, 10); } bool AP_Compass_AK8963::_check_id() diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index c0f02075657..b5d092d53b2 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -64,7 +64,6 @@ private: float _magnetometer_ASA[3] {0, 0, 0}; - uint8_t _compass_instance; bool _initialized; enum Rotation _rotation; }; diff --git a/libraries/AP_Compass/AP_Compass_BMM150.cpp b/libraries/AP_Compass/AP_Compass_BMM150.cpp index eb4dcd0b36d..11ae92b70a3 100644 --- a/libraries/AP_Compass/AP_Compass_BMM150.cpp +++ b/libraries/AP_Compass/AP_Compass_BMM150.cpp @@ -214,15 +214,14 @@ bool AP_Compass_BMM150::init() /* register the compass instance in the frontend */ _dev->set_device_type(DEVTYPE_BMM150); - if (!register_compass(_dev->get_bus_id(), _compass_instance)) { + if (!register_compass(_dev->get_bus_id())) { return false; } - set_dev_id(_compass_instance, _dev->get_bus_id()); - set_rotation(_compass_instance, _rotation); + set_rotation(_rotation); if (_force_external) { - set_external(_compass_instance, true); + set_external(true); } // 2 retries for run @@ -318,13 +317,13 @@ void AP_Compass_BMM150::_update() _last_read_ms = AP_HAL::millis(); - accumulate_sample(raw_field, _compass_instance); + accumulate_sample(raw_field); _dev->check_next_register(); } void AP_Compass_BMM150::read() { - drain_accumulated_samples(_compass_instance); + drain_accumulated_samples(); } diff --git a/libraries/AP_Compass/AP_Compass_BMM150.h b/libraries/AP_Compass/AP_Compass_BMM150.h index 2043f1b8902..56eb25827d6 100644 --- a/libraries/AP_Compass/AP_Compass_BMM150.h +++ b/libraries/AP_Compass/AP_Compass_BMM150.h @@ -54,8 +54,6 @@ private: AP_HAL::OwnPtr _dev; - uint8_t _compass_instance; - struct { int8_t x1; int8_t y1; diff --git a/libraries/AP_Compass/AP_Compass_BMM350.cpp b/libraries/AP_Compass/AP_Compass_BMM350.cpp index d5f83419b8a..d09a1babac7 100644 --- a/libraries/AP_Compass/AP_Compass_BMM350.cpp +++ b/libraries/AP_Compass/AP_Compass_BMM350.cpp @@ -406,17 +406,16 @@ bool AP_Compass_BMM350::init() /* Register the compass instance in the frontend */ _dev->set_device_type(DEVTYPE_BMM350); - if (!register_compass(_dev->get_bus_id(), _compass_instance)) { + if (!register_compass(_dev->get_bus_id())) { return false; } - set_dev_id(_compass_instance, _dev->get_bus_id()); // printf("BMM350: Found at address 0x%x as compass %u\n", _dev->get_bus_address(), _compass_instance); - set_rotation(_compass_instance, _rotation); + set_rotation(_rotation); if (_force_external) { - set_external(_compass_instance, true); + set_external(true); } // Call timer() at 100Hz @@ -481,12 +480,12 @@ void AP_Compass_BMM350::timer() // Store in field vector and convert uT to milligauss Vector3f field { cr_ax_comp_x * 10.0f, cr_ax_comp_y * 10.0f, cr_ax_comp_z * 10.0f }; - accumulate_sample(field, _compass_instance); + accumulate_sample(field); } void AP_Compass_BMM350::read() { - drain_accumulated_samples(_compass_instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_BMM350_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_BMM350.h b/libraries/AP_Compass/AP_Compass_BMM350.h index 4cf5fdf5550..fd6878f8de0 100644 --- a/libraries/AP_Compass/AP_Compass_BMM350.h +++ b/libraries/AP_Compass/AP_Compass_BMM350.h @@ -103,7 +103,6 @@ private: bool set_power_mode(const enum power_mode mode); bool read_bytes(const uint8_t reg, uint8_t *out, const uint16_t read_len); - uint8_t _compass_instance; bool _force_external; enum Rotation _rotation; struct mag_compensate _mag_comp; // Structure for mag compensate diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 5325e71a2a9..0bdea722f02 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -16,7 +16,7 @@ AP_Compass_Backend::AP_Compass_Backend() { } -void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) +void AP_Compass_Backend::rotate_field(Vector3f &mag) { Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; if (MAG_BOARD_ORIENTATION != ROTATION_NONE) { @@ -31,7 +31,7 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) before the board orientation so it is independent of AHRS_ORIENTATION */ - if (!is_external(instance)) { + if (!is_external()) { const uint32_t dev_id = uint32_t(_compass._state[Compass::StateIndex(instance)].dev_id); static const struct offset { uint32_t dev_id; @@ -56,7 +56,7 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) } } -void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance) +void AP_Compass_Backend::publish_raw_field(const Vector3f &mag) { // note that we do not set last_update_usec here as otherwise the // EKF and DCM would end up consuming compass data at the full @@ -71,9 +71,9 @@ void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance #endif } -void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i) +void AP_Compass_Backend::correct_field(Vector3f &mag) { - Compass::mag_state &state = _compass._state[Compass::StateIndex(i)]; + Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; const Vector3f &offsets = state.offset.get(); #if AP_COMPASS_DIAGONALS_ENABLED @@ -86,7 +86,7 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i) // add in scale factor, use a wide sanity check. The calibrator // uses a narrower check. - if (_compass.have_scale_factor(i)) { + if (_compass.have_scale_factor(instance)) { mag *= state.scale_factor; } @@ -111,7 +111,7 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i) being applied so it can be logged correctly */ state.motor_offset.zero(); - if (_compass._per_motor.enabled() && i == 0) { + if (_compass._per_motor.enabled() && instance == 0) { // per-motor correction is only valid for first compass _compass._per_motor.compensate(state.motor_offset); } else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { @@ -134,17 +134,17 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i) #endif // COMPASS_MOT_ENABLED } -void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance, +void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint32_t max_samples) { /* rotate raw_field from sensor frame to body frame */ - rotate_field(field, instance); + rotate_field(field); /* publish raw_field (uncorrected point sample) for calibration use */ - publish_raw_field(field, instance); + publish_raw_field(field); /* correct raw_field for known errors */ - correct_field(field, instance); + correct_field(field); if (!field_ok(field)) { return; @@ -161,8 +161,7 @@ void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance, } } -void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance, - const Vector3f *scaling) +void AP_Compass_Backend::drain_accumulated_samples(const Vector3f *scaling) { WITH_SEMAPHORE(_sem); @@ -177,7 +176,7 @@ void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance, } state.accum /= state.accum_count; - publish_filtered_field(state.accum, instance); + publish_filtered_field(state.accum); state.accum.zero(); state.accum_count = 0; @@ -186,7 +185,7 @@ void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance, /* copy latest data to the frontend from a backend */ -void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t instance) +void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag) { Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; @@ -196,7 +195,7 @@ void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t ins state.last_update_usec = AP_HAL::micros(); } -void AP_Compass_Backend::set_last_update_usec(uint32_t last_update, uint8_t instance) +void AP_Compass_Backend::set_last_update_usec(uint32_t last_update) { Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; state.last_update_usec = last_update; @@ -206,16 +205,19 @@ void AP_Compass_Backend::set_last_update_usec(uint32_t last_update, uint8_t inst register a new backend with frontend, returning instance which should be used in publish_field() */ -bool AP_Compass_Backend::register_compass(int32_t dev_id, uint8_t& instance) const -{ - return _compass.register_compass(dev_id, instance); +bool AP_Compass_Backend::register_compass(int32_t dev_id) +{ + if (!_compass.register_compass(dev_id, instance)) { + return false; + } + set_dev_id(dev_id); + return true; } - /* set dev_id for an instance */ -void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id) +void AP_Compass_Backend::set_dev_id(uint32_t dev_id) { _compass._state[Compass::StateIndex(instance)].dev_id.set_and_notify(dev_id); _compass._state[Compass::StateIndex(instance)].detected_dev_id = dev_id; @@ -224,7 +226,7 @@ void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id) /* save dev_id, used by SITL */ -void AP_Compass_Backend::save_dev_id(uint8_t instance) +void AP_Compass_Backend::save_dev_id() { _compass._state[Compass::StateIndex(instance)].dev_id.save(); } @@ -232,20 +234,20 @@ void AP_Compass_Backend::save_dev_id(uint8_t instance) /* set external for an instance */ -void AP_Compass_Backend::set_external(uint8_t instance, bool external) +void AP_Compass_Backend::set_external(bool external) { if (_compass._state[Compass::StateIndex(instance)].external != 2) { _compass._state[Compass::StateIndex(instance)].external.set_and_notify(external); } } -bool AP_Compass_Backend::is_external(uint8_t instance) +bool AP_Compass_Backend::is_external() { return _compass._state[Compass::StateIndex(instance)].external; } // set rotation of an instance -void AP_Compass_Backend::set_rotation(uint8_t instance, enum Rotation rotation) +void AP_Compass_Backend::set_rotation(enum Rotation rotation) { _compass._state[Compass::StateIndex(instance)].rotation = rotation; } diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index b6067359fd1..e2806997cbd 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -88,6 +88,9 @@ public: protected: + // this backend's instance number + uint8_t instance; + /* * A compass measurement is expected to pass through the following functions: * 1. rotate_field - this rotates the measurement in-place from sensor frame @@ -101,33 +104,33 @@ protected: * All those functions expect the mag field to be in milligauss. */ - void rotate_field(Vector3f &mag, uint8_t instance); - void publish_raw_field(const Vector3f &mag, uint8_t instance); - void correct_field(Vector3f &mag, uint8_t i); - void publish_filtered_field(const Vector3f &mag, uint8_t instance); - void set_last_update_usec(uint32_t last_update, uint8_t instance); + void rotate_field(Vector3f &mag); + void publish_raw_field(const Vector3f &mag); + void correct_field(Vector3f &mag); + void publish_filtered_field(const Vector3f &mag); + void set_last_update_usec(uint32_t last_update); - void accumulate_sample(Vector3f &field, uint8_t instance, + void accumulate_sample(Vector3f &field, uint32_t max_samples = 10); - void drain_accumulated_samples(uint8_t instance, const Vector3f *scale = NULL); + void drain_accumulated_samples(const Vector3f *scale = NULL); - // register a new compass instance with the frontend - bool register_compass(int32_t dev_id, uint8_t& instance) const WARN_IF_UNUSED; + // register compass instance with the frontend + bool register_compass(int32_t dev_id) WARN_IF_UNUSED; // set dev_id for an instance - void set_dev_id(uint8_t instance, uint32_t dev_id); + void set_dev_id(uint32_t dev_id); // save dev_id, used by SITL - void save_dev_id(uint8_t instance); + void save_dev_id(); // set external state for an instance - void set_external(uint8_t instance, bool external); + void set_external(bool external); // tell if instance is an external compass - bool is_external(uint8_t instance); + bool is_external(); // set rotation of an instance - void set_rotation(uint8_t instance, enum Rotation rotation); + void set_rotation(enum Rotation rotation); // get board orientation (for SITL) enum Rotation get_board_orientation(void) const; diff --git a/libraries/AP_Compass/AP_Compass_DroneCAN.cpp b/libraries/AP_Compass/AP_Compass_DroneCAN.cpp index c155ee8ab30..fa078e230af 100644 --- a/libraries/AP_Compass/AP_Compass_DroneCAN.cpp +++ b/libraries/AP_Compass/AP_Compass_DroneCAN.cpp @@ -91,12 +91,10 @@ AP_Compass_Backend* AP_Compass_DroneCAN::probe(uint8_t index) bool AP_Compass_DroneCAN::init() { // Adding 1 is necessary to allow backward compatibility, where this field was set as 1 by default - if (!register_compass(_devid, _instance)) { + if (!register_compass(_devid)) { return false; } - - set_dev_id(_instance, _devid); - set_external(_instance, true); + set_external(true); AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "AP_Compass_DroneCAN loaded\n\r"); return true; @@ -228,6 +226,6 @@ void AP_Compass_DroneCAN::handle_magnetic_field_hires(AP_DroneCAN *ap_dronecan, void AP_Compass_DroneCAN::read(void) { - drain_accumulated_samples(_instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_DRONECAN_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp b/libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp index af305afeb85..f630be205e8 100644 --- a/libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp +++ b/libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp @@ -25,24 +25,23 @@ AP_Compass_Backend *AP_Compass_ExternalAHRS::probe(uint8_t port) if (ret == nullptr) { return nullptr; } - if (!ret->register_compass(devid, ret->instance)) { + if (!ret->register_compass(devid)) { delete ret; return nullptr; } - ret->set_dev_id(ret->instance, devid); - ret->set_external(ret->instance, true); + ret->set_external(true); return ret; } void AP_Compass_ExternalAHRS::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt) { Vector3f field = pkt.field; - accumulate_sample(field, instance); + accumulate_sample(field); } void AP_Compass_ExternalAHRS::read(void) { - drain_accumulated_samples(instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_EXTERNALAHRS_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_ExternalAHRS.h b/libraries/AP_Compass/AP_Compass_ExternalAHRS.h index 29e3367b366..ec79d3cd188 100644 --- a/libraries/AP_Compass/AP_Compass_ExternalAHRS.h +++ b/libraries/AP_Compass/AP_Compass_ExternalAHRS.h @@ -19,7 +19,6 @@ public: private: void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt) override; - uint8_t instance; }; #endif // AP_COMPASS_EXTERNALAHRS_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 1c2d969f34b..a1957367d82 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -196,15 +196,14 @@ bool AP_Compass_HMC5843::init() //register compass instance _bus->set_device_type(DEVTYPE_HMC5883); - if (!register_compass(_bus->get_bus_id(), _compass_instance)) { + if (!register_compass(_bus->get_bus_id())) { return false; } - set_dev_id(_compass_instance, _bus->get_bus_id()); - set_rotation(_compass_instance, _rotation); + set_rotation(_rotation); if (_force_external) { - set_external(_compass_instance, true); + set_external(true); } // read from sensor at 75Hz @@ -241,14 +240,14 @@ void AP_Compass_HMC5843::_timer() raw_field *= _gain_scale; // rotate to the desired orientation - if (is_external(_compass_instance)) { + if (is_external()) { raw_field.rotate(ROTATION_YAW_90); } // We expect to do reads at 10Hz, and we get new data at most 75Hz, so we // don't expect to accumulate more than 8 before a read; let's make it // 14 to give more room for the initialization phase - accumulate_sample(raw_field, _compass_instance, 14); + accumulate_sample(raw_field, 14); } /* @@ -266,7 +265,7 @@ void AP_Compass_HMC5843::read() return; } - drain_accumulated_samples(_compass_instance, &_scaling); + drain_accumulated_samples(&_scaling); } bool AP_Compass_HMC5843::_setup_sampling_mode() diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index a2750c6ceff..4e5b9b6f0f3 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -63,8 +63,6 @@ private: int16_t _mag_y; int16_t _mag_z; - uint8_t _compass_instance; - enum Rotation _rotation; bool _initialised:1; diff --git a/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp b/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp index 0476c5d419e..8252c819cb2 100644 --- a/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp +++ b/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp @@ -98,16 +98,14 @@ bool AP_Compass_IIS2MDC::init() // register compass instance _dev->set_device_type(DEVTYPE_IIS2MDC); - if (!register_compass(_dev->get_bus_id(), _instance)) { + if (!register_compass(_dev->get_bus_id())) { return false; } - set_dev_id(_instance, _dev->get_bus_id()); - - set_rotation(_instance, _rotation); + set_rotation(_rotation); if (_force_external) { - set_external(_instance, true); + set_external(true); } // Enable 100HZ @@ -160,12 +158,12 @@ void AP_Compass_IIS2MDC::timer() Vector3f field{ x * range_scale, y * range_scale, z * range_scale }; - accumulate_sample(field, _instance); + accumulate_sample(field); } void AP_Compass_IIS2MDC::read() { - drain_accumulated_samples(_instance); + drain_accumulated_samples(); } #endif //AP_COMPASS_IIS2MDC_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_IIS2MDC.h b/libraries/AP_Compass/AP_Compass_IIS2MDC.h index cd4989eced0..27db5acf627 100644 --- a/libraries/AP_Compass/AP_Compass_IIS2MDC.h +++ b/libraries/AP_Compass/AP_Compass_IIS2MDC.h @@ -60,7 +60,6 @@ private: AP_HAL::OwnPtr _dev; enum Rotation _rotation; - uint8_t _instance; bool _force_external; }; diff --git a/libraries/AP_Compass/AP_Compass_IST8308.cpp b/libraries/AP_Compass/AP_Compass_IST8308.cpp index 722457dda39..57b41d0615f 100644 --- a/libraries/AP_Compass/AP_Compass_IST8308.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8308.cpp @@ -162,18 +162,17 @@ bool AP_Compass_IST8308::init() //register compass instance _dev->set_device_type(DEVTYPE_IST8308); - if (!register_compass(_dev->get_bus_id(), _instance)) { + if (!register_compass(_dev->get_bus_id())) { return false; } - set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); - set_rotation(_instance, _rotation); + set_rotation(_rotation); if (_force_external) { - set_external(_instance, true); + set_external(true); } _dev->register_periodic_callback(SAMPLING_PERIOD_USEC, @@ -215,12 +214,12 @@ void AP_Compass_IST8308::timer() /* Resolution: 0.1515 µT/LSB - already convert to milligauss */ Vector3f field = Vector3f{x * 1.515f, y * 1.515f, z * 1.515f}; - accumulate_sample(field, _instance); + accumulate_sample(field); } void AP_Compass_IST8308::read() { - drain_accumulated_samples(_instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_IST8308_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_IST8308.h b/libraries/AP_Compass/AP_Compass_IST8308.h index 94d4899dd9a..a55295295d4 100644 --- a/libraries/AP_Compass/AP_Compass_IST8308.h +++ b/libraries/AP_Compass/AP_Compass_IST8308.h @@ -52,7 +52,6 @@ private: AP_HAL::OwnPtr _dev; enum Rotation _rotation; - uint8_t _instance; bool _force_external; }; diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp index 75c0ab8e17d..27a411f0fd1 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp @@ -172,18 +172,17 @@ bool AP_Compass_IST8310::init() // register compass instance _dev->set_device_type(DEVTYPE_IST8310); - if (!register_compass(_dev->get_bus_id(), _instance)) { + if (!register_compass(_dev->get_bus_id())) { return false; } - set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); - set_rotation(_instance, _rotation); + set_rotation(_rotation); if (_force_external) { - set_external(_instance, true); + set_external(true); } _periodic_handle = _dev->register_periodic_callback(SAMPLING_PERIOD_USEC, @@ -247,12 +246,12 @@ void AP_Compass_IST8310::timer() /* Resolution: 0.3 µT/LSB - already convert to milligauss */ Vector3f field = Vector3f{x * 3.0f, y * 3.0f, z * 3.0f}; - accumulate_sample(field, _instance); + accumulate_sample(field); } void AP_Compass_IST8310::read() { - drain_accumulated_samples(_instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_IST8310_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_IST8310.h b/libraries/AP_Compass/AP_Compass_IST8310.h index 96d061a9c2d..2ef19de48e5 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.h +++ b/libraries/AP_Compass/AP_Compass_IST8310.h @@ -59,7 +59,6 @@ private: AP_HAL::Device::PeriodicHandle _periodic_handle; enum Rotation _rotation; - uint8_t _instance; bool _ignore_next_sample; bool _force_external; }; diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp index 392aa62f453..57b158b653d 100644 --- a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp +++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp @@ -106,17 +106,16 @@ bool AP_Compass_LIS3MDL::init() /* register the compass instance in the frontend */ dev->set_device_type(DEVTYPE_LIS3MDL); - if (!register_compass(dev->get_bus_id(), compass_instance)) { + if (!register_compass(dev->get_bus_id())) { return false; } - set_dev_id(compass_instance, dev->get_bus_id()); - printf("Found a LIS3MDL on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), compass_instance); - - set_rotation(compass_instance, rotation); + printf("Found a LIS3MDL on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), instance); + + set_rotation(rotation); if (force_external) { - set_external(compass_instance, true); + set_external(true); } // call timer() at 80Hz @@ -160,7 +159,7 @@ void AP_Compass_LIS3MDL::timer() data.magz * range_scale, }; - accumulate_sample(field, compass_instance); + accumulate_sample(field); } check_registers: @@ -169,7 +168,7 @@ check_registers: void AP_Compass_LIS3MDL::read() { - drain_accumulated_samples(compass_instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_LIS3MDL_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.h b/libraries/AP_Compass/AP_Compass_LIS3MDL.h index 71c5d4a32c7..39e8cd6c1a9 100644 --- a/libraries/AP_Compass/AP_Compass_LIS3MDL.h +++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.h @@ -58,7 +58,6 @@ private: bool init(); void timer(); - uint8_t compass_instance; bool force_external; enum Rotation rotation; }; diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.cpp b/libraries/AP_Compass/AP_Compass_LSM303D.cpp index 7d0437d169d..fdd3107ed21 100644 --- a/libraries/AP_Compass/AP_Compass_LSM303D.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM303D.cpp @@ -270,12 +270,11 @@ bool AP_Compass_LSM303D::init(enum Rotation rotation) /* register the compass instance in the frontend */ _dev->set_device_type(DEVTYPE_LSM303D); - if (!register_compass(_dev->get_bus_id(), _compass_instance)) { + if (!register_compass(_dev->get_bus_id())) { return false; } - set_dev_id(_compass_instance, _dev->get_bus_id()); - set_rotation(_compass_instance, rotation); + set_rotation(rotation); // read at 91Hz. We don't run at 100Hz as fetching data too fast can cause some very // odd periodic changes in the output data @@ -343,7 +342,7 @@ void AP_Compass_LSM303D::_update() Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale; - accumulate_sample(raw_field, _compass_instance, 10); + accumulate_sample(raw_field, 10); } // Read Sensor data @@ -353,7 +352,7 @@ void AP_Compass_LSM303D::read() return; } - drain_accumulated_samples(_compass_instance); + drain_accumulated_samples(); } void AP_Compass_LSM303D::_disable_i2c() diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.h b/libraries/AP_Compass/AP_Compass_LSM303D.h index 328e0477e05..503d9255f40 100644 --- a/libraries/AP_Compass/AP_Compass_LSM303D.h +++ b/libraries/AP_Compass/AP_Compass_LSM303D.h @@ -50,7 +50,6 @@ private: int16_t _mag_y; int16_t _mag_z; - uint8_t _compass_instance; bool _initialised; uint8_t _mag_range_ga; diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp index d423254347e..09dc4c3a770 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp @@ -98,12 +98,11 @@ bool AP_Compass_LSM9DS1::init() //register compass instance _dev->set_device_type(DEVTYPE_LSM9DS1); - if (!register_compass(_dev->get_bus_id(), _compass_instance)) { + if (!register_compass(_dev->get_bus_id())) { goto errout; } - set_dev_id(_compass_instance, _dev->get_bus_id()); - set_rotation(_compass_instance, _rotation); + set_rotation(_rotation); _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, void)); @@ -150,12 +149,12 @@ void AP_Compass_LSM9DS1::_update(void) raw_field *= _scaling; - accumulate_sample(raw_field, _compass_instance); + accumulate_sample(raw_field); } void AP_Compass_LSM9DS1::read() { - drain_accumulated_samples(_compass_instance); + drain_accumulated_samples(); } bool AP_Compass_LSM9DS1::_check_id(void) diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.h b/libraries/AP_Compass/AP_Compass_LSM9DS1.h index 7017883bd62..5f3a0b4c2e9 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.h +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.h @@ -39,7 +39,6 @@ private: void _dump_registers(); AP_HAL::OwnPtr _dev; - uint8_t _compass_instance; float _scaling; enum Rotation _rotation; diff --git a/libraries/AP_Compass/AP_Compass_MAG3110.cpp b/libraries/AP_Compass/AP_Compass_MAG3110.cpp index c486a0fac10..080b196a7d4 100644 --- a/libraries/AP_Compass/AP_Compass_MAG3110.cpp +++ b/libraries/AP_Compass/AP_Compass_MAG3110.cpp @@ -119,14 +119,13 @@ bool AP_Compass_MAG3110::init(enum Rotation rotation) /* register the compass instance in the frontend */ _dev->set_device_type(DEVTYPE_MAG3110); - if (!register_compass(_dev->get_bus_id(), _compass_instance)) { + if (!register_compass(_dev->get_bus_id())) { return false; } - set_dev_id(_compass_instance, _dev->get_bus_id()); - set_rotation(_compass_instance, rotation); + set_rotation(rotation); - set_external(_compass_instance, true); + set_external(true); // read at 75Hz _dev->register_periodic_callback(13333, FUNCTOR_BIND_MEMBER(&AP_Compass_MAG3110::_update, void)); @@ -208,7 +207,7 @@ void AP_Compass_MAG3110::_update() Vector3f raw_field = Vector3f((float)_mag_x, (float)_mag_y, (float)_mag_z) * MAG_SCALE; - accumulate_sample(raw_field, _compass_instance); + accumulate_sample(raw_field); } @@ -219,7 +218,7 @@ void AP_Compass_MAG3110::read() return; } - drain_accumulated_samples(_compass_instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_MAG3110_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_MAG3110.h b/libraries/AP_Compass/AP_Compass_MAG3110.h index 80167ba38d6..1c504c89a59 100644 --- a/libraries/AP_Compass/AP_Compass_MAG3110.h +++ b/libraries/AP_Compass/AP_Compass_MAG3110.h @@ -45,7 +45,6 @@ private: int32_t _mag_y; int32_t _mag_z; - uint8_t _compass_instance; bool _initialised; }; diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.cpp b/libraries/AP_Compass/AP_Compass_MMC3416.cpp index f58b779e31e..f3aef33b01b 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC3416.cpp @@ -93,18 +93,16 @@ bool AP_Compass_MMC3416::init() /* register the compass instance in the frontend */ dev->set_device_type(DEVTYPE_MMC3416); - if (!register_compass(dev->get_bus_id(), compass_instance)) { + if (!register_compass(dev->get_bus_id())) { return false; } - - set_dev_id(compass_instance, dev->get_bus_id()); - printf("Found a MMC3416 on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), compass_instance); - - set_rotation(compass_instance, rotation); + printf("Found a MMC3416 on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), instance); + + set_rotation(rotation); if (force_external) { - set_external(compass_instance, true); + set_external(true); } dev->set_retries(1); @@ -253,7 +251,7 @@ void AP_Compass_MMC3416::timer() // sensor is not FRD field.y = -field.y; - accumulate_sample(field, compass_instance); + accumulate_sample(field); if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { state = STATE_REFILL1; @@ -283,7 +281,7 @@ void AP_Compass_MMC3416::timer() field.y = -field.y; last_sample_ms = AP_HAL::millis(); - accumulate_sample(field, compass_instance); + accumulate_sample(field); // we stay in STATE_MEASURE_WAIT3 for measure_count_limit cycles if (measure_count++ >= measure_count_limit) { @@ -301,7 +299,7 @@ void AP_Compass_MMC3416::timer() void AP_Compass_MMC3416::read() { - drain_accumulated_samples(compass_instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_MMC3416_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.h b/libraries/AP_Compass/AP_Compass_MMC3416.h index 005a8840d1c..c858a99116c 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.h +++ b/libraries/AP_Compass/AP_Compass_MMC3416.h @@ -64,7 +64,6 @@ private: void timer(); void accumulate_field(Vector3f &field); - uint8_t compass_instance; bool force_external; Vector3f offset; uint16_t measure_count; diff --git a/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp b/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp index f932865ab32..43d51495399 100644 --- a/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp @@ -109,18 +109,16 @@ bool AP_Compass_MMC5XX3::init() /* register the compass instance in the frontend */ dev->set_device_type(DEVTYPE_MMC5983); - if (!register_compass(dev->get_bus_id(), compass_instance)) { + if (!register_compass(dev->get_bus_id())) { return false; } - set_dev_id(compass_instance, dev->get_bus_id()); + printf("Found a MMC5983 on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), instance); - printf("Found a MMC5983 on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), compass_instance); - - set_rotation(compass_instance, rotation); + set_rotation(rotation); if (force_external) { - set_external(compass_instance, true); + set_external(true); } dev->set_retries(1); @@ -250,7 +248,7 @@ void AP_Compass_MMC5XX3::timer() offset = offset * 0.5f + new_offset * 0.5f; } - accumulate_sample(field, compass_instance); + accumulate_sample(field); if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) { printf("failed to initiate measurement\n"); @@ -288,7 +286,7 @@ void AP_Compass_MMC5XX3::timer() float((data1[4] << 8) + data1[5]) - zero_offset}; field *= counts_to_milliGauss; field -= offset; - accumulate_sample(field, compass_instance); + accumulate_sample(field); // we stay in STATE_MEASURE for measure_count_limit cycles if (measure_count++ >= measure_count_limit) { @@ -306,7 +304,7 @@ void AP_Compass_MMC5XX3::timer() void AP_Compass_MMC5XX3::read() { - drain_accumulated_samples(compass_instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_MMC5XX3_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_MMC5xx3.h b/libraries/AP_Compass/AP_Compass_MMC5xx3.h index 8acd6e56336..7c3ba8228b9 100644 --- a/libraries/AP_Compass/AP_Compass_MMC5xx3.h +++ b/libraries/AP_Compass/AP_Compass_MMC5xx3.h @@ -64,7 +64,6 @@ private: void timer(); void accumulate_field(Vector3f &field); - uint8_t compass_instance; bool force_external; Vector3f offset; uint16_t measure_count; diff --git a/libraries/AP_Compass/AP_Compass_MSP.cpp b/libraries/AP_Compass/AP_Compass_MSP.cpp index c764c8ccfca..2dc06bba0a5 100644 --- a/libraries/AP_Compass/AP_Compass_MSP.cpp +++ b/libraries/AP_Compass/AP_Compass_MSP.cpp @@ -35,12 +35,11 @@ AP_Compass_Backend *AP_Compass_MSP::probe(uint8_t _msp_instance) bool AP_Compass_MSP::init() { auto devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_MSP, 0, msp_instance, 0); - if (!register_compass(devid, instance)) { + if (!register_compass(devid)) { return false; } - set_dev_id(instance, devid); - set_external(instance, true); + set_external(true); return true; } @@ -56,7 +55,7 @@ void AP_Compass_MSP::handle_msp(const MSP::msp_compass_data_message_t &pkt) void AP_Compass_MSP::read(void) { - drain_accumulated_samples(instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_MSP_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp index c0627764bde..804731d7613 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp +++ b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp @@ -115,18 +115,17 @@ bool AP_Compass_QMC5883L::init() //register compass instance _dev->set_device_type(DEVTYPE_QMC5883L); - if (!register_compass(_dev->get_bus_id(), _instance)) { + if (!register_compass(_dev->get_bus_id())) { return false; } - set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); - set_rotation(_instance, _rotation); + set_rotation(_rotation); if (_force_external) { - set_external(_instance, true); + set_external(true); } //Enable 100HZ @@ -191,16 +190,16 @@ void AP_Compass_QMC5883L::timer() Vector3f field = Vector3f{x * range_scale , y * range_scale, z * range_scale }; // rotate to the desired orientation - if (is_external(_instance)) { + if (is_external()) { field.rotate(ROTATION_YAW_90); } - accumulate_sample(field, _instance, 20); + accumulate_sample(field, 20); } void AP_Compass_QMC5883L::read() { - drain_accumulated_samples(_instance); + drain_accumulated_samples(); } void AP_Compass_QMC5883L::_dump_registers() diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.h b/libraries/AP_Compass/AP_Compass_QMC5883L.h index 7343dd45fda..1fc1d7b4255 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883L.h +++ b/libraries/AP_Compass/AP_Compass_QMC5883L.h @@ -67,7 +67,6 @@ private: AP_HAL::OwnPtr _dev; enum Rotation _rotation; - uint8_t _instance; bool _force_external; }; diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp index d4987f203e1..f30c9f7dfb5 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp +++ b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp @@ -126,18 +126,17 @@ bool AP_Compass_QMC5883P::init() //register compass instance _dev->set_device_type(DEVTYPE_QMC5883P); - if (!register_compass(_dev->get_bus_id(), _instance)) { + if (!register_compass(_dev->get_bus_id())) { return false; } - set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); - set_rotation(_instance, _rotation); + set_rotation(_rotation); if (_force_external) { - set_external(_instance, true); + set_external(true); } //Enable 100HZ @@ -195,12 +194,12 @@ void AP_Compass_QMC5883P::timer() Vector3f field = Vector3f{x * range_scale, y * range_scale, z * range_scale }; - accumulate_sample(field, _instance, 20); + accumulate_sample(field, 20); } void AP_Compass_QMC5883P::read() { - drain_accumulated_samples(_instance); + drain_accumulated_samples(); } void AP_Compass_QMC5883P::_dump_registers() diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.h b/libraries/AP_Compass/AP_Compass_QMC5883P.h index 2b30e2f3c46..9e68353080c 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883P.h +++ b/libraries/AP_Compass/AP_Compass_QMC5883P.h @@ -66,7 +66,6 @@ private: AP_HAL::OwnPtr _dev; enum Rotation _rotation; - uint8_t _instance; bool _force_external; }; diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp index 6a64bd0d0b1..aae03971848 100644 --- a/libraries/AP_Compass/AP_Compass_RM3100.cpp +++ b/libraries/AP_Compass/AP_Compass_RM3100.cpp @@ -145,17 +145,16 @@ bool AP_Compass_RM3100::init() /* register the compass instance in the frontend */ dev->set_device_type(DEVTYPE_RM3100); - if (!register_compass(dev->get_bus_id(), compass_instance)) { + if (!register_compass(dev->get_bus_id())) { return false; } - set_dev_id(compass_instance, dev->get_bus_id()); - DEV_PRINTF("RM3100: Found at address 0x%x as compass %u\n", dev->get_bus_address(), compass_instance); - - set_rotation(compass_instance, rotation); + DEV_PRINTF("RM3100: Found at address 0x%x as compass %u\n", dev->get_bus_address(), instance); + + set_rotation(rotation); if (force_external) { - set_external(compass_instance, true); + set_external(true); } // call timer() at 80Hz @@ -232,7 +231,7 @@ void AP_Compass_RM3100::timer() magz * _scaler }; - accumulate_sample(field, compass_instance); + accumulate_sample(field); } check_registers: @@ -241,7 +240,7 @@ check_registers: void AP_Compass_RM3100::read() { - drain_accumulated_samples(compass_instance); + drain_accumulated_samples(); } #endif // AP_COMPASS_RM3100_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_RM3100.h b/libraries/AP_Compass/AP_Compass_RM3100.h index 2b6419f03c9..25a0ee8cef8 100644 --- a/libraries/AP_Compass/AP_Compass_RM3100.h +++ b/libraries/AP_Compass/AP_Compass_RM3100.h @@ -57,7 +57,6 @@ private: bool init(); void timer(); - uint8_t compass_instance; bool force_external; enum Rotation rotation; float _scaler = 1.0; diff --git a/libraries/AP_Compass/AP_Compass_SITL.cpp b/libraries/AP_Compass/AP_Compass_SITL.cpp index e3171e8d76f..3d388a73b67 100644 --- a/libraries/AP_Compass/AP_Compass_SITL.cpp +++ b/libraries/AP_Compass/AP_Compass_SITL.cpp @@ -18,18 +18,18 @@ AP_Compass_SITL::AP_Compass_SITL(uint8_t _sitl_instance) : if (dev_id == 0) { return; } - if (!register_compass(dev_id, _compass_instance)) { + if (!register_compass(dev_id)) { return; } - set_dev_id(_compass_instance, dev_id); if (_sitl->mag_save_ids) { // save so the compass always comes up configured in SITL - save_dev_id(_compass_instance); + save_dev_id(); } - set_rotation(_compass_instance, ROTATION_NONE); + set_rotation(ROTATION_NONE); - if (_compass.get_offsets(_compass_instance).is_zero()) { - _compass.set_offsets(_compass_instance, _sitl->mag_ofs[sitl_instance]); + // Scroll through the registered compasses, and set the offsets + if (_compass.get_offsets(instance).is_zero()) { + _compass.set_offsets(instance, _sitl->mag_ofs[sitl_instance]); } // we want to simulate a calibrated compass by default, so set @@ -40,7 +40,7 @@ AP_Compass_SITL::AP_Compass_SITL(uint8_t _sitl_instance) : // make first compass external if (sitl_instance == 0) { - set_external(_compass_instance, true); + set_external(true); } hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void)); @@ -130,7 +130,7 @@ void AP_Compass_SITL::_timer() switch (_sitl->mag_fail[sitl_instance]) { case 0: - accumulate_sample(f, _compass_instance, 10); + accumulate_sample(f, 10); _last_data = f; break; case 1: @@ -138,13 +138,13 @@ void AP_Compass_SITL::_timer() break; case 2: // frozen compass - accumulate_sample(_last_data, _compass_instance, 10); + accumulate_sample(_last_data, 10); break; } } void AP_Compass_SITL::read() { - drain_accumulated_samples(_compass_instance, nullptr); + drain_accumulated_samples(nullptr); } #endif // AP_COMPASS_SITL_ENABLED