diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 8ee3752b22..c466d06990 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -122,8 +122,11 @@ then param set BAT_N_CELLS 4 param set CAL_ACC0_ID 1311244 + param set CAL_ACC1_ID 1311500 param set CAL_GYRO0_ID 1311244 + param set CAL_GYRO1_ID 1311500 param set CAL_MAG0_ID 197388 + param set CAL_MAG1_ID 197644 param set CBRK_AIRSPD_CHK 0 param set CBRK_SUPPLY_CHK 894281 diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index d0b450c726..9d3ffb4014 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -154,10 +154,17 @@ private: static Simulator *_instance; // simulated sensor instances - PX4Accelerometer _px4_accel{1311244}; // 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 - 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 + PX4Accelerometer _px4_accel_0{1311244, ROTATION_NONE}; // 1311244: 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 + + 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_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 271ff78971..316f7a6c93 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -219,31 +219,42 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor if (PX4_ISFINITE(sensors.temperature)) { temperature = sensors.temperature; - _px4_accel.set_temperature(temperature); - _px4_baro.set_temperature(temperature); - _px4_gyro.set_temperature(temperature); - _px4_mag.set_temperature(temperature); + _px4_accel_0.set_temperature(temperature); + _px4_accel_1.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 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 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 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 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