Skip to content

Commit

Permalink
WIP
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Apr 27, 2020
1 parent dd67727 commit 2f63346
Show file tree
Hide file tree
Showing 27 changed files with 494 additions and 1,079 deletions.
53 changes: 11 additions & 42 deletions msg/sensor_correction.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,52 +6,21 @@ uint64 timestamp # time since system start (microseconds)

# Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset
# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
# corrections for uORB instance 0
float32[3] gyro_offset_0 # gyro XYZ offsets in the sensor frame in rad/s
float32[3] gyro_scale_0 # gyro XYZ scale factors in the sensor frame

# corrections for uORB instance 1
float32[3] gyro_offset_1 # gyro XYZ offsets in the sensor frame in rad/s
float32[3] gyro_scale_1 # gyro XYZ scale factors in the sensor frame

# corrections for uORB instance 2
float32[3] gyro_offset_2 # gyro XYZ offsets in the sensor frame in rad/s
float32[3] gyro_scale_2 # gyro XYZ scale factors in the sensor frame
uint32[3] gyro_device_ids
float32[3] gyro_offset_0 # gyro 0 XYZ offsets in the sensor frame in rad/s
float32[3] gyro_offset_1 # gyro 1 XYZ offsets in the sensor frame in rad/s
float32[3] gyro_offset_2 # gyro 2 XYZ offsets in the sensor frame in rad/s

# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset
# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
# corrections for uORB instance 0
float32[3] accel_offset_0 # accelerometer XYZ offsets in the sensor frame in m/s/s
float32[3] accel_scale_0 # accelerometer XYZ scale factors in the sensor frame

# corrections for uORB instance 1
float32[3] accel_offset_1 # accelerometer XYZ offsets in the sensor frame in m/s/s
float32[3] accel_scale_1 # accelerometer XYZ scale factors in the sensor frame

# corrections for uORB instance 2
float32[3] accel_offset_2 # accelerometer XYZ offsets in the sensor frame in m/s/s
float32[3] accel_scale_2 # accelerometer XYZ scale factors in the sensor frame
uint32[3] accel_device_ids
float32[3] accel_offset_0 # accelerometer 0 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_offset_1 # accelerometer 1 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_offset_2 # accelerometer 2 XYZ offsets in the sensor frame in m/s/s

# Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset
# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
# corrections for uORB instance 0
float32 baro_offset_0 # barometric pressure offsets in the sensor frame in m/s/s
float32 baro_scale_0 # barometric pressure scale factors in the sensor frame

# corrections for uORB instance 1
float32 baro_offset_1 # barometric pressure offsets in the sensor frame in m/s/s
float32 baro_scale_1 # barometric pressure scale factors in the sensor frame

# corrections for uORB instance 2
float32 baro_offset_2 # barometric pressure offsets in the sensor frame in m/s/s
float32 baro_scale_2 # barometric pressure scale factors in the sensor frame

# Mapping from uORB index to parameter index for each sensor type. For example accel_mapping[1] contains the
# compensation parameter system index for the sensor_accel uORB index 1 data.
uint8[3] gyro_mapping
uint8[3] accel_mapping
uint8[3] baro_mapping

uint32[3] gyro_device_ids
uint32[3] accel_device_ids
uint32[3] baro_device_ids
float32 baro_offset_0 # barometric pressure 0 offsets in the sensor frame in m/s/s
float32 baro_offset_1 # barometric pressure 1 offsets in the sensor frame in m/s/s
float32 baro_offset_2 # barometric pressure 2 offsets in the sensor frame in m/s/s
105 changes: 50 additions & 55 deletions src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,14 +170,35 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
const uint8_t N = sample.samples;
const float dt = sample.dt;

// reset integrator if previous sample was too long ago
if ((sample.timestamp_sample > _timestamp_sample_prev)
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {

ResetIntegrator();
}

_timestamp_sample_prev = sample.timestamp_sample;

// trapezoidal integration (equally spaced, scaled by dt later)
_integrator_samples += 1;
_integrator_fifo_samples += N;
const Vector3f integration_raw{
(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1))
};

_integration_raw += integration_raw;
_last_sample[0] = sample.x[N - 1];
_last_sample[1] = sample.y[N - 1];
_last_sample[2] = sample.z[N - 1];

// publish raw data immediately
{
// average
float x = (float)sum(sample.x, N) / (float)N;
float y = (float)sum(sample.y, N) / (float)N;
float z = (float)sum(sample.z, N) / (float)N;

// Apply rotation (before scaling)
float x = integration_raw(0) / (float)N;
float y = integration_raw(1) / (float)N;
float z = integration_raw(2) / (float)N;
rotate_3f(_rotation, x, y, z);

// Apply range scale
Expand Down Expand Up @@ -211,63 +232,39 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
_integrator_clipping(2) += clip_count_z;

// integrated data (INS)
{
// reset integrator if previous sample was too long ago
if ((sample.timestamp_sample > _timestamp_sample_prev)
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {

ResetIntegrator();
}

// integrate
_integrator_samples += 1;
_integrator_fifo_samples += N;

// trapezoidal integration (equally spaced, scaled by dt later)
_integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1));
_integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1));
_integration_raw(2) += (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1));
_last_sample[0] = sample.x[N - 1];
_last_sample[1] = sample.y[N - 1];
_last_sample[2] = sample.z[N - 1];


if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {

// Apply rotation (before scaling)
rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2));

// Apply calibration and scale to seconds
const Vector3f delta_velocity{_integration_raw *_scale * 1e-6f * dt};
// Apply rotation (before scaling)
rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2));

// fill sensor_accel_integrated and publish
sensor_accel_integrated_s report;
// Apply calibration and scale to seconds
const Vector3f delta_velocity{_integration_raw *_scale * 1e-6f * dt};

report.timestamp_sample = sample.timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
delta_velocity.copyTo(report.delta_velocity);
report.dt = _integrator_fifo_samples * dt; // time span in microseconds
report.samples = _integrator_fifo_samples;
// fill sensor_accel_integrated and publish
sensor_accel_integrated_s report;

rotate_3f(_rotation, _integrator_clipping(0), _integrator_clipping(1), _integrator_clipping(2));
const Vector3f clipping{_integrator_clipping};
report.timestamp_sample = sample.timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
delta_velocity.copyTo(report.delta_velocity);
report.dt = _integrator_fifo_samples * dt; // time span in microseconds
report.samples = _integrator_fifo_samples;

for (int i = 0; i < 3; i++) {
report.clip_counter[i] = fabsf(roundf(clipping(i)));
}
rotate_3f(_rotation, _integrator_clipping(0), _integrator_clipping(1), _integrator_clipping(2));
const Vector3f clipping{_integrator_clipping};

report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);
for (int i = 0; i < 3; i++) {
report.clip_counter[i] = fabsf(roundf(clipping(i)));
}

// update vibration metrics
UpdateVibrationMetrics(delta_velocity);
report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);

// reset integrator
ResetIntegrator();
}
// update vibration metrics
UpdateVibrationMetrics(delta_velocity);

_timestamp_sample_prev = sample.timestamp_sample;
// reset integrator
ResetIntegrator();
}

// publish sensor fifo
Expand Down Expand Up @@ -319,8 +316,6 @@ void PX4Accelerometer::ResetIntegrator()
_integrator_fifo_samples = 0;
_integration_raw.zero();
_integrator_clipping.zero();

_timestamp_sample_prev = 0;
}

void PX4Accelerometer::UpdateClipLimit()
Expand Down
105 changes: 50 additions & 55 deletions src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,14 +172,35 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
const uint8_t N = sample.samples;
const float dt = sample.dt;

// reset integrator if previous sample was too long ago
if ((sample.timestamp_sample > _timestamp_sample_prev)
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {

ResetIntegrator();
}

_timestamp_sample_prev = sample.timestamp_sample;

// trapezoidal integration (equally spaced, scaled by dt later)
_integrator_samples += 1;
_integrator_fifo_samples += N;
const Vector3f integration_raw{
(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1))
};

_integration_raw += integration_raw;
_last_sample[0] = sample.x[N - 1];
_last_sample[1] = sample.y[N - 1];
_last_sample[2] = sample.z[N - 1];

// publish raw data immediately
{
// average
float x = (float)sum(sample.x, N) / (float)N;
float y = (float)sum(sample.y, N) / (float)N;
float z = (float)sum(sample.z, N) / (float)N;

// Apply rotation (before scaling)
float x = integration_raw(0) / (float)N;
float y = integration_raw(1) / (float)N;
float z = integration_raw(2) / (float)N;
rotate_3f(_rotation, x, y, z);

// Apply range scale
Expand Down Expand Up @@ -213,63 +234,39 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
_integrator_clipping(2) += clip_count_z;

// integrated data (INS)
{
// reset integrator if previous sample was too long ago
if ((sample.timestamp_sample > _timestamp_sample_prev)
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {

ResetIntegrator();
}

// integrate
_integrator_samples += 1;
_integrator_fifo_samples += N;

// trapezoidal integration (equally spaced, scaled by dt later)
_integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1));
_integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1));
_integration_raw(2) += (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1));
_last_sample[0] = sample.x[N - 1];
_last_sample[1] = sample.y[N - 1];
_last_sample[2] = sample.z[N - 1];


if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {

// Apply rotation (before scaling)
rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2));

// Apply calibration and scale to seconds
const Vector3f delta_angle{_integration_raw *_scale * 1e-6f * dt};
// Apply rotation (before scaling)
rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2));

// fill sensor_gyro_integrated and publish
sensor_gyro_integrated_s report;
// Apply calibration and scale to seconds
const Vector3f delta_angle{_integration_raw *_scale * 1e-6f * dt};

report.timestamp_sample = sample.timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
delta_angle.copyTo(report.delta_angle);
report.dt = _integrator_fifo_samples * dt; // time span in microseconds
report.samples = _integrator_fifo_samples;
// fill sensor_gyro_integrated and publish
sensor_gyro_integrated_s report;

rotate_3f(_rotation, _integrator_clipping(0), _integrator_clipping(1), _integrator_clipping(2));
const Vector3f clipping{_integrator_clipping};
report.timestamp_sample = sample.timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
delta_angle.copyTo(report.delta_angle);
report.dt = _integrator_fifo_samples * dt; // time span in microseconds
report.samples = _integrator_fifo_samples;

for (int i = 0; i < 3; i++) {
report.clip_counter[i] = fabsf(roundf(clipping(i)));
}
rotate_3f(_rotation, _integrator_clipping(0), _integrator_clipping(1), _integrator_clipping(2));
const Vector3f clipping{_integrator_clipping};

report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);
for (int i = 0; i < 3; i++) {
report.clip_counter[i] = fabsf(roundf(clipping(i)));
}

// update vibration metrics
UpdateVibrationMetrics(delta_angle);
report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);

// reset integrator
ResetIntegrator();
}
// update vibration metrics
UpdateVibrationMetrics(delta_angle);

_timestamp_sample_prev = sample.timestamp_sample;
// reset integrator
ResetIntegrator();
}

// publish sensor fifo
Expand Down Expand Up @@ -322,8 +319,6 @@ void PX4Gyroscope::ResetIntegrator()
_integrator_fifo_samples = 0;
_integration_raw.zero();
_integrator_clipping.zero();

_timestamp_sample_prev = 0;
}

void PX4Gyroscope::UpdateClipLimit()
Expand Down
Loading

0 comments on commit 2f63346

Please sign in to comment.