Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

replace FIFOSample -> sensor_accel_fifo/sensor_gyro_fifo

Browse Source
This commit is contained in:
Daniel Agar
2020-05-31 21:25:13 -04:00
parent 6b343c1f5e
commit 56c86e77bd
34 changed files with 61 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+9 -20
View File
@@ -107,13 +107,19 @@ void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, floa
Publish(timestamp_sample, x, y, z, clip_count);
}
void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
{
const uint8_t N = sample.samples;
const float dt = sample.dt;
// publish fifo
sample.device_id = _device_id;
sample.scale = _scale;
sample.rotation = _rotation;
sample.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(sample);
{
// trapezoidal integration (equally spaced, scaled by dt later)
const uint8_t N = sample.samples;
const Vector3f integral{
(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)),
@@ -138,23 +144,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
// publish
Publish(sample.timestamp_sample, x, y, z, clip_count);
}
// publish fifo
sensor_accel_fifo_s fifo{};
fifo.device_id = _device_id;
fifo.timestamp_sample = sample.timestamp_sample;
fifo.dt = dt;
fifo.scale = _scale;
fifo.samples = N;
fifo.rotation = _rotation;
memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * N);
fifo.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(fifo);
}
void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3])
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+1 -14
View File
@@ -61,20 +61,7 @@ public:
void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
struct FIFOSample {
hrt_abstime timestamp_sample;
uint8_t samples; // number of samples
float dt; // in microseconds
int16_t x[32];
int16_t y[32];
int16_t z[32];
};
static_assert(sizeof(FIFOSample::x) == sizeof(sensor_accel_fifo_s::x), "FIFOSample.x invalid size");
static_assert(sizeof(FIFOSample::y) == sizeof(sensor_accel_fifo_s::y), "FIFOSample.y invalid size");
static_assert(sizeof(FIFOSample::z) == sizeof(sensor_accel_fifo_s::z), "FIFOSample.z invalid size");
void updateFIFO(const FIFOSample &sample);
void updateFIFO(sensor_accel_fifo_s &sample);
private:
void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3]);
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+9 -20
View File
@@ -88,13 +88,19 @@ void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y,
Publish(timestamp_sample, x, y, z);
}
void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
{
const uint8_t N = sample.samples;
const float dt = sample.dt;
// publish fifo
sample.device_id = _device_id;
sample.scale = _scale;
sample.rotation = _rotation;
sample.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(sample);
{
// trapezoidal integration (equally spaced, scaled by dt later)
const uint8_t N = sample.samples;
const Vector3f integral{
(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)),
@@ -112,23 +118,6 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
// publish
Publish(sample.timestamp_sample, x, y, z);
}
// publish fifo
sensor_gyro_fifo_s fifo{};
fifo.device_id = _device_id;
fifo.timestamp_sample = sample.timestamp_sample;
fifo.dt = dt;
fifo.scale = _scale;
fifo.samples = N;
fifo.rotation = _rotation;
memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * N);
fifo.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(fifo);
}
void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z)
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+1 -14
View File
@@ -60,20 +60,7 @@ public:
void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
struct FIFOSample {
hrt_abstime timestamp_sample;
uint8_t samples; // number of samples
float dt; // in microseconds
int16_t x[32];
int16_t y[32];
int16_t z[32];
};
static_assert(sizeof(FIFOSample::x) == sizeof(sensor_gyro_fifo_s::x), "FIFOSample.x invalid size");
static_assert(sizeof(FIFOSample::y) == sizeof(sensor_gyro_fifo_s::y), "FIFOSample.y invalid size");
static_assert(sizeof(FIFOSample::z) == sizeof(sensor_gyro_fifo_s::z), "FIFOSample.z invalid size");
void updateFIFO(const FIFOSample &sample);
void updateFIFO(sensor_gyro_fifo_s &sample);
private:
void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z);