Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 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_blocked[ACCEL_COUNT_MAX] {};
bool _accel_stuck[ACCEL_COUNT_MAX] {}; bool _accel_stuck[ACCEL_COUNT_MAX] {};
sensor_accel_fifo_s _last_accel_fifo{};
matrix::Vector3f _last_accel[GYRO_COUNT_MAX] {}; matrix::Vector3f _last_accel[GYRO_COUNT_MAX] {};
bool _gyro_blocked[GYRO_COUNT_MAX] {}; bool _gyro_blocked[GYRO_COUNT_MAX] {};
bool _gyro_stuck[GYRO_COUNT_MAX] {}; bool _gyro_stuck[GYRO_COUNT_MAX] {};
sensor_gyro_fifo_s _last_gyro_fifo{};
matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {}; matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {};
bool _baro_blocked{false}; bool _baro_blocked{false};
src/modules/simulator/simulator_mavlink.cpp
+64 -12
View File
@@ -207,13 +207,39 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
// accel // accel
if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) { if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
for (int i = 0; i < ACCEL_COUNT_MAX; i++) { for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
if (_accel_stuck[i]) { if (i == 0) {
_px4_accel[i].update(time, _last_accel[i](0), _last_accel[i](1), _last_accel[i](2)); // 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;
} else if (!_accel_blocked[i]) { _px4_accel[i].set_scale(ACCEL_FIFO_SCALE);
_px4_accel[i].set_temperature(_sensors_temperature); _px4_accel[i].set_range(ACCEL_FIFO_RANGE);
_px4_accel[i].update(time, sensors.xacc, sensors.yacc, sensors.zacc);
_last_accel[i] = matrix::Vector3f{sensors.xacc, sensors.yacc, sensors.zacc}; 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));
} else if (!_accel_blocked[i]) {
_px4_accel[i].set_temperature(_sensors_temperature);
_px4_accel[i].update(time, sensors.xacc, sensors.yacc, sensors.zacc);
_last_accel[i] = matrix::Vector3f{sensors.xacc, sensors.yacc, sensors.zacc};
}
} }
} }
} }
@@ -221,13 +247,39 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
// gyro // gyro
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) { if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) {
for (int i = 0; i < GYRO_COUNT_MAX; i++) { for (int i = 0; i < GYRO_COUNT_MAX; i++) {
if (_gyro_stuck[i]) { if (i == 0) {
_px4_gyro[i].update(time, _last_gyro[i](0), _last_gyro[i](1), _last_gyro[i](2)); // 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);
} else if (!_gyro_blocked[i]) { _px4_gyro[i].set_scale(GYRO_FIFO_SCALE);
_px4_gyro[i].set_temperature(_sensors_temperature); _px4_gyro[i].set_range(GYRO_FIFO_RANGE);
_px4_gyro[i].update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
_last_gyro[i] = matrix::Vector3f{sensors.xgyro, sensors.ygyro, sensors.zgyro}; 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));
} else if (!_gyro_blocked[i]) {
_px4_gyro[i].set_temperature(_sensors_temperature);
_px4_gyro[i].update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
_last_gyro[i] = matrix::Vector3f{sensors.xgyro, sensors.ygyro, sensors.zgyro};
}
} }
} }
} }