Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

rename ins get_primary_accel to get_default_accel #27383

10 changes: 5 additions & 5 deletions libraries/AP_AHRS/AP_AHRS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -591,8 +591,8 @@ void AP_AHRS::update_EKF2(void)
// Use the primary EKF to select the primary gyro
const AP_InertialSensor &_ins = AP::ins();
const int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
const uint8_t primary_gyro = primary_imu>=0?primary_imu:_ins.get_primary_gyro();
const uint8_t primary_accel = primary_imu>=0?primary_imu:_ins.get_primary_accel();
const uint8_t primary_gyro = primary_imu>=0?primary_imu:_ins.get_default_gyro();
const uint8_t primary_accel = primary_imu>=0?primary_imu:_ins.get_default_accel();

// get gyro bias for primary EKF and change sign to give gyro drift
// Note sign convention used by EKF is bias = measurement - truth
Expand Down Expand Up @@ -660,8 +660,8 @@ void AP_AHRS::update_EKF3(void)

// Use the primary EKF to select the primary gyro
const int8_t primary_imu = EKF3.getPrimaryCoreIMUIndex();
const uint8_t primary_gyro = primary_imu>=0?primary_imu:_ins.get_primary_gyro();
const uint8_t primary_accel = primary_imu>=0?primary_imu:_ins.get_primary_accel();
const uint8_t primary_gyro = primary_imu>=0?primary_imu:_ins.get_default_gyro();
const uint8_t primary_accel = primary_imu>=0?primary_imu:_ins.get_default_accel();

// get gyro bias for primary EKF and change sign to give gyro drift
// Note sign convention used by EKF is bias = measurement - truth
Expand Down Expand Up @@ -3215,7 +3215,7 @@ uint8_t AP_AHRS::_get_primary_IMU_index() const
#endif
}
if (imu == -1) {
imu = AP::ins().get_primary_gyro();
imu = AP::ins().get_default_gyro();
}
return imu;
}
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_AHRS/AP_AHRS_Backend.h
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ class AP_AHRS_Backend
// get the index of the current primary accelerometer sensor
virtual uint8_t get_primary_accel_index(void) const {
#if AP_INERTIALSENSOR_ENABLED
return AP::ins().get_primary_accel();
return AP::ins().get_default_accel();
#else
return 0;
#endif
Expand All @@ -86,7 +86,7 @@ class AP_AHRS_Backend
// get the index of the current primary gyro sensor
virtual uint8_t get_primary_gyro_index(void) const {
#if AP_INERTIALSENSOR_ENABLED
return AP::ins().get_primary_gyro();
return AP::ins().get_default_gyro();
#else
return 0;
#endif
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_DAL/AP_DAL_InertialSensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,9 @@ void AP_DAL_InertialSensor::start_frame()
const log_RISH old_RISH = _RISH;

_RISH.loop_rate_hz = ins.get_loop_rate_hz();
_RISH.primary_gyro = ins.get_primary_gyro();
_RISH.default_gyro = ins.get_default_gyro();
_RISH.loop_delta_t = ins.get_loop_delta_t();
_RISH.primary_accel = ins.get_primary_accel();
_RISH.default_accel = ins.get_default_accel();
_RISH.accel_count = ins.get_accel_count();
_RISH.gyro_count = ins.get_gyro_count();
WRITE_REPLAY_BLOCK_IFCHANGED(RISH, _RISH, old_RISH);
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_DAL/AP_DAL_InertialSensor.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ class AP_DAL_InertialSensor {

// accel stuff
uint8_t get_accel_count(void) const { return _RISH.accel_count; }
uint8_t get_primary_accel(void) const { return _RISH.primary_accel; };
uint8_t get_default_accel(void) const { return _RISH.default_accel; };

bool use_accel(uint8_t instance) const { return _RISI[instance].use_accel; }
const Vector3f &get_accel(uint8_t i) const { return accel_filtered[i]; }
Expand All @@ -30,7 +30,7 @@ class AP_DAL_InertialSensor {

// gyro stuff
uint8_t get_gyro_count(void) const { return _RISH.gyro_count; }
uint8_t get_primary_gyro(void) const { return _RISH.primary_gyro; };
uint8_t get_default_gyro(void) const { return _RISH.default_gyro; };

bool use_gyro(uint8_t instance) const { return _RISI[instance].use_gyro; }
const Vector3f &get_gyro(uint8_t i) const { return gyro_filtered[i]; }
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_DAL/LogStructure.h
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,8 @@ struct log_RFRN {
// Replay Data Structure - Inertial Sensor header
struct log_RISH {
uint16_t loop_rate_hz;
uint8_t primary_gyro;
uint8_t primary_accel;
uint8_t default_gyro;
uint8_t default_accel;
float loop_delta_t;
uint8_t accel_count;
uint8_t gyro_count;
Expand Down
16 changes: 8 additions & 8 deletions libraries/AP_InertialSensor/AP_InertialSensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1933,13 +1933,13 @@ void AP_InertialSensor::update(void)
// set primary to first healthy accel and gyro
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i] && _use(i)) {
_primary_gyro = i;
_default_gyro = i;
break;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i] && _use(i)) {
_primary_accel = i;
_default_accel = i;
break;
}
}
Expand Down Expand Up @@ -2209,7 +2209,7 @@ void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vect
// peak hold detector for slower mechanisms to detect spikes
void AP_InertialSensor::set_accel_peak_hold(uint8_t instance, const Vector3f &accel)
{
if (instance != _primary_accel) {
if (instance != _default_accel) {
// we only record for primary accel
return;
}
Expand Down Expand Up @@ -2390,15 +2390,15 @@ void AP_InertialSensor::_acal_save_calibrations()
case 1:
// The first level step of accel cal will be taken as gnd truth,
// i.e. trim will be set as per the output of primary accel from the level step
get_primary_accel_cal_sample_avg(0,aligned_sample);
get_default_accel_cal_sample_avg(0,aligned_sample);
_trim_rad.zero();
_calculate_trim(aligned_sample, _trim_rad);
_new_trim = true;
break;
case 2:
// Reference accel is truth, in this scenario there is a reference accel
// as mentioned in ACC_BODY_ALIGNED
if (get_primary_accel_cal_sample_avg(0,misaligned_sample) && get_fixed_mount_accel_cal_sample(0,aligned_sample)) {
if (get_default_accel_cal_sample_avg(0,misaligned_sample) && get_fixed_mount_accel_cal_sample(0,aligned_sample)) {
// determine trim from aligned sample vs misaligned sample
Vector3f cross = (misaligned_sample%aligned_sample);
float dot = (misaligned_sample*aligned_sample);
Expand Down Expand Up @@ -2462,7 +2462,7 @@ bool AP_InertialSensor::get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vec
/*
Returns Primary accelerometer level data averaged during accel calibration's first step
*/
bool AP_InertialSensor::get_primary_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const
bool AP_InertialSensor::get_default_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const
{
uint8_t count = 0;
Vector3f avg = Vector3f(0,0,0);
Expand Down Expand Up @@ -2749,8 +2749,8 @@ void AP_InertialSensor::send_uart_data(void)
data.length = sizeof(data);
data.timestamp_us = AP_HAL::micros();

get_delta_angle(get_primary_gyro(), data.delta_angle, data.delta_angle_dt);
get_delta_velocity(get_primary_accel(), data.delta_velocity, data.delta_velocity_dt);
get_delta_angle(get_default_gyro(), data.delta_angle, data.delta_angle_dt);
get_delta_velocity(get_default_accel(), data.delta_velocity, data.delta_velocity_dt);

data.counter = uart.counter++;
data.crc = crc_xmodem((const uint8_t *)&data, sizeof(data)-sizeof(uint16_t));
Expand Down
40 changes: 20 additions & 20 deletions libraries/AP_InertialSensor/AP_InertialSensor.h
Original file line number Diff line number Diff line change
Expand Up @@ -108,34 +108,34 @@ class AP_InertialSensor : AP_AccelCal_Client
/// @returns vector of rotational rates in radians/sec
///
const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; }
const Vector3f &get_gyro(void) const { return get_gyro(_primary_gyro); }
const Vector3f &get_gyro(void) const { return get_gyro(_default_gyro); }

// set gyro offsets in radians/sec
const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset(i); }
const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); }
const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_default_gyro); }

//get delta angle if available
bool get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const;
bool get_delta_angle(Vector3f &delta_angle, float &delta_angle_dt) const {
return get_delta_angle(_primary_gyro, delta_angle, delta_angle_dt);
return get_delta_angle(_default_gyro, delta_angle, delta_angle_dt);
}

//get delta velocity if available
bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const;
bool get_delta_velocity(Vector3f &delta_velocity, float &delta_velocity_dt) const {
return get_delta_velocity(_primary_accel, delta_velocity, delta_velocity_dt);
return get_delta_velocity(_default_accel, delta_velocity, delta_velocity_dt);
}

/// Fetch the current accelerometer values
///
/// @returns vector of current accelerations in m/s/s
///
const Vector3f &get_accel(uint8_t i) const { return _accel[i]; }
const Vector3f &get_accel(void) const { return get_accel(_primary_accel); }
const Vector3f &get_accel(void) const { return get_accel(_default_accel); }

// multi-device interface
bool get_gyro_health(uint8_t instance) const { return (instance<_gyro_count) ? _gyro_healthy[instance] : false; }
bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
bool get_gyro_health(void) const { return get_gyro_health(_default_gyro); }
bool get_gyro_health_all(void) const;
bool gyros_consistent(uint8_t threshold) const;
uint8_t get_gyro_count(void) const { return MIN(INS_MAX_INSTANCES, _gyro_count); }
Expand All @@ -145,7 +145,7 @@ class AP_InertialSensor : AP_AccelCal_Client
Gyro_Calibration_Timing gyro_calibration_timing();

bool get_accel_health(uint8_t instance) const { return (instance<_accel_count) ? _accel_healthy[instance] : false; }
bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
bool get_accel_health(void) const { return get_accel_health(_default_accel); }
bool get_accel_health_all(void) const;
bool accels_consistent(float accel_error_threshold) const;
uint8_t get_accel_count(void) const { return MIN(INS_MAX_INSTANCES, _accel_count); }
Expand All @@ -158,30 +158,30 @@ class AP_InertialSensor : AP_AccelCal_Client

// FFT support access
#if HAL_GYROFFT_ENABLED
const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_primary_gyro]; }
const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_default_gyro]; }
FloatBuffer& get_raw_gyro_window(uint8_t instance, uint8_t axis) { return _gyro_window[instance][axis]; }
FloatBuffer& get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_primary_gyro, axis); }
uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_primary_gyro); }
uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_primary_gyro]; }
FloatBuffer& get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_default_gyro, axis); }
uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_default_gyro); }
uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_default_gyro]; }
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
bool has_fft_notch() const;
#endif
#endif
bool set_gyro_window_size(uint16_t size);
// get accel offsets in m/s/s
const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset(i); }
const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); }
const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_default_accel); }

// get accel scale
const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale(i); }
const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); }
const Vector3f &get_accel_scale(void) const { return get_accel_scale(_default_accel); }

// return a 3D vector defining the position offset of the IMU accelerometer in metres relative to the body frame origin
const Vector3f &get_imu_pos_offset(uint8_t instance) const {
return _accel_pos(instance);
}
const Vector3f &get_imu_pos_offset(void) const {
return _accel_pos(_primary_accel);
return _accel_pos(_default_accel);
}

// return the temperature if supported. Zero is returned if no
Expand Down Expand Up @@ -222,8 +222,8 @@ class AP_InertialSensor : AP_AccelCal_Client

bool healthy(void) const { return get_gyro_health() && get_accel_health(); }

uint8_t get_primary_accel(void) const { return _primary_accel; }
uint8_t get_primary_gyro(void) const { return _primary_gyro; }
uint8_t get_default_accel(void) const { return _default_accel; }
uint8_t get_default_gyro(void) const { return _default_gyro; }

// get the gyro filter rate in Hz
uint16_t get_gyro_filter_hz(void) const { return _gyro_filter_cutoff; }
Expand All @@ -250,7 +250,7 @@ class AP_InertialSensor : AP_AccelCal_Client
void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt);

// retrieve latest calculated vibration levels
Vector3f get_vibration_levels() const { return get_vibration_levels(_primary_accel); }
Vector3f get_vibration_levels() const { return get_vibration_levels(_default_accel); }
Vector3f get_vibration_levels(uint8_t instance) const;

// retrieve and clear accelerometer clipping count
Expand All @@ -275,7 +275,7 @@ class AP_InertialSensor : AP_AccelCal_Client
bool get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vector3f& ret) const;

// Returns primary accelerometer level data averaged during accel calibration's first step
bool get_primary_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const;
bool get_default_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const;

// Returns newly calculated trim values if calculated
bool get_new_trim(Vector3f &trim_rad);
Expand Down Expand Up @@ -657,8 +657,8 @@ class AP_InertialSensor : AP_AccelCal_Client
bool _accel_id_ok[INS_MAX_INSTANCES];

// primary accel and gyro
uint8_t _primary_gyro;
uint8_t _primary_accel;
uint8_t _default_gyro;
uint8_t _default_accel;

// mask of accels and gyros which we will be actively using
// and this should wait for in wait_for_sample()
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Mount/SoloGimbal.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -245,7 +245,7 @@ void SoloGimbal::update_fast() {
// single gyro mode - one of the first two gyros are unhealthy or don't exist
// just read primary gyro
Vector3f dAng;
readVehicleDeltaAngle(ins.get_primary_gyro(), dAng);
readVehicleDeltaAngle(ins.get_default_gyro(), dAng);
_vehicle_delta_angles += dAng;
}
}
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -335,13 +335,13 @@ void NavEKF2_core::readIMUData()
if (ins.use_accel(imu_index)) {
accel_active = imu_index;
} else {
accel_active = ins.get_primary_accel();
accel_active = ins.get_default_accel();
}

if (ins.use_gyro(imu_index)) {
gyro_active = imu_index;
} else {
gyro_active = ins.get_primary_gyro();
gyro_active = ins.get_default_gyro();
}

if (gyro_active != gyro_index_active) {
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -387,13 +387,13 @@ void NavEKF3_core::readIMUData()
if (ins.use_accel(imu_index)) {
accel_active = imu_index;
} else {
accel_active = ins.get_primary_accel();
accel_active = ins.get_default_accel();
}

if (ins.use_gyro(imu_index)) {
gyro_active = imu_index;
} else {
gyro_active = ins.get_primary_gyro();
gyro_active = ins.get_default_gyro();
}

if (gyro_active != gyro_index_active) {
Expand Down
2 changes: 1 addition & 1 deletion libraries/GCS_MAVLink/GCS_Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2172,7 +2172,7 @@ void GCS_MAVLINK::send_highres_imu()
.temperature = 0.0,
.fields_updated = (HIGHRES_IMU_UPDATED_XACC | HIGHRES_IMU_UPDATED_YACC | HIGHRES_IMU_UPDATED_ZACC |
HIGHRES_IMU_UPDATED_XGYRO | HIGHRES_IMU_UPDATED_YGYRO | HIGHRES_IMU_UPDATED_ZGYRO),
.id = ins.get_primary_accel(),
.id = ins.get_default_accel(),
};


Expand Down
Loading