Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

simulator: make first accel/gyro simulated FIFO

Browse Source
This commit is contained in:
Daniel Agar
2021-02-25 11:39:55 -05:00
parent 0e796fc17b
commit 9d47f7ecda
2 changed files with 66 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator.h
+2
View File
@@ -274,10 +274,12 @@ private:
bool _accel_blocked[ACCEL_COUNT_MAX] {};
bool _accel_stuck[ACCEL_COUNT_MAX] {};
sensor_accel_fifo_s _last_accel_fifo{};
matrix::Vector3f _last_accel[GYRO_COUNT_MAX] {};
bool _gyro_blocked[GYRO_COUNT_MAX] {};
bool _gyro_stuck[GYRO_COUNT_MAX] {};
sensor_gyro_fifo_s _last_gyro_fifo{};
matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {};
bool _baro_blocked{false};
src/modules/simulator/simulator_mavlink.cpp
+52
View File
@@ -207,6 +207,31 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
// accel
if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
if (i == 0) {
// accel 0 is simulated FIFO
static constexpr float ACCEL_FIFO_SCALE = CONSTANTS_ONE_G / 2048.f;
static constexpr float ACCEL_FIFO_RANGE = 16.f * CONSTANTS_ONE_G;
_px4_accel[i].set_scale(ACCEL_FIFO_SCALE);
_px4_accel[i].set_range(ACCEL_FIFO_RANGE);
if (_accel_stuck[i]) {
_px4_accel[i].updateFIFO(_last_accel_fifo);
} else if (!_accel_blocked[i]) {
_px4_accel[i].set_temperature(_sensors_temperature);
_last_accel_fifo.samples = 1;
_last_accel_fifo.dt = time - _last_accel_fifo.timestamp_sample;
_last_accel_fifo.timestamp_sample = time;
_last_accel_fifo.x[0] = sensors.xacc / ACCEL_FIFO_SCALE;
_last_accel_fifo.y[0] = sensors.yacc / ACCEL_FIFO_SCALE;
_last_accel_fifo.z[0] = sensors.zacc / ACCEL_FIFO_SCALE;
_px4_accel[i].updateFIFO(_last_accel_fifo);
}
} else {
if (_accel_stuck[i]) {
_px4_accel[i].update(time, _last_accel[i](0), _last_accel[i](1), _last_accel[i](2));
@@ -217,10 +242,36 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
}
}
}
}
// gyro
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) {
for (int i = 0; i < GYRO_COUNT_MAX; i++) {
if (i == 0) {
// gyro 0 is simulated FIFO
static constexpr float GYRO_FIFO_SCALE = math::radians(2000.f / 32768.f);
static constexpr float GYRO_FIFO_RANGE = math::radians(2000.f);
_px4_gyro[i].set_scale(GYRO_FIFO_SCALE);
_px4_gyro[i].set_range(GYRO_FIFO_RANGE);
if (_gyro_stuck[i]) {
_px4_gyro[i].updateFIFO(_last_gyro_fifo);
} else if (!_gyro_blocked[i]) {
_px4_gyro[i].set_temperature(_sensors_temperature);
_last_gyro_fifo.samples = 1;
_last_gyro_fifo.dt = time - _last_gyro_fifo.timestamp_sample;
_last_gyro_fifo.timestamp_sample = time;
_last_gyro_fifo.x[0] = sensors.xgyro / GYRO_FIFO_SCALE;
_last_gyro_fifo.y[0] = sensors.ygyro / GYRO_FIFO_SCALE;
_last_gyro_fifo.z[0] = sensors.zgyro / GYRO_FIFO_SCALE;
_px4_gyro[i].updateFIFO(_last_gyro_fifo);
}
} else {
if (_gyro_stuck[i]) {
_px4_gyro[i].update(time, _last_gyro[i](0), _last_gyro[i](1), _last_gyro[i](2));
@@ -231,6 +282,7 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
}
}
}
}
// magnetometer
if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_mag_blocked) {