Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: make compass instance number part of AP_Compass_Backend

Browse Source 
rather than each backend storing this itself.

This allows many methods to assume that they are working on the current instance number rather than being passed the instance number

registering the compass via the compass backend method now infers setting the device ID
This commit is contained in:
Peter Barker
2025-10-14 14:48:58 +11:00
committed by Peter Barker
parent da676fd438
commit dabcf0e91c
41 changed files with 157 additions and 196 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_Compass/AP_Compass_AK09916.cpp
+5 -6
View File
@@ -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();
libraries/AP_Compass/AP_Compass_AK09916.h
-1
View File
@@ -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;
libraries/AP_Compass/AP_Compass_AK8963.cpp
+4 -5
View File
@@ -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()
libraries/AP_Compass/AP_Compass_AK8963.h
-1
View File
@@ -64,7 +64,6 @@ private:
float _magnetometer_ASA[3] {0, 0, 0};
uint8_t _compass_instance;
bool _initialized;
enum Rotation _rotation;
};
libraries/AP_Compass/AP_Compass_BMM150.cpp
+5 -6
View File
@@ -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();
}
libraries/AP_Compass/AP_Compass_BMM150.h
-2
View File
@@ -54,8 +54,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
uint8_t _compass_instance;
struct {
int8_t x1;
int8_t y1;
libraries/AP_Compass/AP_Compass_BMM350.cpp
+5 -6
View File
@@ -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
libraries/AP_Compass/AP_Compass_BMM350.h
-1
View File
@@ -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
libraries/AP_Compass/AP_Compass_Backend.cpp
+27 -25
View File
@@ -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;
}
libraries/AP_Compass/AP_Compass_Backend.h
+17 -14
View File
@@ -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;
libraries/AP_Compass/AP_Compass_DroneCAN.cpp
+3 -5
View File
@@ -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
libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp
+4 -5
View File
@@ -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
libraries/AP_Compass/AP_Compass_ExternalAHRS.h
-1
View File
@@ -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
libraries/AP_Compass/AP_Compass_HMC5843.cpp
+6 -7
View File
@@ -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()
libraries/AP_Compass/AP_Compass_HMC5843.h
-2
View File
@@ -63,8 +63,6 @@ private:
int16_t _mag_y;
int16_t _mag_z;
uint8_t _compass_instance;
enum Rotation _rotation;
bool _initialised:1;
libraries/AP_Compass/AP_Compass_IIS2MDC.cpp
+5 -7
View File
@@ -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
libraries/AP_Compass/AP_Compass_IIS2MDC.h
-1
View File
@@ -60,7 +60,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
enum Rotation _rotation;
uint8_t _instance;
bool _force_external;
};
libraries/AP_Compass/AP_Compass_IST8308.cpp
+5 -6
View File
@@ -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
libraries/AP_Compass/AP_Compass_IST8308.h
-1
View File
@@ -52,7 +52,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
enum Rotation _rotation;
uint8_t _instance;
bool _force_external;
};
libraries/AP_Compass/AP_Compass_IST8310.cpp
+5 -6
View File
@@ -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
libraries/AP_Compass/AP_Compass_IST8310.h
-1
View File
@@ -59,7 +59,6 @@ private:
AP_HAL::Device::PeriodicHandle _periodic_handle;
enum Rotation _rotation;
uint8_t _instance;
bool _ignore_next_sample;
bool _force_external;
};
libraries/AP_Compass/AP_Compass_LIS3MDL.cpp
+7 -8
View File
@@ -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
libraries/AP_Compass/AP_Compass_LIS3MDL.h
-1
View File
@@ -58,7 +58,6 @@ private:
bool init();
void timer();
uint8_t compass_instance;
bool force_external;
enum Rotation rotation;
};
libraries/AP_Compass/AP_Compass_LSM303D.cpp
+4 -5
View File
@@ -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()
libraries/AP_Compass/AP_Compass_LSM303D.h
-1
View File
@@ -50,7 +50,6 @@ private:
int16_t _mag_y;
int16_t _mag_z;
uint8_t _compass_instance;
bool _initialised;
uint8_t _mag_range_ga;
libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
+4 -5
View File
@@ -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)
libraries/AP_Compass/AP_Compass_LSM9DS1.h
-1
View File
@@ -39,7 +39,6 @@ private:
void _dump_registers();
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
uint8_t _compass_instance;
float _scaling;
enum Rotation _rotation;
libraries/AP_Compass/AP_Compass_MAG3110.cpp
+5 -6
View File
@@ -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
libraries/AP_Compass/AP_Compass_MAG3110.h
-1
View File
@@ -45,7 +45,6 @@ private:
int32_t _mag_y;
int32_t _mag_z;
uint8_t _compass_instance;
bool _initialised;
};
libraries/AP_Compass/AP_Compass_MMC3416.cpp
+8 -10
View File
@@ -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
libraries/AP_Compass/AP_Compass_MMC3416.h
-1
View File
@@ -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;
libraries/AP_Compass/AP_Compass_MMC5xx3.cpp
+7 -9
View File
@@ -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
libraries/AP_Compass/AP_Compass_MMC5xx3.h
-1
View File
@@ -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;
libraries/AP_Compass/AP_Compass_MSP.cpp
+3 -4
View File
@@ -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
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
+6 -7
View File
@@ -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()
libraries/AP_Compass/AP_Compass_QMC5883L.h
-1
View File
@@ -67,7 +67,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
enum Rotation _rotation;
uint8_t _instance;
bool _force_external;
};
libraries/AP_Compass/AP_Compass_QMC5883P.cpp
+5 -6
View File
@@ -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()
libraries/AP_Compass/AP_Compass_QMC5883P.h
-1
View File
@@ -66,7 +66,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
enum Rotation _rotation;
uint8_t _instance;
bool _force_external;
};
libraries/AP_Compass/AP_Compass_RM3100.cpp
+7 -8
View File
@@ -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
libraries/AP_Compass/AP_Compass_RM3100.h
-1
View File
@@ -57,7 +57,6 @@ private:
bool init();
void timer();
uint8_t compass_instance;
bool force_external;
enum Rotation rotation;
float _scaler = 1.0;
libraries/AP_Compass/AP_Compass_SITL.cpp
+10 -10
View File
@@ -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