zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

simulator: publish 2 instances of accel/baro/gyro/mag

sbg
Daniel Agar 4 years ago committed by GitHub
parent
668c16da83
commit
5017a7e33a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 33 additions and 12 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      ROMFS/px4fmu_common/init.d-posix/rcS
  2. 15
      src/modules/simulator/simulator.h
  3. 27
      src/modules/simulator/simulator_mavlink.cpp

3
ROMFS/px4fmu_common/init.d-posix/rcS
Unescape View File

@ -122,8 +122,11 @@ then
param set BAT_N_CELLS 4 param set BAT_N_CELLS 4
param set CAL_ACC0_ID 1311244 param set CAL_ACC0_ID 1311244
param set CAL_ACC1_ID 1311500
param set CAL_GYRO0_ID 1311244 param set CAL_GYRO0_ID 1311244
param set CAL_GYRO1_ID 1311500
param set CAL_MAG0_ID 197388 param set CAL_MAG0_ID 197388
param set CAL_MAG1_ID 197644
param set CBRK_AIRSPD_CHK 0 param set CBRK_AIRSPD_CHK 0
param set CBRK_SUPPLY_CHK 894281 param set CBRK_SUPPLY_CHK 894281

15
src/modules/simulator/simulator.h
Unescape View File

@ -154,10 +154,17 @@ private:
static Simulator *_instance; static Simulator *_instance;
// simulated sensor instances // simulated sensor instances
PX4Accelerometer _px4_accel{1311244}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION PX4Accelerometer _px4_accel_0{1311244, ROTATION_NONE}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{1311244}; // 2294028: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION PX4Accelerometer _px4_accel_1{1311500, ROTATION_NONE}; // 1311500: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION PX4Gyroscope _px4_gyro_0{1311244, ROTATION_NONE}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro_1{1311500, ROTATION_NONE}; // 1311500: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag_0{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag_1{197644, ROTATION_NONE}; // 197644: DRV_MAG_DEVTYPE_MAGSIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro_0{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
PX4Barometer _px4_baro_1{6620428}; // 6620428: DRV_BARO_DEVTYPE_BAROSIM, BUS: 2, ADDR: 4, TYPE: SIMULATION
perf_counter_t _perf_sim_delay{perf_alloc(PC_ELAPSED, MODULE_NAME": network delay")}; perf_counter_t _perf_sim_delay{perf_alloc(PC_ELAPSED, MODULE_NAME": network delay")};
perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")}; perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")};

27
src/modules/simulator/simulator_mavlink.cpp
Unescape View File

@ -219,31 +219,42 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
if (PX4_ISFINITE(sensors.temperature)) { if (PX4_ISFINITE(sensors.temperature)) {
temperature = sensors.temperature; temperature = sensors.temperature;
_px4_accel.set_temperature(temperature); _px4_accel_0.set_temperature(temperature);
_px4_baro.set_temperature(temperature); _px4_accel_1.set_temperature(temperature);
_px4_gyro.set_temperature(temperature);
_px4_mag.set_temperature(temperature); _px4_baro_0.set_temperature(temperature);
_px4_baro_1.set_temperature(temperature);
_px4_gyro_0.set_temperature(temperature);
_px4_gyro_1.set_temperature(temperature);
_px4_mag_0.set_temperature(temperature);
_px4_mag_1.set_temperature(temperature);
} }
} }
// accel // accel
if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL && !_param_sim_accel_block.get()) { if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL && !_param_sim_accel_block.get()) {
_px4_accel.update(time, sensors.xacc, sensors.yacc, sensors.zacc); _px4_accel_0.update(time, sensors.xacc, sensors.yacc, sensors.zacc);
_px4_accel_1.update(time, sensors.xacc, sensors.yacc, sensors.zacc);
} }
// gyro // gyro
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_param_sim_gyro_block.get()) { if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_param_sim_gyro_block.get()) {
_px4_gyro.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro); _px4_gyro_0.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
_px4_gyro_1.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
} }
// magnetometer // magnetometer
if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_param_sim_mag_block.get()) { if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_param_sim_mag_block.get()) {
_px4_mag.update(time, sensors.xmag, sensors.ymag, sensors.zmag); _px4_mag_0.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
_px4_mag_1.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
} }
// baro // baro
if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_param_sim_baro_block.get()) { if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_param_sim_baro_block.get()) {
_px4_baro.update(time, sensors.abs_pressure); _px4_baro_0.update(time, sensors.abs_pressure);
_px4_baro_1.update(time, sensors.abs_pressure);
} }
// differential pressure // differential pressure

Write Preview
Loading…
Cancel
Save