mirror of https://github.com/ArduPilot/ardupilot
SITL: add ENABLED defines for simulated I2C devices
Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
This commit is contained in:
parent
17f18f7fc5
commit
70fe304c7e
|
@ -1,5 +1,7 @@
|
|||
#include "SIM_Airspeed_DLVR.h"
|
||||
|
||||
#if AP_SIM_AIRSPEED_DLVR_ENABLED
|
||||
|
||||
#include "SITL.h"
|
||||
|
||||
int SITL::Airspeed_DLVR::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
|
||||
|
@ -68,3 +70,5 @@ void SITL::Airspeed_DLVR::update(const class Aircraft &aircraft)
|
|||
// To Do: Add a sensor board temperature offset parameter
|
||||
temperature = AP_Baro::get_temperatureC_for_alt_amsl(sim_alt);
|
||||
}
|
||||
|
||||
#endif // AP_SIM_AIRSPEED_DLVR_ENABLED
|
||||
|
|
|
@ -1,5 +1,9 @@
|
|||
#pragma once
|
||||
|
||||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_AIRSPEED_DLVR_ENABLED
|
||||
|
||||
#include "SIM_I2CDevice.h"
|
||||
|
||||
namespace SITL {
|
||||
|
@ -26,3 +30,5 @@ private:
|
|||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_AIRSPEED_DLVR_ENABLED
|
||||
|
|
|
@ -1,5 +1,7 @@
|
|||
#include "SIM_BattMonitor_SMBus_Maxell.h"
|
||||
|
||||
#if AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED
|
||||
|
||||
SITL::Maxell::Maxell() :
|
||||
SIM_BattMonitor_SMBus_Generic()
|
||||
{
|
||||
|
@ -15,3 +17,5 @@ SITL::Maxell::Maxell() :
|
|||
|
||||
set_register(SMBusBattGenericDevReg::SERIAL, (uint16_t)37);
|
||||
}
|
||||
|
||||
#endif // AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED
|
||||
|
|
|
@ -1,3 +1,7 @@
|
|||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED
|
||||
|
||||
#include "SIM_BattMonitor_SMBus_Generic.h"
|
||||
|
||||
#include <AP_Common/Bitmask.h>
|
||||
|
@ -25,3 +29,5 @@ public:
|
|||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED
|
||||
|
|
|
@ -1,4 +1,7 @@
|
|||
#include "SIM_BattMonitor_SMBus_Rotoye.h"
|
||||
|
||||
#if AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED
|
||||
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
|
||||
SITL::Rotoye::Rotoye() :
|
||||
|
@ -28,3 +31,5 @@ void SITL::Rotoye::update(const class Aircraft &aircraft)
|
|||
set_register(SMBusBattRotoyeDevReg::TEMP_EXT, int16_t(outside_temp + 100)); // it's a little warmer inside.... (10 degrees here)
|
||||
}
|
||||
}
|
||||
|
||||
#endif // AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED
|
||||
|
|
|
@ -1,3 +1,7 @@
|
|||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED
|
||||
|
||||
#include "SIM_BattMonitor_SMBus_Generic.h"
|
||||
|
||||
#include <AP_Common/Bitmask.h>
|
||||
|
@ -33,3 +37,5 @@ private:
|
|||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED
|
||||
|
|
|
@ -24,6 +24,7 @@
|
|||
#include "SIM_I2C.h"
|
||||
#include "SIM_ToshibaLED.h"
|
||||
#include "SIM_MaxSonarI2CXL.h"
|
||||
#include "SIM_BattMonitor_SMBus_Generic.h"
|
||||
#include "SIM_BattMonitor_SMBus_Maxell.h"
|
||||
#include "SIM_BattMonitor_SMBus_Rotoye.h"
|
||||
#include "SIM_Airspeed_DLVR.h"
|
||||
|
@ -59,20 +60,38 @@ static IgnoredI2CDevice ignored;
|
|||
#if AP_SIM_TOSHIBALED_ENABLED
|
||||
static ToshibaLED toshibaled;
|
||||
#endif
|
||||
#if AP_SIM_MAXSONAR_I2C_XL_ENABLED
|
||||
static MaxSonarI2CXL maxsonari2cxl;
|
||||
static MaxSonarI2CXL maxsonari2cxl_2;
|
||||
#endif
|
||||
#if AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED
|
||||
static Maxell maxell;
|
||||
#endif
|
||||
#if AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED
|
||||
static Rotoye rotoye;
|
||||
#endif
|
||||
static SIM_BattMonitor_SMBus_Generic smbus_generic;
|
||||
#if AP_SIM_AIRSPEED_DLVR_ENABLED
|
||||
static Airspeed_DLVR airspeed_dlvr;
|
||||
#endif
|
||||
#if AP_SIM_TEMPERATURE_TSYS01_ENABLED
|
||||
static TSYS01 tsys01;
|
||||
#endif
|
||||
#if AP_SIM_TSYS03_ENABLED
|
||||
static TSYS03 tsys03;
|
||||
#endif
|
||||
#if AP_SIM_TEMPERATURE_MCP9600_ENABLED
|
||||
static MCP9600 mcp9600;
|
||||
#endif
|
||||
#if AP_SIM_ICM40609_ENABLED
|
||||
static ICM40609 icm40609;
|
||||
#endif
|
||||
#if AP_SIM_MS5525_ENABLED
|
||||
static MS5525 ms5525;
|
||||
#endif
|
||||
#if AP_SIM_MS5611_ENABLED
|
||||
static MS5611 ms5611;
|
||||
#endif
|
||||
#if AP_SIM_LP5562_ENABLED
|
||||
static LP5562 lp5562;
|
||||
#endif
|
||||
|
@ -95,25 +114,43 @@ struct i2c_device_at_address {
|
|||
uint8_t addr;
|
||||
I2CDevice &device;
|
||||
} i2c_devices[] {
|
||||
#if AP_SIM_MAXSONAR_I2C_XL_ENABLED
|
||||
{ 0, 0x70, maxsonari2cxl }, // RNGFNDx_TYPE = 2, RNGFNDx_ADDR = 112
|
||||
#endif
|
||||
#if AP_SIM_TEMPERATURE_MCP9600_ENABLED
|
||||
{ 0, 0x60, mcp9600 }, // 0x60 is low address
|
||||
#endif
|
||||
#if AP_SIM_MAXSONAR_I2C_XL_ENABLED
|
||||
{ 0, 0x71, maxsonari2cxl_2 }, // RNGFNDx_TYPE = 2, RNGFNDx_ADDR = 113
|
||||
#endif
|
||||
#if AP_SIM_ICM40609_ENABLED
|
||||
{ 1, 0x01, icm40609 },
|
||||
#endif
|
||||
#if AP_SIM_TOSHIBALED_ENABLED
|
||||
{ 1, 0x55, toshibaled },
|
||||
#endif
|
||||
{ 1, 0x38, ignored }, // NCP5623
|
||||
{ 1, 0x39, ignored }, // NCP5623C
|
||||
{ 1, 0x40, ignored }, // KellerLD
|
||||
#if AP_SIM_MS5525_ENABLED
|
||||
{ 1, 0x76, ms5525 }, // MS5525: ARSPD_TYPE = 4
|
||||
#endif
|
||||
#if AP_SIM_INA3221_ENABLED
|
||||
{ 1, 0x42, ina3221 },
|
||||
#endif
|
||||
#if AP_SIM_TEMPERATURE_TSYS01_ENABLED
|
||||
{ 1, 0x77, tsys01 },
|
||||
#endif
|
||||
#if AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED
|
||||
{ 1, 0x0B, rotoye }, // Rotoye: BATTx_MONITOR 19, BATTx_I2C_ADDR 13
|
||||
#endif
|
||||
#if AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED
|
||||
{ 2, 0x0B, maxell }, // Maxell: BATTx_MONITOR 16, BATTx_I2C_ADDR 13
|
||||
#endif
|
||||
{ 3, 0x0B, smbus_generic}, // BATTx_MONITOR 7, BATTx_I2C_ADDR 13
|
||||
#if AP_SIM_AIRSPEED_DLVR_ENABLED
|
||||
{ 2, 0x28, airspeed_dlvr }, // ARSPD_TYPE = 7 5inch H2O sensor
|
||||
#endif
|
||||
#if AP_SIM_LP5562_ENABLED
|
||||
{ 2, 0x30, lp5562 }, // LP5562 RGB LED driver
|
||||
#endif
|
||||
|
@ -126,7 +163,9 @@ struct i2c_device_at_address {
|
|||
#if AP_SIM_TSYS03_ENABLED
|
||||
{ 2, 0x40, tsys03 },
|
||||
#endif
|
||||
#if AP_SIM_MS5611_ENABLED
|
||||
{ 2, 0x77, ms5611 }, // MS5611: BARO_PROBE_EXT = 2
|
||||
#endif
|
||||
#if AP_SIM_COMPASS_QMC5883L_ENABLED
|
||||
{ 2, 0x0D, qmc5883l },
|
||||
#endif
|
||||
|
|
|
@ -1,3 +1,7 @@
|
|||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_ICM40609_ENABLED
|
||||
|
||||
#include "SIM_Invensense_v3.h"
|
||||
|
||||
namespace SITL {
|
||||
|
@ -21,3 +25,5 @@ private:
|
|||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_ICM40609_ENABLED
|
||||
|
|
|
@ -1,5 +1,7 @@
|
|||
#include "SIM_MS5525.h"
|
||||
|
||||
#if AP_SIM_MS5525_ENABLED
|
||||
|
||||
#include <SITL/SITL.h>
|
||||
|
||||
using namespace SITL;
|
||||
|
@ -72,3 +74,5 @@ void MS5525::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
|
|||
const uint8_t instance = 0; // TODO: work out which sensor this is
|
||||
P_Pa = AP::sitl()->state.airspeed_raw_pressure[instance] + AP::sitl()->airspeed[instance].offset;
|
||||
}
|
||||
|
||||
#endif // AP_SIM_MS5525_ENABLED
|
||||
|
|
|
@ -1,3 +1,7 @@
|
|||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_MS5525_ENABLED
|
||||
|
||||
#include "SIM_MS5XXX.h"
|
||||
|
||||
#include <AP_Common/Bitmask.h>
|
||||
|
@ -43,3 +47,5 @@ private:
|
|||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_MS5525_ENABLED
|
||||
|
|
|
@ -1,5 +1,7 @@
|
|||
#include "SIM_MS5611.h"
|
||||
|
||||
#if AP_SIM_MS5611_ENABLED
|
||||
|
||||
#include <SITL/SITL.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
@ -121,3 +123,5 @@ void MS5611::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
|
|||
// TO DO add in wind correction by inheritting from AP_Baro_SITL_Generic?
|
||||
// P_Pa += AP_Baro_SITL::wind_pressure_correction(instance);
|
||||
}
|
||||
|
||||
#endif // AP_SIM_MS5611_ENABLED
|
||||
|
|
|
@ -1,3 +1,7 @@
|
|||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_MS5611_ENABLED
|
||||
|
||||
#include "SIM_MS5XXX.h"
|
||||
|
||||
#include <AP_Common/Bitmask.h>
|
||||
|
@ -43,3 +47,5 @@ private:
|
|||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_MS5611_ENABLED
|
||||
|
|
|
@ -1,5 +1,9 @@
|
|||
#pragma once
|
||||
|
||||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_MAXSONAR_I2C_XL_ENABLED
|
||||
|
||||
#include "SIM_I2CDevice.h"
|
||||
|
||||
namespace SITL {
|
||||
|
@ -29,3 +33,5 @@ private:
|
|||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_MAXSONAR_I2C_XL_ENABLED
|
||||
|
|
|
@ -1,5 +1,7 @@
|
|||
#include "SIM_Temperature_MCP9600.h"
|
||||
|
||||
#if AP_SIM_TEMPERATURE_MCP9600_ENABLED
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
@ -65,3 +67,5 @@ int MCP9600::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
|
|||
{
|
||||
return I2CRegisters_ConfigurableLength::rdwr(data);
|
||||
}
|
||||
|
||||
#endif // AP_SIM_TEMPERATURE_MCP9600_ENABLED
|
||||
|
|
|
@ -1,3 +1,7 @@
|
|||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_TEMPERATURE_MCP9600_ENABLED
|
||||
|
||||
#include "SIM_I2CDevice.h"
|
||||
|
||||
/*
|
||||
|
@ -36,3 +40,5 @@ private:
|
|||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_TEMPERATURE_MCP9600_ENABLED
|
||||
|
|
|
@ -1,5 +1,7 @@
|
|||
#include "SIM_Temperature_TSYS01.h"
|
||||
|
||||
#if AP_SIM_TEMPERATURE_TSYS01_ENABLED
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
constexpr const int32_t SITL::TSYS01::_k[5];
|
||||
|
@ -196,3 +198,5 @@ float SITL::TSYS01::get_sim_temperature() const
|
|||
// To Do: Add a sensor board temperature offset parameter
|
||||
return AP_Baro::get_temperatureC_for_alt_amsl(sim_alt) + 25;
|
||||
}
|
||||
|
||||
#endif // AP_SIM_TEMPERATURE_TSYS01_ENABLED
|
||||
|
|
|
@ -1,3 +1,7 @@
|
|||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_TEMPERATURE_TSYS01_ENABLED
|
||||
|
||||
#include "SIM_I2CDevice.h"
|
||||
|
||||
/*
|
||||
|
@ -63,3 +67,5 @@ private:
|
|||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_TEMPERATURE_TSYS01_ENABLED
|
||||
|
|
|
@ -148,6 +148,42 @@
|
|||
#define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_AIRSPEED_DLVR_ENABLED
|
||||
#define AP_SIM_AIRSPEED_DLVR_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED
|
||||
#define AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED
|
||||
#define AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_INA3221_ENABLED
|
||||
#define AP_SIM_INA3221_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_ICM40609_ENABLED
|
||||
#define AP_SIM_ICM40609_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_MS5525_ENABLED
|
||||
#define AP_SIM_MS5525_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_MS5611_ENABLED
|
||||
#define AP_SIM_MS5611_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_MAXSONAR_I2C_XL_ENABLED
|
||||
#define AP_SIM_MAXSONAR_I2C_XL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_TEMPERATURE_MCP9600_ENABLED
|
||||
#define AP_SIM_TEMPERATURE_MCP9600_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_TEMPERATURE_TSYS01_ENABLED
|
||||
#define AP_SIM_TEMPERATURE_TSYS01_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue