diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h index 6c2a6d2851ad7..5dc97edb2aac1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h @@ -66,8 +66,6 @@ class AP_InertialSensor_ADIS1647x : public AP_InertialSensor_Backend { Delta32 =3 } opmode; - uint8_t accel_instance; - uint8_t gyro_instance; enum Rotation rotation; uint8_t drdy_pin; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h index 5b5773a3272f0..6622906e2dd49 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h @@ -62,8 +62,6 @@ class AP_InertialSensor_BMI055 : public AP_InertialSensor_Backend { AP_HAL::OwnPtr dev_accel; AP_HAL::OwnPtr dev_gyro; - uint8_t accel_instance; - uint8_t gyro_instance; enum Rotation rotation; uint8_t temperature_counter; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h index d8820529aa88a..005ddc2615daf 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h @@ -79,8 +79,6 @@ class AP_InertialSensor_BMI088 : public AP_InertialSensor_Backend { AP_HAL::OwnPtr dev_gyro; AP_HAL::Device::PeriodicHandle gyro_periodic_handle; - uint8_t accel_instance; - uint8_t gyro_instance; enum Rotation rotation; uint8_t temperature_counter; enum DevTypes _accel_devtype; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp index 8659d053ffe1b..777ab3cf0e30d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp @@ -201,8 +201,8 @@ void AP_InertialSensor_BMI160::start() _dev->get_semaphore()->give(); - if (!_imu.register_accel(_accel_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)) || - !_imu.register_gyro(_gyro_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160))) { + if (!_imu.register_accel(accel_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)) || + !_imu.register_gyro(gyro_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160))) { return; } @@ -213,8 +213,8 @@ void AP_InertialSensor_BMI160::start() bool AP_InertialSensor_BMI160::update() { - update_accel(_accel_instance); - update_gyro(_gyro_instance); + update_accel(accel_instance); + update_gyro(gyro_instance); return true; } @@ -420,11 +420,11 @@ void AP_InertialSensor_BMI160::_read_fifo() accel *= _accel_scale; gyro *= _gyro_scale; - _rotate_and_correct_accel(_accel_instance, accel); - _rotate_and_correct_gyro(_gyro_instance, gyro); + _rotate_and_correct_accel(accel_instance, accel); + _rotate_and_correct_gyro(gyro_instance, gyro); - _notify_new_accel_raw_sample(_accel_instance, accel); - _notify_new_gyro_raw_sample(_gyro_instance, gyro); + _notify_new_accel_raw_sample(accel_instance, accel); + _notify_new_gyro_raw_sample(gyro_instance, gyro); } if (excess) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h index 77aac9eb08f3b..fe5940015ab3f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h @@ -120,10 +120,7 @@ class AP_InertialSensor_BMI160 : public AP_InertialSensor_Backend { AP_HAL::OwnPtr _dev; enum Rotation _rotation; - uint8_t _accel_instance; float _accel_scale; - - uint8_t _gyro_instance; float _gyro_scale; AP_HAL::DigitalSource *_int1_pin; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp index 0d3bb850718d5..41776d0143e9f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp @@ -212,14 +212,14 @@ void AP_InertialSensor_BMI270::start() _dev->get_semaphore()->give(); - if (!_imu.register_accel(_accel_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270)) || - !_imu.register_gyro(_gyro_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270))) { + if (!_imu.register_accel(accel_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270)) || + !_imu.register_gyro(gyro_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270))) { return; } // setup sensor rotations from probe() - set_gyro_orientation(_gyro_instance, _rotation); - set_accel_orientation(_accel_instance, _rotation); + set_gyro_orientation(gyro_instance, _rotation); + set_accel_orientation(accel_instance, _rotation); /* Call read_fifo() at 1600Hz */ periodic_handle = _dev->register_periodic_callback(BACKEND_PERIOD_US, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI270::read_fifo, void)); @@ -227,8 +227,8 @@ void AP_InertialSensor_BMI270::start() bool AP_InertialSensor_BMI270::update() { - update_accel(_accel_instance); - update_gyro(_gyro_instance); + update_accel(accel_instance); + update_gyro(gyro_instance); return true; } @@ -364,8 +364,8 @@ void AP_InertialSensor_BMI270::fifo_reset() // flush and reset FIFO write_register(BMI270_REG_CMD, BMI270_CMD_FIFOFLUSH); - notify_accel_fifo_reset(_accel_instance); - notify_gyro_fifo_reset(_gyro_instance); + notify_accel_fifo_reset(accel_instance); + notify_gyro_fifo_reset(gyro_instance); } /* @@ -383,8 +383,8 @@ void AP_InertialSensor_BMI270::read_fifo(void) uint8_t len[2]; if (!read_registers(BMI270_REG_FIFO_LENGTH_LSB, len, 2)) { - _inc_accel_error_count(_accel_instance); - _inc_gyro_error_count(_gyro_instance); + _inc_accel_error_count(accel_instance); + _inc_gyro_error_count(gyro_instance); return; } uint16_t fifo_length = len[0] + (len[1]<<8); @@ -403,8 +403,8 @@ void AP_InertialSensor_BMI270::read_fifo(void) uint8_t data[fifo_length]; if (!read_registers(BMI270_REG_FIFO_DATA, data, fifo_length)) { - _inc_accel_error_count(_accel_instance); - _inc_gyro_error_count(_gyro_instance); + _inc_accel_error_count(accel_instance); + _inc_gyro_error_count(gyro_instance); return; } @@ -462,14 +462,14 @@ void AP_InertialSensor_BMI270::read_fifo(void) temperature_counter = 0; uint8_t tbuf[2]; if (!read_registers(BMI270_REG_TEMPERATURE_LSB, tbuf, 2)) { - _inc_accel_error_count(_accel_instance); - _inc_gyro_error_count(_gyro_instance); + _inc_accel_error_count(accel_instance); + _inc_gyro_error_count(gyro_instance); } else { uint16_t tval = tbuf[0] | (tbuf[1] << 8); if (tval != 0x8000) { // 0x8000 is invalid int16_t klsb = static_cast(tval); float temp_degc = klsb * 0.002f + 23.0f; - _publish_temperature(_accel_instance, temp_degc); + _publish_temperature(accel_instance, temp_degc); } } } @@ -487,8 +487,8 @@ void AP_InertialSensor_BMI270::parse_accel_frame(const uint8_t* d) accel *= scale; - _rotate_and_correct_accel(_accel_instance, accel); - _notify_new_accel_raw_sample(_accel_instance, accel); + _rotate_and_correct_accel(accel_instance, accel); + _notify_new_accel_raw_sample(accel_instance, accel); } void AP_InertialSensor_BMI270::parse_gyro_frame(const uint8_t* d) @@ -502,8 +502,8 @@ void AP_InertialSensor_BMI270::parse_gyro_frame(const uint8_t* d) Vector3f gyro(xyz[0], xyz[1], xyz[2]); gyro *= scale; - _rotate_and_correct_gyro(_gyro_instance, gyro); - _notify_new_gyro_raw_sample(_gyro_instance, gyro); + _rotate_and_correct_gyro(gyro_instance, gyro); + _notify_new_gyro_raw_sample(gyro_instance, gyro); } bool AP_InertialSensor_BMI270::hardware_init() diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h index fb21ca0f57de5..0e16d230945c7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h @@ -115,8 +115,6 @@ class AP_InertialSensor_BMI270 : public AP_InertialSensor_Backend { enum Rotation _rotation; - uint8_t _accel_instance; - uint8_t _gyro_instance; uint8_t temperature_counter; static const uint8_t maximum_fifo_config_file[]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index e51d9362b80b5..b86b9bb57fe15 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -141,6 +141,10 @@ class AP_InertialSensor_Backend //Default Clip Limit float _clip_limit = 15.5f * GRAVITY_MSS; + // instance numbers of accel and gyro data + uint8_t gyro_instance; + uint8_t accel_instance; + void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__; void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__; @@ -281,6 +285,14 @@ class AP_InertialSensor_Backend _imu._accel_orientation[instance] = rotation; } + uint8_t get_gyro_instance() const { + return gyro_instance; + } + + uint8_t get_accel_instance() const { + return accel_instance; + } + // increment clipping counted. Used by drivers that do decimation before supplying // samples to the frontend void increment_clip_count(uint8_t instance) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h b/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h index 1af9e4c033c9f..cc3853aba261b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h @@ -21,8 +21,6 @@ class AP_InertialSensor_ExternalAHRS : public AP_InertialSensor_Backend bool get_output_banner(char* banner, uint8_t banner_len) override; private: - uint8_t gyro_instance; - uint8_t accel_instance; const uint8_t serial_port; bool started; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 50ff76205accd..18eb0a2080fa5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -202,8 +202,8 @@ void AP_InertialSensor_Invensense::_fifo_reset(bool log_error) _dev->set_speed(AP_HAL::Device::SPEED_HIGH); _last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN; - notify_accel_fifo_reset(_accel_instance); - notify_gyro_fifo_reset(_gyro_instance); + notify_accel_fifo_reset(accel_instance); + notify_gyro_fifo_reset(gyro_instance); } void AP_InertialSensor_Invensense::_fast_fifo_reset() @@ -211,8 +211,8 @@ void AP_InertialSensor_Invensense::_fast_fifo_reset() fast_reset_count++; _register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl | BIT_USER_CTRL_FIFO_RESET); - notify_accel_fifo_reset(_accel_instance); - notify_gyro_fifo_reset(_gyro_instance); + notify_accel_fifo_reset(accel_instance); + notify_gyro_fifo_reset(gyro_instance); } @@ -224,7 +224,7 @@ bool AP_InertialSensor_Invensense::_has_auxiliary_bus() void AP_InertialSensor_Invensense::start() { // pre-fetch instance numbers for checking fast sampling settings - if (!_imu.get_gyro_instance(_gyro_instance) || !_imu.get_accel_instance(_accel_instance)) { + if (!_imu.get_gyro_instance(gyro_instance) || !_imu.get_accel_instance(accel_instance)) { return; } @@ -320,8 +320,8 @@ void AP_InertialSensor_Invensense::start() break; } - if (!_imu.register_gyro(_gyro_instance, 1000, _dev->get_bus_id_devtype(gdev)) || - !_imu.register_accel(_accel_instance, 1000, _dev->get_bus_id_devtype(adev))) { + if (!_imu.register_gyro(gyro_instance, 1000, _dev->get_bus_id_devtype(gdev)) || + !_imu.register_accel(accel_instance, 1000, _dev->get_bus_id_devtype(adev))) { return; } @@ -329,12 +329,12 @@ void AP_InertialSensor_Invensense::start() _set_filter_register(); // update backend sample rate - _set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz); - _set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz); + _set_accel_raw_sample_rate(accel_instance, _accel_backend_rate_hz); + _set_gyro_raw_sample_rate(gyro_instance, _gyro_backend_rate_hz); // indicate what multiplier is appropriate for the sensors' // readings to fit them into an int16_t: - _set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel); + _set_raw_sample_accel_multiplier(accel_instance, multiplier_accel); // set sample rate to 1000Hz and apply a software filter // In this configuration, the gyro sample rate is 8kHz @@ -426,8 +426,8 @@ void AP_InertialSensor_Invensense::start() _dev->set_speed(AP_HAL::Device::SPEED_HIGH); // setup sensor rotations from probe() - set_gyro_orientation(_gyro_instance, _rotation); - set_accel_orientation(_accel_instance, _rotation); + set_gyro_orientation(gyro_instance, _rotation); + set_accel_orientation(accel_instance, _rotation); // setup scale factors for fifo data after downsampling _fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate; @@ -447,7 +447,7 @@ void AP_InertialSensor_Invensense::start() bool AP_InertialSensor_Invensense::get_output_banner(char* banner, uint8_t banner_len) { if (_fast_sampling) { snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz", - _gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001); + gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001); return true; } return false; @@ -459,10 +459,10 @@ bool AP_InertialSensor_Invensense::get_output_banner(char* banner, uint8_t banne */ bool AP_InertialSensor_Invensense::update() /* front end */ { - update_accel(_accel_instance); - update_gyro(_gyro_instance); + update_accel(accel_instance); + update_gyro(gyro_instance); - _publish_temperature(_accel_instance, _temp_filtered); + _publish_temperature(accel_instance, _temp_filtered); if (fast_reset_count) { // check if we have reported in the last 1 seconds or @@ -472,13 +472,13 @@ bool AP_InertialSensor_Invensense::update() /* front end */ if (now - last_fast_reset_count_report_ms > 5000U) { last_fast_reset_count_report_ms = now; char param_name[sizeof("IMUxx_RST")]; - snprintf(param_name, sizeof(param_name), "IMU%u_RST", MIN(_gyro_instance,9)); + snprintf(param_name, sizeof(param_name), "IMU%u_RST", MIN(gyro_instance,9)); gcs().send_named_float(param_name, fast_reset_count); } #endif #if HAL_LOGGING_ENABLED if (last_fast_reset_count != fast_reset_count) { - AP::logger().Write_MessageF("IMU%u fast fifo reset %u", _gyro_instance, fast_reset_count); + AP::logger().Write_MessageF("IMU%u fast fifo reset %u", gyro_instance, fast_reset_count); last_fast_reset_count = fast_reset_count; } #endif @@ -602,7 +602,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl return false; } else { if (!hal.scheduler->in_expected_delay()) { - debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); + debug("temp reset IMU[%u] %d %d", accel_instance, _raw_temp, t2); } _fifo_reset(true); return false; @@ -615,11 +615,11 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl -int16_val(data, 6)); gyro *= _gyro_scale; - _rotate_and_correct_accel(_accel_instance, accel); - _rotate_and_correct_gyro(_gyro_instance, gyro); + _rotate_and_correct_accel(accel_instance, accel); + _rotate_and_correct_gyro(gyro_instance, gyro); - _notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set); - _notify_new_gyro_raw_sample(_gyro_instance, gyro); + _notify_new_accel_raw_sample(accel_instance, accel, 0, fsync_set); + _notify_new_gyro_raw_sample(gyro_instance, gyro); _temp_filtered = _temp_filter.apply(temp); } @@ -652,7 +652,7 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam ret = false; } else { if (!hal.scheduler->in_expected_delay()) { - debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); + debug("temp reset IMU[%u] %d %d", accel_instance, _raw_temp, t2); } _fifo_reset(true); ret = false; @@ -674,14 +674,14 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam _accum.accel += _accum.accel_filter.apply(a); Vector3f a2 = a * _accel_scale; - _notify_new_accel_sensor_rate_sample(_accel_instance, a2); + _notify_new_accel_sensor_rate_sample(accel_instance, a2); _accum.accel_count++; if (_accum.accel_count % _accel_fifo_downsample_rate == 0) { _accum.accel *= _fifo_accel_scale; - _rotate_and_correct_accel(_accel_instance, _accum.accel); - _notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false); + _rotate_and_correct_accel(accel_instance, _accum.accel); + _notify_new_accel_raw_sample(accel_instance, _accum.accel, 0, false); _accum.accel.zero(); _accum.accel_count = 0; // we assume that the gyro rate is always >= and a multiple of the accel rate @@ -696,20 +696,20 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam -int16_val(data, 6)); Vector3f g2 = g * _gyro_scale; - _notify_new_gyro_sensor_rate_sample(_gyro_instance, g2); + _notify_new_gyro_sensor_rate_sample(gyro_instance, g2); _accum.gyro += g; if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) { _accum.gyro *= _fifo_gyro_scale; - _rotate_and_correct_gyro(_gyro_instance, _accum.gyro); - _notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro); + _rotate_and_correct_gyro(gyro_instance, _accum.gyro); + _notify_new_gyro_raw_sample(gyro_instance, _accum.gyro); _accum.gyro.zero(); } } if (clipped) { - increment_clip_count(_accel_instance); + increment_clip_count(accel_instance); } if (ret) { @@ -788,7 +788,7 @@ void AP_InertialSensor_Invensense::_read_fifo() if (_fast_sampling) { if (!_accumulate_sensor_rate_sampling(rx, n)) { if (!hal.scheduler->in_expected_delay() && !_enable_fast_fifo_reset) { - debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE); + debug("IMU[%u] stop at %u of %u", accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE); } break; } @@ -831,8 +831,8 @@ void AP_InertialSensor_Invensense::_read_fifo() AP_HAL::Device::checkreg reg; if (!_dev->check_next_register(reg)) { log_register_change(_dev->get_bus_id(), reg); - _inc_gyro_error_count(_gyro_instance); - _inc_accel_error_count(_accel_instance); + _inc_gyro_error_count(gyro_instance); + _inc_accel_error_count(accel_instance); } _dev->set_speed(AP_HAL::Device::SPEED_HIGH); } @@ -890,7 +890,7 @@ void AP_InertialSensor_Invensense::_set_filter_register(void) _gyro_to_accel_sample_ratio = 2; _gyro_backend_rate_hz = _accel_backend_rate_hz = 1000; - if (enable_fast_sampling(_accel_instance)) { + if (enable_fast_sampling(accel_instance)) { _fast_sampling = _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; if (_fast_sampling) { // constrain the gyro rate to be at least the loop rate @@ -919,11 +919,11 @@ void AP_InertialSensor_Invensense::_set_filter_register(void) } // for logging purposes set the oversamping rate - _set_accel_oversampling(_accel_instance, _accel_fifo_downsample_rate); - _set_gyro_oversampling(_gyro_instance, _gyro_fifo_downsample_rate); + _set_accel_oversampling(accel_instance, _accel_fifo_downsample_rate); + _set_gyro_oversampling(gyro_instance, _gyro_fifo_downsample_rate); - _set_accel_sensor_rate_sampling_enabled(_accel_instance, true); - _set_gyro_sensor_rate_sampling_enabled(_gyro_instance, true); + _set_accel_sensor_rate_sampling_enabled(accel_instance, true); + _set_gyro_sensor_rate_sampling_enabled(gyro_instance, true); /* set divider for internal sample rate to 0x1F when fast sampling enabled. This reduces the impact of the slave diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 7f77e35e48aaf..522dfd527d3c3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -115,10 +115,6 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend int16_t _raw_temp; - // instance numbers of accel and gyro data - uint8_t _gyro_instance; - uint8_t _accel_instance; - float temp_sensitivity = 1.0f/340; // degC/LSB float temp_zero = 36.53f; // degC diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp index 1e8be9d2b56a1..b8279d339fd62 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp @@ -155,8 +155,8 @@ void AP_InertialSensor_Invensensev2::_fifo_reset() _dev->set_speed(AP_HAL::Device::SPEED_HIGH); _last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN; - notify_accel_fifo_reset(_accel_instance); - notify_gyro_fifo_reset(_gyro_instance); + notify_accel_fifo_reset(accel_instance); + notify_gyro_fifo_reset(gyro_instance); } bool AP_InertialSensor_Invensensev2::_has_auxiliary_bus() @@ -167,7 +167,7 @@ bool AP_InertialSensor_Invensensev2::_has_auxiliary_bus() void AP_InertialSensor_Invensensev2::start() { // pre-fetch instance numbers for checking fast sampling settings - if (!_imu.get_gyro_instance(_gyro_instance) || !_imu.get_accel_instance(_accel_instance)) { + if (!_imu.get_gyro_instance(gyro_instance) || !_imu.get_accel_instance(accel_instance)) { return; } @@ -207,8 +207,8 @@ void AP_InertialSensor_Invensensev2::start() break; } - if (!_imu.register_gyro(_gyro_instance, 1125, _dev->get_bus_id_devtype(gdev)) || - !_imu.register_accel(_accel_instance, 1125, _dev->get_bus_id_devtype(adev))) { + if (!_imu.register_gyro(gyro_instance, 1125, _dev->get_bus_id_devtype(gdev)) || + !_imu.register_accel(accel_instance, 1125, _dev->get_bus_id_devtype(adev))) { return; } @@ -219,12 +219,12 @@ void AP_InertialSensor_Invensensev2::start() _register_write(INV2REG_FSYNC_CONFIG, FSYNC_CONFIG_EXT_SYNC_AZ, true); #endif // update backend sample rate - _set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz); - _set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz); + _set_accel_raw_sample_rate(accel_instance, _accel_backend_rate_hz); + _set_gyro_raw_sample_rate(gyro_instance, _gyro_backend_rate_hz); // indicate what multiplier is appropriate for the sensors' // readings to fit them into an int16_t: - _set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel); + _set_raw_sample_accel_multiplier(accel_instance, multiplier_accel); // set sample rate to 1.125KHz _register_write(INV2REG_GYRO_SMPLRT_DIV, 0, true); @@ -238,8 +238,8 @@ void AP_InertialSensor_Invensensev2::start() _dev->set_speed(AP_HAL::Device::SPEED_HIGH); // setup sensor rotations from probe() - set_gyro_orientation(_gyro_instance, _rotation); - set_accel_orientation(_accel_instance, _rotation); + set_gyro_orientation(gyro_instance, _rotation); + set_accel_orientation(accel_instance, _rotation); // setup scale factors for fifo data after downsampling _fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate; @@ -259,7 +259,7 @@ void AP_InertialSensor_Invensensev2::start() bool AP_InertialSensor_Invensensev2::get_output_banner(char* banner, uint8_t banner_len) { if (_fast_sampling) { snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz", - _gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001); + gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001); return true; } return false; @@ -270,10 +270,10 @@ bool AP_InertialSensor_Invensensev2::get_output_banner(char* banner, uint8_t ban */ bool AP_InertialSensor_Invensensev2::update() { - update_accel(_accel_instance); - update_gyro(_gyro_instance); + update_accel(accel_instance); + update_gyro(gyro_instance); - _publish_temperature(_accel_instance, _temp_filtered); + _publish_temperature(accel_instance, _temp_filtered); return true; } @@ -341,7 +341,7 @@ bool AP_InertialSensor_Invensensev2::_accumulate(uint8_t *samples, uint8_t n_sam int16_t t2 = int16_val(data, 6); if (!_check_raw_temp(t2)) { if (!hal.scheduler->in_expected_delay()) { - debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); + debug("temp reset IMU[%u] %d %d", accel_instance, _raw_temp, t2); } _fifo_reset(); return false; @@ -353,11 +353,11 @@ bool AP_InertialSensor_Invensensev2::_accumulate(uint8_t *samples, uint8_t n_sam -int16_val(data, 5)); gyro *= GYRO_SCALE; - _rotate_and_correct_accel(_accel_instance, accel); - _rotate_and_correct_gyro(_gyro_instance, gyro); + _rotate_and_correct_accel(accel_instance, accel); + _rotate_and_correct_gyro(gyro_instance, gyro); - _notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set); - _notify_new_gyro_raw_sample(_gyro_instance, gyro); + _notify_new_accel_raw_sample(accel_instance, accel, 0, fsync_set); + _notify_new_gyro_raw_sample(gyro_instance, gyro); _temp_filtered = _temp_filter.apply(temp); } @@ -386,7 +386,7 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s int16_t t2 = int16_val(data, 6); if (!_check_raw_temp(t2)) { if (!hal.scheduler->in_expected_delay()) { - debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); + debug("temp reset IMU[%u] %d %d", accel_instance, _raw_temp, t2); } _fifo_reset(); ret = false; @@ -406,14 +406,14 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s _accum.accel += _accum.accel_filter.apply(a); Vector3f a2 = a * _accel_scale; - _notify_new_accel_sensor_rate_sample(_accel_instance, a2); + _notify_new_accel_sensor_rate_sample(accel_instance, a2); _accum.accel_count++; if (_accum.accel_count % _accel_fifo_downsample_rate == 0) { _accum.accel *= _fifo_accel_scale; - _rotate_and_correct_accel(_accel_instance, _accum.accel); - _notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false); + _rotate_and_correct_accel(accel_instance, _accum.accel); + _notify_new_accel_raw_sample(accel_instance, _accum.accel, 0, false); _accum.accel.zero(); _accum.accel_count = 0; // we assume that the gyro rate is always >= and a multiple of the accel rate @@ -428,20 +428,20 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s -int16_val(data, 5)); Vector3f g2 = g * GYRO_SCALE; - _notify_new_gyro_sensor_rate_sample(_gyro_instance, g2); + _notify_new_gyro_sensor_rate_sample(gyro_instance, g2); _accum.gyro += g; if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) { _accum.gyro *= _fifo_gyro_scale; - _rotate_and_correct_gyro(_gyro_instance, _accum.gyro); - _notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro); + _rotate_and_correct_gyro(gyro_instance, _accum.gyro); + _notify_new_gyro_raw_sample(gyro_instance, _accum.gyro); _accum.gyro.zero(); } } if (clipped) { - increment_clip_count(_accel_instance); + increment_clip_count(accel_instance); } if (ret) { @@ -519,7 +519,7 @@ void AP_InertialSensor_Invensensev2::_read_fifo() if (_fast_sampling) { if (!_accumulate_sensor_rate_sampling(rx, n)) { if (!hal.scheduler->in_expected_delay()) { - debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/INV2_SAMPLE_SIZE); + debug("IMU[%u] stop at %u of %u", accel_instance, n_samples, bytes_read/INV2_SAMPLE_SIZE); } break; } @@ -542,8 +542,8 @@ void AP_InertialSensor_Invensensev2::_read_fifo() AP_HAL::Device::checkreg reg; if (!_dev->check_next_register(reg)) { log_register_change(_dev->get_bus_id(), reg); - _inc_gyro_error_count(_gyro_instance); - _inc_accel_error_count(_accel_instance); + _inc_gyro_error_count(gyro_instance); + _inc_accel_error_count(accel_instance); } _dev->set_speed(AP_HAL::Device::SPEED_HIGH); } @@ -608,7 +608,7 @@ void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void) _gyro_fifo_downsample_rate = _accel_fifo_downsample_rate = 1; _gyro_backend_rate_hz = _accel_backend_rate_hz = 1125; - if (enable_fast_sampling(_accel_instance)) { + if (enable_fast_sampling(accel_instance)) { _fast_sampling = _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; if (_fast_sampling) { // constrain the gyro rate to be at least the loop rate @@ -631,11 +631,11 @@ void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void) _accel_backend_rate_hz *= MIN(fast_sampling_rate, 4); // for logging purposes set the oversamping rate - _set_accel_oversampling(_accel_instance, _accel_fifo_downsample_rate); - _set_gyro_oversampling(_gyro_instance, _gyro_fifo_downsample_rate); + _set_accel_oversampling(accel_instance, _accel_fifo_downsample_rate); + _set_gyro_oversampling(gyro_instance, _gyro_fifo_downsample_rate); - _set_accel_sensor_rate_sampling_enabled(_accel_instance, true); - _set_gyro_sensor_rate_sampling_enabled(_gyro_instance, true); + _set_accel_sensor_rate_sampling_enabled(accel_instance, true); + _set_gyro_sensor_rate_sampling_enabled(gyro_instance, true); /* set divider for internal sample rate to 0x1F when fast sampling enabled. This reduces the impact of the slave diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h index 9becc1d60e644..b6e21d0034d1d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h @@ -103,10 +103,6 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend int16_t _raw_temp; - // instance numbers of accel and gyro data - uint8_t _gyro_instance; - uint8_t _accel_instance; - float temp_sensitivity = 1.0f/333.87f; // degC/LSB float temp_zero = 21; // degC diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index aa9971db0d48e..b3946aa81dd47 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -74,10 +74,6 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend bool accumulate_samples(const struct FIFOData *data, uint8_t n_samples); bool accumulate_highres_samples(const struct FIFODataHighRes *data, uint8_t n_samples); - // instance numbers of accel and gyro data - uint8_t gyro_instance; - uint8_t accel_instance; - // reset FIFO configure1 register uint8_t fifo_config1; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 0d97d71f8448d..82a19e2261656 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -250,8 +250,8 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void) */ void AP_InertialSensor_L3G4200D::start(void) { - if (!_imu.register_gyro(_gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_L3G4200D)) || - !_imu.register_accel(_accel_instance, 800, _dev_accel->get_bus_id_devtype(DEVTYPE_L3G4200D))) { + if (!_imu.register_gyro(gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_L3G4200D)) || + !_imu.register_accel(accel_instance, 800, _dev_accel->get_bus_id_devtype(DEVTYPE_L3G4200D))) { return; } @@ -276,8 +276,8 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz) */ bool AP_InertialSensor_L3G4200D::update(void) { - update_gyro(_gyro_instance); - update_accel(_accel_instance); + update_gyro(gyro_instance); + update_accel(accel_instance); return true; } @@ -313,8 +313,8 @@ void AP_InertialSensor_L3G4200D::_accumulate_gyro (void) // Adjust for chip scaling to get radians/sec //hal.console->printf("gyro %f \r\n",gyro.x); gyro *= L3G4200D_GYRO_SCALE_R_S; - _rotate_and_correct_gyro(_gyro_instance, gyro); - _notify_new_gyro_raw_sample(_gyro_instance, gyro); + _rotate_and_correct_gyro(gyro_instance, gyro); + _notify_new_gyro_raw_sample(gyro_instance, gyro); } } } @@ -341,8 +341,8 @@ void AP_InertialSensor_L3G4200D::_accumulate_accel (void) Vector3f accel = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]); // Adjust for chip scaling to get m/s/s accel *= ADXL345_ACCELEROMETER_SCALE_M_S; - _rotate_and_correct_accel(_accel_instance, accel); - _notify_new_accel_raw_sample(_accel_instance, accel); + _rotate_and_correct_accel(accel_instance, accel); + _notify_new_accel_raw_sample(accel_instance, accel); } } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h index 535613571d49e..b97b430f6df16 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h @@ -52,9 +52,5 @@ class AP_InertialSensor_L3G4200D : public AP_InertialSensor_Backend LowPassFilter2pVector3f _gyro_filter; enum Rotation _rotation; - - // gyro and accel instances - uint8_t _gyro_instance; - uint8_t _accel_instance; }; #endif // __AP_INERTIAL_SENSOR_L3G4200D2_H__ \ No newline at end of file diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index e6d91738cd4e1..4c1919c734bb9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -512,19 +512,19 @@ bool AP_InertialSensor_LSM9DS0::_hardware_init() */ void AP_InertialSensor_LSM9DS0::start(void) { - if (!_imu.register_gyro(_gyro_instance, 760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20)) || - !_imu.register_accel(_accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D))) { + if (!_imu.register_gyro(gyro_instance, 760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20)) || + !_imu.register_accel(accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D))) { return; } if (whoami_g == LSM9DS0_G_WHOAMI_H) { - set_gyro_orientation(_gyro_instance, _rotation_gH); + set_gyro_orientation(gyro_instance, _rotation_gH); } else { - set_gyro_orientation(_gyro_instance, _rotation_g); + set_gyro_orientation(gyro_instance, _rotation_g); } - set_accel_orientation(_accel_instance, _rotation_a); + set_accel_orientation(accel_instance, _rotation_a); - _set_accel_max_abs_offset(_accel_instance, 5.0f); + _set_accel_max_abs_offset(accel_instance, 5.0f); /* start the timer process to read samples */ _dev_gyro->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, void)); @@ -700,11 +700,11 @@ void AP_InertialSensor_LSM9DS0::_poll_data() AP_HAL::Device::checkreg reg; if (!_dev_gyro->check_next_register(reg)) { log_register_change(_dev_gyro->get_bus_id(), reg); - _inc_gyro_error_count(_gyro_instance); + _inc_gyro_error_count(gyro_instance); } if (!_dev_accel->check_next_register(reg)) { log_register_change(_dev_accel->get_bus_id(), reg); - _inc_accel_error_count(_accel_instance); + _inc_accel_error_count(accel_instance); } } @@ -740,8 +740,8 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a() Vector3f accel_data(raw_data.x, -raw_data.y, -raw_data.z); accel_data *= _accel_scale; - _rotate_and_correct_accel(_accel_instance, accel_data); - _notify_new_accel_raw_sample(_accel_instance, accel_data, AP_HAL::micros64()); + _rotate_and_correct_accel(accel_instance, accel_data); + _notify_new_accel_raw_sample(accel_instance, accel_data, AP_HAL::micros64()); // read temperature every 10th sample if (_temp_counter++ >= 10) { @@ -770,15 +770,15 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g() gyro_data *= _gyro_scale; - _rotate_and_correct_gyro(_gyro_instance, gyro_data); - _notify_new_gyro_raw_sample(_gyro_instance, gyro_data, AP_HAL::micros64()); + _rotate_and_correct_gyro(gyro_instance, gyro_data); + _notify_new_gyro_raw_sample(gyro_instance, gyro_data, AP_HAL::micros64()); } bool AP_InertialSensor_LSM9DS0::update() { - update_gyro(_gyro_instance); - update_accel(_accel_instance); - _publish_temperature(_accel_instance, _temperature); + update_gyro(gyro_instance); + update_accel(accel_instance); + _publish_temperature(accel_instance, _temperature); return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h index 29fff90270541..18244897a24ee 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h @@ -97,8 +97,6 @@ class AP_InertialSensor_LSM9DS0 : public AP_InertialSensor_Backend float _accel_scale; int _drdy_pin_num_a; int _drdy_pin_num_g; - uint8_t _gyro_instance; - uint8_t _accel_instance; float _temperature; uint8_t _temp_counter; LowPassFilter2pFloat _temp_filter; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp index df73ecb62fe04..d95cfce0f0fcc 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp @@ -321,8 +321,8 @@ void AP_InertialSensor_LSM9DS1::_fifo_reset() _dev->set_speed(AP_HAL::Device::SPEED_HIGH); - notify_accel_fifo_reset(_accel_instance); - notify_gyro_fifo_reset(_gyro_instance); + notify_accel_fifo_reset(accel_instance); + notify_gyro_fifo_reset(gyro_instance); } /* @@ -330,15 +330,15 @@ void AP_InertialSensor_LSM9DS1::_fifo_reset() */ void AP_InertialSensor_LSM9DS1::start(void) { - if (!_imu.register_gyro(_gyro_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_GYR_LSM9DS1)) || - !_imu.register_accel(_accel_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_ACC_LSM9DS1))) { + if (!_imu.register_gyro(gyro_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_GYR_LSM9DS1)) || + !_imu.register_accel(accel_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_ACC_LSM9DS1))) { return; } - set_accel_orientation(_accel_instance, _rotation); - set_gyro_orientation(_gyro_instance, _rotation); + set_accel_orientation(accel_instance, _rotation); + set_gyro_orientation(gyro_instance, _rotation); - _set_accel_max_abs_offset(_accel_instance, 5.0f); + _set_accel_max_abs_offset(accel_instance, 5.0f); /* start the timer process to read samples */ _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS1::_poll_data, void)); @@ -431,7 +431,7 @@ void AP_InertialSensor_LSM9DS1::_poll_data() AP_HAL::Device::checkreg reg; if (!_dev->check_next_register(reg)) { log_register_change(_dev->get_bus_id(), reg); - _inc_accel_error_count(_accel_instance); + _inc_accel_error_count(accel_instance); } } @@ -465,8 +465,8 @@ void AP_InertialSensor_LSM9DS1::_read_data_transaction_x(uint16_t samples) Vector3f accel_data(raw_data.x, raw_data.y, -raw_data.z); accel_data *= _accel_scale; - _rotate_and_correct_accel(_accel_instance, accel_data); - _notify_new_accel_raw_sample(_accel_instance, accel_data); + _rotate_and_correct_accel(accel_instance, accel_data); + _notify_new_accel_raw_sample(accel_instance, accel_data); } /* @@ -501,14 +501,14 @@ void AP_InertialSensor_LSM9DS1::_read_data_transaction_g(uint16_t samples) Vector3f gyro_data(raw_data.x, raw_data.y, -raw_data.z); gyro_data *= _gyro_scale; - _rotate_and_correct_gyro(_gyro_instance, gyro_data); - _notify_new_gyro_raw_sample(_gyro_instance, gyro_data); + _rotate_and_correct_gyro(gyro_instance, gyro_data); + _notify_new_gyro_raw_sample(gyro_instance, gyro_data); } bool AP_InertialSensor_LSM9DS1::update() { - update_gyro(_gyro_instance); - update_accel(_accel_instance); + update_gyro(gyro_instance); + update_accel(accel_instance); return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.h index f52859c1bbd96..7997476abac39 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.h @@ -68,7 +68,5 @@ class AP_InertialSensor_LSM9DS1 : public AP_InertialSensor_Backend float _gyro_scale; float _accel_scale; int _drdy_pin_num_xg; - uint8_t _gyro_instance; - uint8_t _accel_instance; enum Rotation _rotation; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp index 860ae4aa261db..fa061c4078125 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp @@ -336,13 +336,13 @@ bool AP_InertialSensor_RST::_init_sensor(void) */ void AP_InertialSensor_RST::start(void) { - if (!_imu.register_gyro(_gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_I3G4250D)) || - !_imu.register_accel(_accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_IIS328DQ))) { + if (!_imu.register_gyro(gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_I3G4250D)) || + !_imu.register_accel(accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_IIS328DQ))) { return; } - set_gyro_orientation(_gyro_instance, _rotation_g); - set_accel_orientation(_accel_instance, _rotation_a); + set_gyro_orientation(gyro_instance, _rotation_g); + set_accel_orientation(accel_instance, _rotation_a); // start the timer process to read samples _dev_gyro->register_periodic_callback(1150, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_RST::gyro_measure, void)); @@ -354,8 +354,8 @@ void AP_InertialSensor_RST::start(void) */ bool AP_InertialSensor_RST::update(void) { - update_gyro(_gyro_instance); - update_accel(_accel_instance); + update_gyro(gyro_instance); + update_accel(accel_instance); return true; } @@ -376,8 +376,8 @@ void AP_InertialSensor_RST::gyro_measure(void) if (_dev_gyro->read_registers(GYRO_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) { gyro = Vector3f(raw_data[0], raw_data[1], raw_data[2]); gyro *= _gyro_scale; - _rotate_and_correct_gyro(_gyro_instance, gyro); - _notify_new_gyro_raw_sample(_gyro_instance, gyro); + _rotate_and_correct_gyro(gyro_instance, gyro); + _notify_new_gyro_raw_sample(gyro_instance, gyro); } } @@ -397,8 +397,8 @@ void AP_InertialSensor_RST::accel_measure(void) if (_dev_accel->read_registers(ACCEL_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) { accel = Vector3f(raw_data[0], raw_data[1], raw_data[2]); accel *= _accel_scale; - _rotate_and_correct_accel(_accel_instance, accel); - _notify_new_accel_raw_sample(_accel_instance, accel); + _rotate_and_correct_accel(accel_instance, accel); + _notify_new_accel_raw_sample(accel_instance, accel); } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_RST.h b/libraries/AP_InertialSensor/AP_InertialSensor_RST.h index 90d76d3e39376..0efaa6cbbef86 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_RST.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_RST.h @@ -47,9 +47,6 @@ class AP_InertialSensor_RST : public AP_InertialSensor_Backend float _gyro_scale; float _accel_scale; - // gyro and accel instances - uint8_t _gyro_instance; - uint8_t _accel_instance; enum Rotation _rotation_g; enum Rotation _rotation_a; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCHA63T.h b/libraries/AP_InertialSensor/AP_InertialSensor_SCHA63T.h index 3ffc203a26ece..64110a1260404 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SCHA63T.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCHA63T.h @@ -84,7 +84,5 @@ class AP_InertialSensor_SCHA63T : public AP_InertialSensor_Backend AP_HAL::OwnPtr dev_uno; AP_HAL::OwnPtr dev_due; - uint8_t accel_instance; - uint8_t gyro_instance; enum Rotation rotation; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h index ac95bac811e25..856f40ab003a7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h @@ -45,8 +45,6 @@ class AP_InertialSensor_SITL : public AP_InertialSensor_Backend const uint16_t gyro_sample_hz; const uint16_t accel_sample_hz; - uint8_t gyro_instance; - uint8_t accel_instance; uint64_t next_gyro_sample; uint64_t next_accel_sample; float gyro_time;