Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : Simulated i2c buses and devices
17 : */
18 :
19 :
20 : #include "SIM_config.h"
21 : #include <GCS_MAVLink/GCS.h>
22 : #include <SITL/SITL.h>
23 :
24 : #include "SIM_I2C.h"
25 :
26 : #include "SIM_Airspeed_DLVR.h"
27 : #include "SIM_AS5600.h"
28 : #include "SIM_BattMonitor_SMBus_Generic.h"
29 : #include "SIM_BattMonitor_SMBus_Maxell.h"
30 : #include "SIM_BattMonitor_SMBus_Rotoye.h"
31 : #include "SIM_ICM40609.h"
32 : #include "SIM_INA3221.h"
33 : #include "SIM_IS31FL3195.h"
34 : #include "SIM_RF_LightWareI2C_Legacy16Bit.h"
35 : #include "SIM_RF_TOFSenseF_I2C.h"
36 : #include "SIM_RF_LightWare_GRF_I2C.h"
37 : #include "SIM_LM2755.h"
38 : #include "SIM_LP5562.h"
39 : #include "SIM_MaxSonarI2CXL.h"
40 : #include "SIM_MS5525.h"
41 : #include "SIM_MS5611.h"
42 : #include "SIM_QMC5883L.h"
43 : #include "SIM_RF_Benewake_TFMiniPlus.h"
44 : #include "SIM_Temperature_MCP9600.h"
45 : #include "SIM_Temperature_SHT3x.h"
46 : #include "SIM_Temperature_TSYS01.h"
47 : #include "SIM_Temperature_TSYS03.h"
48 : #include "SIM_TeraRangerI2C.h"
49 : #include "SIM_TFS20L.h"
50 : #include "SIM_ToshibaLED.h"
51 :
52 : #include <signal.h>
53 :
54 : using namespace SITL;
55 :
56 : enum class IOCtlType {
57 : RDWR = 0,
58 : };
59 :
60 : class IgnoredI2CDevice : public I2CDevice
61 : {
62 : public:
63 46068 : int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
64 46068 : return -1;
65 : }
66 : };
67 : static IgnoredI2CDevice ignored;
68 :
69 : #if AP_SIM_TOSHIBALED_ENABLED
70 : static ToshibaLED toshibaled;
71 : #endif
72 : #if AP_SIM_RF_LIGHTWAREI2C_LEGACY16BIT_ENABLED
73 : static LightWareI2C_Legacy16Bit lightwarei2c_legacy16bit;
74 : #endif // AP_SIM_RF_LIGHTWAREI2C_LEGACY16BIT_ENABLED
75 : #if AP_SIM_MAXSONAR_I2C_XL_ENABLED
76 : static MaxSonarI2CXL maxsonari2cxl;
77 : static MaxSonarI2CXL maxsonari2cxl_2;
78 : #endif
79 : #if AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED
80 : static Maxell maxell;
81 : #endif
82 : #if AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED
83 : static Rotoye rotoye;
84 : #endif
85 : static SIM_BattMonitor_SMBus_Generic smbus_generic;
86 : #if AP_SIM_AIRSPEED_DLVR_ENABLED
87 : static Airspeed_DLVR airspeed_dlvr;
88 : #endif
89 : #if AP_SIM_TEMPERATURE_SHT3X_ENABLED
90 : static SHT3x sht3x;
91 : #endif // AP_SIM_TEMPERATURE_SHT3X_ENABLED
92 : #if AP_SIM_TEMPERATURE_TSYS01_ENABLED
93 : static TSYS01 tsys01;
94 : #endif
95 : #if AP_SIM_TSYS03_ENABLED
96 : static TSYS03 tsys03;
97 : #endif
98 : #if AP_SIM_TEMPERATURE_MCP9600_ENABLED
99 : static MCP9600 mcp9600;
100 : #endif
101 : #if AP_SIM_ICM40609_ENABLED
102 : static ICM40609 icm40609;
103 : #endif
104 : #if AP_SIM_MS5525_ENABLED
105 : static MS5525 ms5525;
106 : #endif
107 : #if AP_SIM_MS5611_ENABLED
108 : static MS5611 ms5611;
109 : #endif
110 : #if AP_SIM_LP5562_ENABLED
111 : static LP5562 lp5562;
112 : #endif
113 : #if AP_SIM_LM2755_ENABLED
114 : static LM2755 lm2755;
115 : #endif
116 : #if AP_SIM_IS31FL3195_ENABLED
117 : static IS31FL3195 is31fl3195;
118 : #define SIM_IS31FL3195_ADDR 0x54
119 : #endif
120 : #if AP_SIM_COMPASS_QMC5883L_ENABLED
121 : static QMC5883L qmc5883l;
122 : #endif
123 : #if AP_SIM_RF_BENEWAKE_TFMINIPLUS_ENABLED
124 : static Benewake_TFMiniPlus benewake_tfminiplus;
125 : #endif // AP_SIM_RF_BENEWAKE_TFMINIPLUS_ENABLED
126 : #if AP_SIM_INA3221_ENABLED
127 : static INA3221 ina3221;
128 : #endif
129 : #if AP_SIM_TERARANGERI2C_ENABLED
130 : static TeraRangerI2C terarangeri2c;
131 : #endif
132 : #if AP_SIM_AS5600_ENABLED
133 : static AS5600 as5600; // AoA sensor
134 : #endif // AP_SIM_AS5600_ENABLED
135 :
136 : #if AP_SIM_TFS20L_ENABLED
137 : static TFS20L tfs20l; // Benewake TFS20L rangefinder
138 : #endif // AP_SIM_TFS20L_ENABLED
139 :
140 : #if AP_SIM_RF_TOFSENSEF_I2C_ENABLED
141 : static TOFSenseF_I2C tofsensef;
142 : #endif // AP_SIM_RF_TOFSENSEF_I2C_ENABLED
143 :
144 : #if AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED
145 : static LightWareGRF_I2C lightware_grf_i2c;
146 : #endif // AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED
147 :
148 : struct i2c_device_at_address {
149 : uint8_t bus;
150 : uint8_t addr;
151 : I2CDevice &device;
152 : } i2c_devices[] {
153 : #if AP_SIM_TERARANGERI2C_ENABLED
154 : { 0, 0x31, terarangeri2c }, // RNGFNDx_TYPE = 14, RNGFNDx_ADDR = 49
155 : #endif // AP_SIM_TERARANGERI2C_ENABLED
156 : #if AP_SIM_RF_LIGHTWAREI2C_LEGACY16BIT_ENABLED
157 : { 0, 0x66, lightwarei2c_legacy16bit }, // RNGFNDx_TYPE = 7, RNGFNDx_ADDR = 0x66
158 : #endif // AP_SIM_RF_LIGHTWAREI2C_LEGACY16BIT_ENABLED
159 : #if AP_SIM_MAXSONAR_I2C_XL_ENABLED
160 : { 0, 0x70, maxsonari2cxl }, // RNGFNDx_TYPE = 2, RNGFNDx_ADDR = 112
161 : #endif
162 : #if AP_SIM_TEMPERATURE_MCP9600_ENABLED
163 : { 0, 0x60, mcp9600 }, // 0x60 is low address
164 : #endif
165 : #if AP_SIM_MAXSONAR_I2C_XL_ENABLED
166 : { 0, 0x71, maxsonari2cxl_2 }, // RNGFNDx_TYPE = 2, RNGFNDx_ADDR = 113
167 : #endif
168 : #if AP_SIM_ICM40609_ENABLED
169 : { 1, 0x01, icm40609 },
170 : #endif
171 : #if AP_SIM_TEMPERATURE_SHT3X_ENABLED
172 : { 1, 0x44, sht3x },
173 : #endif
174 : #if AP_SIM_TOSHIBALED_ENABLED
175 : { 1, 0x55, toshibaled },
176 : #endif
177 : { 1, 0x38, ignored }, // NCP5623
178 : { 1, 0x39, ignored }, // NCP5623C
179 : #if AP_SIM_AS5600_ENABLED
180 : { 1, 0x36, as5600 },
181 : #endif
182 : { 1, 0x40, ignored }, // KellerLD
183 : #if AP_SIM_MS5525_ENABLED
184 : { 1, 0x76, ms5525 }, // MS5525: ARSPD_TYPE = 4
185 : #endif
186 : #if AP_SIM_INA3221_ENABLED
187 : { 1, 0x42, ina3221 },
188 : #endif
189 : #if AP_SIM_TEMPERATURE_TSYS01_ENABLED
190 : { 1, 0x77, tsys01 },
191 : #endif
192 : #if AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED
193 : { 1, 0x0B, rotoye }, // Rotoye: BATTx_MONITOR 19, BATTx_I2C_ADDR 13
194 : #endif
195 : #if AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED
196 : { 2, 0x0B, maxell }, // Maxell: BATTx_MONITOR 16, BATTx_I2C_ADDR 13
197 : #endif
198 : { 3, 0x0B, smbus_generic}, // BATTx_MONITOR 7, BATTx_I2C_ADDR 13
199 : #if AP_SIM_AIRSPEED_DLVR_ENABLED
200 : { 2, 0x28, airspeed_dlvr }, // ARSPD_TYPE = 7 5inch H2O sensor
201 : #endif
202 : #if AP_SIM_LP5562_ENABLED
203 : { 2, 0x30, lp5562 }, // LP5562 RGB LED driver
204 : #endif
205 : #if AP_SIM_LM2755_ENABLED
206 : { 2, 0x67, lm2755 }, // LM2755 RGB LED driver
207 : #endif
208 : #if AP_SIM_IS31FL3195_ENABLED
209 : { 2, SIM_IS31FL3195_ADDR, is31fl3195 }, // IS31FL3195 RGB LED driver; see page 9
210 : #endif
211 : #if AP_SIM_RF_BENEWAKE_TFMINIPLUS_ENABLED
212 : { 2, 0x09, benewake_tfminiplus }, // TFMiniPlus rfnd, non-default-address
213 : #endif // AP_SIM_RF_BENEWAKE_TFMINIPLUS_ENABLED
214 : #if AP_SIM_TSYS03_ENABLED
215 : { 2, 0x40, tsys03 },
216 : #endif
217 : #if AP_SIM_MS5611_ENABLED
218 : { 2, 0x77, ms5611 }, // MS5611: BARO_PROBE_EXT = 2
219 : #endif
220 : #if AP_SIM_COMPASS_QMC5883L_ENABLED
221 : { 2, 0x0D, qmc5883l },
222 : #endif
223 : #if AP_SIM_TFS20L_ENABLED
224 : { 0, 0x10, tfs20l }, // RNGFNDx_TYPE = 46, RNGFNDx_ADDR = 0x10
225 : #endif // AP_SIM_TFS20L_ENABLED
226 : #if AP_SIM_RF_TOFSENSEF_I2C_ENABLED
227 : { 0, 0x08, tofsensef }, // RNGFNDx_TYPE = 40, RNGFNDx_ADDR = 0x08
228 : #endif // AP_SIM_RF_TOFSENSEF_I2C_ENABLED
229 : #if AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED
230 : { 2, 0x66, lightware_grf_i2c }, // RNGFNDx_TYPE = 48, RNGFNDx_ADDR = 0x66
231 : #endif // AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED
232 : };
233 :
234 182068647 : void I2C::init()
235 : {
236 182068647 : if (initialised) {
237 182066553 : return;
238 : }
239 2094 : initialised = true;
240 :
241 62820 : for (auto &i : i2c_devices) {
242 60726 : i.device.init();
243 : }
244 :
245 : #if AP_SIM_IS31FL3195_ENABLED
246 : // IS31FL3195 needs to know its own address:
247 2094 : is31fl3195.set_product_id(SIM_IS31FL3195_ADDR);
248 : #endif
249 :
250 : // sanity check the i2c_devices structure to ensure we don't have
251 : // two devices at the same address on the same bus:
252 60726 : for (uint8_t i=0; i<ARRAY_SIZE(i2c_devices)-1; i++) {
253 58632 : const auto &dev_i = i2c_devices[i];
254 908796 : for (uint8_t j=i+1; j<ARRAY_SIZE(i2c_devices); j++) {
255 850164 : const auto &dev_j = i2c_devices[j];
256 850164 : if (dev_i.bus == dev_j.bus &&
257 253374 : dev_i.addr == dev_j.addr) {
258 0 : AP_HAL::panic("Two devices at the same address on the same bus");
259 : }
260 : }
261 : }
262 : }
263 :
264 178759942 : void I2C::update(const class Aircraft &aircraft)
265 : {
266 178759942 : init(); // lazily set up the device table on first use
267 :
268 5362798260 : for (auto daa : i2c_devices) {
269 5184038318 : daa.device.update(aircraft);
270 : }
271 178759942 : }
272 :
273 3308705 : int I2C::ioctl_rdwr(i2c_rdwr_ioctl_data *data)
274 : {
275 3308705 : const uint8_t addr = data->msgs[0].addr;
276 3308705 : const uint8_t bus = data->msgs[0].bus;
277 65994736 : for (auto &dev_at_address : i2c_devices) {
278 65603073 : if (dev_at_address.addr != addr) {
279 62236931 : continue;
280 : }
281 3366142 : if (dev_at_address.bus != bus) {
282 449100 : continue;
283 : }
284 2917042 : return dev_at_address.device.rdwr(data);
285 : }
286 : // ::fprintf(stderr, "Unhandled i2c message: bus=%u addr=0x%02x flags=%u len=%u\n", msg.bus, msg.addr, msg.flags, msg.len);
287 391663 : return -1; // ?!
288 : }
289 :
290 3308705 : int I2C::ioctl(uint8_t ioctl_type, void *data)
291 : {
292 3308705 : init(); // lazily set up the device table on first use
293 :
294 3308705 : switch ((IOCtlType) ioctl_type) {
295 3308705 : case IOCtlType::RDWR:
296 3308705 : return ioctl_rdwr((i2c_rdwr_ioctl_data*)data);
297 : }
298 0 : return -1;
299 : }
|
