Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
4 :
5 : #include <assert.h>
6 : #include <errno.h>
7 : #include <signal.h>
8 : #include <stdio.h>
9 : #include <sys/types.h>
10 : #include <sys/stat.h>
11 : #include <fcntl.h>
12 :
13 : #include "AP_HAL_SITL.h"
14 : #include "AP_HAL_SITL_Namespace.h"
15 : #include "HAL_SITL_Class.h"
16 : #include "Scheduler.h"
17 : #include "AnalogIn.h"
18 : #include "UARTDriver.h"
19 : #include "I2CDevice.h"
20 : #include "Storage.h"
21 : #include "RCInput.h"
22 : #include "RCOutput.h"
23 : #include "GPIO.h"
24 : #include "SITL_State.h"
25 : #include "Util.h"
26 : #include "DSP.h"
27 : #include "CANSocketIface.h"
28 : #include "SPIDevice.h"
29 :
30 : #include <AP_BoardConfig/AP_BoardConfig.h>
31 : #include <AP_HAL_Empty/AP_HAL_Empty.h>
32 : #include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
33 : #include <AP_InternalError/AP_InternalError.h>
34 : #include <AP_Logger/AP_Logger.h>
35 : #include <AP_RCProtocol/AP_RCProtocol_config.h>
36 : #include <AP_HAL/SIMState.h>
37 : #include <AP_HAL/utility/Socket_native.h>
38 :
39 : using namespace HALSITL;
40 :
41 : HAL_SITL& hal_sitl = (HAL_SITL&)AP_HAL::get_HAL_mutable();
42 :
43 : static Storage sitlStorage;
44 : static SITL_State sitlState;
45 : static Scheduler sitlScheduler(&sitlState);
46 : #if AP_RCPROTOCOL_ENABLED
47 : static RCInput sitlRCInput;
48 : #else
49 : static Empty::RCInput sitlRCInput;
50 : #endif
51 : static RCOutput sitlRCOutput(&sitlState);
52 : static GPIO sitlGPIO(&sitlState);
53 : static AnalogIn sitlAnalogIn(&sitlState);
54 :
55 : #if AP_SIM_ENABLED
56 : static AP_HAL::SIMState xsimstate;
57 : #endif
58 :
59 : #if HAL_WITH_DSP
60 : static DSP dspDriver;
61 : #endif
62 :
63 :
64 : // use the Empty HAL for hardware we don't emulate
65 : static Empty::OpticalFlow emptyOpticalFlow;
66 : static Empty::Flash emptyFlash;
67 :
68 : static UARTDriver sitlSerial0Driver(0, &sitlState);
69 : static UARTDriver sitlSerial1Driver(1, &sitlState);
70 : static UARTDriver sitlSerial2Driver(2, &sitlState);
71 : static UARTDriver sitlSerial3Driver(3, &sitlState);
72 : static UARTDriver sitlSerial4Driver(4, &sitlState);
73 : static UARTDriver sitlSerial5Driver(5, &sitlState);
74 : static UARTDriver sitlSerial6Driver(6, &sitlState);
75 : static UARTDriver sitlSerial7Driver(7, &sitlState);
76 : static UARTDriver sitlSerial8Driver(8, &sitlState);
77 : static UARTDriver sitlSerial9Driver(9, &sitlState);
78 :
79 : static I2CDeviceManager i2c_mgr_instance;
80 :
81 : #if defined(HAL_BUILD_AP_PERIPH)
82 : static Empty::SPIDeviceManager spi_mgr_instance;
83 : #else
84 : static SPIDeviceManager spi_mgr_instance;
85 : #endif
86 : static Util utilInstance(&sitlState);
87 :
88 : #if HAL_NUM_CAN_IFACES
89 : static HALSITL::CANIface* canDrivers[HAL_NUM_CAN_IFACES];
90 : #endif
91 :
92 : static Empty::WSPIDeviceManager wspi_mgr_instance;
93 :
94 2145 : HAL_SITL::HAL_SITL() :
95 : AP_HAL::HAL(
96 : &sitlSerial0Driver,
97 : &sitlSerial1Driver,
98 : &sitlSerial2Driver,
99 : &sitlSerial3Driver,
100 : &sitlSerial4Driver,
101 : &sitlSerial5Driver,
102 : &sitlSerial6Driver,
103 : &sitlSerial7Driver,
104 : &sitlSerial8Driver,
105 : &sitlSerial9Driver,
106 : &i2c_mgr_instance,
107 : &spi_mgr_instance, /* spi */
108 : &wspi_mgr_instance,
109 : &sitlAnalogIn, /* analogin */
110 : &sitlStorage, /* storage */
111 : &sitlSerial0Driver, /* console */
112 : &sitlGPIO, /* gpio */
113 : &sitlRCInput, /* rcinput */
114 : &sitlRCOutput, /* rcoutput */
115 : &sitlScheduler, /* scheduler */
116 : &utilInstance, /* util */
117 : &emptyOpticalFlow, /* onboard optical flow */
118 : &emptyFlash, /* flash driver */
119 : #if AP_SIM_ENABLED
120 : &xsimstate,
121 : #endif
122 : #if HAL_WITH_DSP
123 : &dspDriver, /* dsp driver */
124 : #endif
125 : #if HAL_NUM_CAN_IFACES
126 : (AP_HAL::CANIface**)canDrivers
127 : #else
128 : nullptr
129 : #endif
130 : ), /* CAN */
131 2145 : _sitl_state(&sitlState)
132 2145 : {}
133 :
134 : static char *new_argv[100];
135 :
136 : /*
137 : save watchdog data
138 : */
139 424 : static bool watchdog_save(const uint32_t *data, uint32_t nwords)
140 : {
141 424 : int fd = ::open("persistent.dat.tmp", O_WRONLY|O_CREAT|O_TRUNC, 0644);
142 424 : bool ret = false;
143 424 : if (fd != -1) {
144 424 : if (::write(fd, data, nwords*4) == (ssize_t)(nwords*4)) {
145 424 : ret = true;
146 : }
147 424 : ::close(fd);
148 : }
149 424 : if (ret) {
150 424 : ret = ::rename("persistent.dat.tmp", "persistent.dat") == 0;
151 : }
152 424 : return ret;
153 : }
154 :
155 : /*
156 : load watchdog data
157 : */
158 1 : static bool watchdog_load(uint32_t *data, uint32_t nwords)
159 : {
160 1 : int fd = ::open("persistent.dat", O_RDONLY, 0644);
161 1 : bool ret = false;
162 1 : if (fd != -1) {
163 1 : ret = (::read(fd, data, nwords*4) == (ssize_t)(nwords*4));
164 1 : ::close(fd);
165 : }
166 1 : return ret;
167 : }
168 :
169 : /*
170 : implement watchdoh reset via SIGALRM
171 : */
172 1 : static void sig_alrm(int signum)
173 : {
174 : static char env[] = "SITL_WATCHDOG_RESET=1";
175 1 : putenv(env);
176 1 : printf("GOT SIGALRM\n");
177 1 : SocketAPM_native::cleanup_unix_paths();
178 1 : execv(new_argv[0], new_argv);
179 1 : }
180 :
181 454 : void HAL_SITL::exit_signal_handler(int signum)
182 : {
183 454 : HALSITL::Scheduler::_should_exit = true;
184 454 : }
185 :
186 2086 : void HAL_SITL::setup_signal_handlers() const
187 : {
188 2086 : struct sigaction sa = { };
189 :
190 2086 : sa.sa_flags = SA_NOCLDSTOP;
191 2086 : sa.sa_handler = HAL_SITL::exit_signal_handler;
192 2086 : sigaction(SIGTERM, &sa, NULL);
193 : #if defined(HAL_COVERAGE_BUILD) && HAL_COVERAGE_BUILD == 1
194 2086 : sigaction(SIGINT, &sa, NULL);
195 2086 : sigaction(SIGHUP, &sa, NULL);
196 2086 : sigaction(SIGQUIT, &sa, NULL);
197 : #endif
198 :
199 2086 : }
200 :
201 : /*
202 : fill 8k of stack with NaN. This allows us to find uses of
203 : uninitialised memory without valgrind
204 : */
205 4171669 : static void fill_stack_nan(void)
206 : {
207 : float stk[2048];
208 4171669 : fill_nanf(stk, ARRAY_SIZE(stk));
209 4171669 : }
210 :
211 6 : uint8_t HAL_SITL::get_instance() const
212 : {
213 6 : return _sitl_state->get_instance();
214 : }
215 :
216 : #if defined(HAL_BUILD_AP_PERIPH)
217 : bool HAL_SITL::run_in_maintenance_mode() const
218 : {
219 : return _sitl_state->run_in_maintenance_mode();
220 : }
221 : #endif
222 :
223 187275482 : uint32_t HAL_SITL::get_uart_output_full_queue_count() const
224 : {
225 187275482 : return _sitl_state->_serial_0_outqueue_full_count;
226 : }
227 :
228 2094 : void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const
229 : {
230 2094 : assert(callbacks);
231 :
232 2094 : utilInstance.init(argc, argv);
233 2094 : _sitl_state->init(argc, argv);
234 :
235 2094 : scheduler->init();
236 2094 : serial(0)->begin(115200);
237 :
238 2094 : rcin->init();
239 2094 : rcout->init();
240 :
241 : // spi->init();
242 2094 : analogin->init();
243 :
244 2094 : if (getenv("SITL_WATCHDOG_RESET")) {
245 1 : INTERNAL_ERROR(AP_InternalError::error_t::watchdog_reset);
246 1 : if (watchdog_load((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4)) {
247 1 : serial(0)->printf("Loaded watchdog data");
248 1 : utilInstance.last_persistent_data = utilInstance.persistent_data;
249 : }
250 : }
251 :
252 : // form a new argv, removing problem parameters. This is used for reboot
253 2094 : uint8_t new_argv_offset = 0;
254 19586 : for (uint8_t i=0; i<ARRAY_SIZE(new_argv) && i<argc; i++) {
255 17492 : if (!strcmp(argv[i], "-w")) {
256 : // don't wipe params on reboot
257 294 : continue;
258 : }
259 17198 : new_argv[new_argv_offset++] = argv[i];
260 : }
261 :
262 2094 : fill_stack_nan();
263 :
264 2094 : callbacks->setup();
265 2086 : scheduler->set_system_initialized();
266 :
267 : #if HAL_LOGGING_ENABLED
268 2086 : if (getenv("SITL_WATCHDOG_RESET")) {
269 1 : const AP_HAL::Util::PersistentData &pd = util->persistent_data;
270 2 : AP::logger().WriteCritical("WDOG", "TimeUS,Task,IErr,IErrCnt,IErrLn,MavMsg,MavCmd,SemLine", "QbIHHHHH",
271 : AP_HAL::micros64(),
272 1 : pd.scheduler_task,
273 1 : pd.internal_errors,
274 1 : pd.internal_error_count,
275 1 : pd.internal_error_last_line,
276 1 : pd.last_mavlink_msgid,
277 1 : pd.last_mavlink_cmd,
278 1 : pd.semaphore_line);
279 : }
280 : #endif
281 :
282 2086 : bool using_watchdog = AP_BoardConfig::watchdog_enabled();
283 2086 : if (using_watchdog) {
284 3 : signal(SIGALRM, sig_alrm);
285 3 : alarm(2);
286 : }
287 2086 : setup_signal_handlers();
288 :
289 2086 : uint32_t last_watchdog_save = AP_HAL::millis();
290 2086 : uint8_t fill_count = 0;
291 :
292 : while (true) {
293 41686708 : if (HALSITL::Scheduler::_should_exit) {
294 454 : ::fprintf(stderr, "Exitting\n");
295 454 : exit(0);
296 : }
297 41686254 : if (fill_count++ % 10 == 0) {
298 : // only fill every 10 loops. This still gives us a lot of
299 : // protection, but saves a lot of CPU
300 4169575 : fill_count = 1u;
301 4169575 : fill_stack_nan();
302 : }
303 41686254 : callbacks->loop();
304 41684622 : HALSITL::Scheduler::_run_io_procs();
305 :
306 41684622 : uint32_t now = AP_HAL::millis();
307 41684622 : if (now - last_watchdog_save >= 100 && using_watchdog) {
308 : // save persistent data every 100ms
309 424 : last_watchdog_save = now;
310 424 : watchdog_save((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4);
311 : }
312 :
313 41684622 : if (using_watchdog) {
314 : // note that this only works for a speedup of 1
315 2137 : alarm(2);
316 : }
317 41684622 : }
318 :
319 : actually_reboot();
320 : }
321 :
322 1639 : void HAL_SITL::actually_reboot()
323 : {
324 1639 : SocketAPM_native::cleanup_unix_paths();
325 1639 : execv(new_argv[0], new_argv);
326 1639 : AP_HAL::panic("PANIC: REBOOT FAILED: %s", strerror(errno));
327 : }
328 :
329 : static HAL_SITL hal_sitl_inst;
330 :
331 2137 : const AP_HAL::HAL& AP_HAL::get_HAL() {
332 2137 : return hal_sitl_inst;
333 : }
334 :
335 4239 : AP_HAL::HAL& AP_HAL::get_HAL_mutable() {
336 4239 : return hal_sitl_inst;
337 : }
338 :
339 : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
