mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: rename _rotate_and_offset to _publish
This commit is contained in:
parent
502446d821
commit
155c173ed1
|
@ -1032,8 +1032,8 @@ void AP_InertialSensor::update(void)
|
|||
if (!_hil_mode) {
|
||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
||||
// mark sensors unhealthy and let update() in each backend
|
||||
// mark them healthy via _rotate_and_offset_gyro() and
|
||||
// _rotate_and_offset_accel()
|
||||
// mark them healthy via _publish_gyro() and
|
||||
// _publish_accel()
|
||||
_gyro_healthy[i] = false;
|
||||
_accel_healthy[i] = false;
|
||||
}
|
||||
|
|
|
@ -12,7 +12,7 @@ AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
|
|||
/*
|
||||
rotate gyro vector and add the gyro offset
|
||||
*/
|
||||
void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro)
|
||||
void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro)
|
||||
{
|
||||
_imu._gyro[instance] = gyro;
|
||||
_imu._gyro[instance].rotate(_imu._board_orientation);
|
||||
|
@ -23,7 +23,7 @@ void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const
|
|||
/*
|
||||
rotate accel vector, scale and add the accel offset
|
||||
*/
|
||||
void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel)
|
||||
void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel)
|
||||
{
|
||||
_imu._accel[instance] = accel;
|
||||
_imu._accel[instance].rotate(_imu._board_orientation);
|
||||
|
|
|
@ -36,7 +36,7 @@ public:
|
|||
/*
|
||||
* Update the sensor data. Called by the frontend to transfer
|
||||
* accumulated sensor readings to the frontend structure via the
|
||||
* _rotate_and_offset_gyro() and _rotate_and_offset_accel() functions
|
||||
* _publish_gyro() and _publish_accel() functions
|
||||
*/
|
||||
virtual bool update() = 0;
|
||||
|
||||
|
@ -64,8 +64,11 @@ protected:
|
|||
// rotate gyro vector and offset
|
||||
void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro);
|
||||
|
||||
// rotate accel vector, scale and offset
|
||||
void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel);
|
||||
// rotate gyro vector, offset and publish
|
||||
void _publish_gyro(uint8_t instance, const Vector3f &gyro);
|
||||
|
||||
// rotate accel vector, scale, offset and publish
|
||||
void _publish_accel(uint8_t instance, const Vector3f &accel);
|
||||
|
||||
// set accelerometer error_count
|
||||
void _set_accel_error_count(uint8_t instance, uint32_t error_count);
|
||||
|
|
|
@ -182,11 +182,11 @@ bool AP_InertialSensor_Flymaple::update(void)
|
|||
|
||||
// Adjust for chip scaling to get m/s/s
|
||||
accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
|
||||
_rotate_and_offset_accel(_accel_instance, accel);
|
||||
_publish_accel(_accel_instance, accel);
|
||||
|
||||
// Adjust for chip scaling to get radians/sec
|
||||
gyro *= FLYMAPLE_GYRO_SCALE_R_S;
|
||||
_rotate_and_offset_gyro(_gyro_instance, gyro);
|
||||
_publish_gyro(_gyro_instance, gyro);
|
||||
|
||||
if (_last_filter_hz != _imu.get_filter()) {
|
||||
_set_filter_frequency(_imu.get_filter());
|
||||
|
|
|
@ -251,11 +251,11 @@ bool AP_InertialSensor_L3G4200D::update(void)
|
|||
|
||||
// Adjust for chip scaling to get m/s/s
|
||||
accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
|
||||
_rotate_and_offset_accel(_accel_instance, accel);
|
||||
_publish_accel(_accel_instance, accel);
|
||||
|
||||
// Adjust for chip scaling to get radians/sec
|
||||
gyro *= L3G4200D_GYRO_SCALE_R_S;
|
||||
_rotate_and_offset_gyro(_gyro_instance, gyro);
|
||||
_publish_gyro(_gyro_instance, gyro);
|
||||
|
||||
if (_last_filter_hz != _imu.get_filter()) {
|
||||
_set_filter_frequency(_imu.get_filter());
|
||||
|
|
|
@ -295,10 +295,10 @@ bool AP_InertialSensor_MPU6000::update( void )
|
|||
hal.scheduler->resume_timer_procs();
|
||||
|
||||
gyro *= _gyro_scale / num_samples;
|
||||
_rotate_and_offset_gyro(_gyro_instance, gyro);
|
||||
_publish_gyro(_gyro_instance, gyro);
|
||||
|
||||
accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
|
||||
_rotate_and_offset_accel(_accel_instance, accel);
|
||||
_publish_accel(_accel_instance, accel);
|
||||
|
||||
if (_last_filter_hz != _imu.get_filter()) {
|
||||
if (_spi_sem->take(10)) {
|
||||
|
|
|
@ -1101,10 +1101,10 @@ bool AP_InertialSensor_MPU9150::update(void)
|
|||
hal.scheduler->resume_timer_procs();
|
||||
|
||||
accel *= MPU9150_ACCEL_SCALE_2G;
|
||||
_rotate_and_offset_accel(_accel_instance, accel);
|
||||
_publish_accel(_accel_instance, accel);
|
||||
|
||||
gyro *= MPU9150_GYRO_SCALE_2000;
|
||||
_rotate_and_offset_gyro(_gyro_instance, gyro);
|
||||
_publish_gyro(_gyro_instance, gyro);
|
||||
|
||||
if (_last_filter_hz != _imu.get_filter()) {
|
||||
_set_filter_frequency(_imu.get_filter());
|
||||
|
|
|
@ -289,8 +289,8 @@ bool AP_InertialSensor_MPU9250::update( void )
|
|||
gyro.rotate(ROTATION_ROLL_180);
|
||||
#endif
|
||||
|
||||
_rotate_and_offset_gyro(_gyro_instance, gyro);
|
||||
_rotate_and_offset_accel(_accel_instance, accel);
|
||||
_publish_gyro(_gyro_instance, gyro);
|
||||
_publish_accel(_accel_instance, accel);
|
||||
|
||||
if (_last_filter_hz != _imu.get_filter()) {
|
||||
_set_filter(_imu.get_filter());
|
||||
|
|
|
@ -104,14 +104,14 @@ bool AP_InertialSensor_Oilpan::update()
|
|||
v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x,
|
||||
_sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y,
|
||||
_sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z);
|
||||
_rotate_and_offset_gyro(_gyro_instance, v);
|
||||
_publish_gyro(_gyro_instance, v);
|
||||
|
||||
// copy accels to frontend
|
||||
v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
|
||||
_sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
|
||||
_sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
|
||||
v *= OILPAN_ACCEL_SCALE_1G;
|
||||
_rotate_and_offset_accel(_accel_instance, v);
|
||||
_publish_accel(_accel_instance, v);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
@ -107,20 +107,20 @@ bool AP_InertialSensor_PX4::update(void)
|
|||
|
||||
for (uint8_t k=0; k<_num_accel_instances; k++) {
|
||||
Vector3f accel = _accel_in[k];
|
||||
// calling _rotate_and_offset_accel sets the sensor healthy,
|
||||
// calling _publish_accel sets the sensor healthy,
|
||||
// so we only want to do this if we have new data from it
|
||||
if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
|
||||
_rotate_and_offset_accel(_accel_instance[k], accel);
|
||||
_publish_accel(_accel_instance[k], accel);
|
||||
_last_accel_update_timestamp[k] = _last_accel_timestamp[k];
|
||||
}
|
||||
}
|
||||
|
||||
for (uint8_t k=0; k<_num_gyro_instances; k++) {
|
||||
Vector3f gyro = _gyro_in[k];
|
||||
// calling _rotate_and_offset_accel sets the sensor healthy,
|
||||
// calling _publish_accel sets the sensor healthy,
|
||||
// so we only want to do this if we have new data from it
|
||||
if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) {
|
||||
_rotate_and_offset_gyro(_gyro_instance[k], gyro);
|
||||
_publish_gyro(_gyro_instance[k], gyro);
|
||||
_last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue