/
HAL_ChibiOS_Class.cpp
310 lines (263 loc) · 8.16 KB
/
HAL_ChibiOS_Class.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <assert.h>
#include "HAL_ChibiOS_Class.h"
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
#include <AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h>
#include "shared_dma.h"
#include "sdcard.h"
#include "hwdef/common/usbcfg.h"
#include "hwdef/common/stm32_util.h"
#include "hwdef/common/watchdog.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_InternalError/AP_InternalError.h>
#ifndef HAL_BOOTLOADER_BUILD
#include <AP_Logger/AP_Logger.h>
#endif
#include <hwdef.h>
#ifndef HAL_NO_UARTDRIVER
static HAL_UARTA_DRIVER;
static HAL_UARTB_DRIVER;
static HAL_UARTC_DRIVER;
static HAL_UARTD_DRIVER;
static HAL_UARTE_DRIVER;
static HAL_UARTF_DRIVER;
static HAL_UARTG_DRIVER;
static HAL_UARTH_DRIVER;
#else
static Empty::UARTDriver uartADriver;
static Empty::UARTDriver uartBDriver;
static Empty::UARTDriver uartCDriver;
static Empty::UARTDriver uartDDriver;
static Empty::UARTDriver uartEDriver;
static Empty::UARTDriver uartFDriver;
static Empty::UARTDriver uartGDriver;
static Empty::UARTDriver uartHDriver;
#endif
#if HAL_USE_I2C == TRUE && defined(HAL_I2C_DEVICE_LIST)
static ChibiOS::I2CDeviceManager i2cDeviceManager;
#else
static Empty::I2CDeviceManager i2cDeviceManager;
#endif
#if HAL_USE_SPI == TRUE
static ChibiOS::SPIDeviceManager spiDeviceManager;
#else
static Empty::SPIDeviceManager spiDeviceManager;
#endif
#if HAL_USE_ADC == TRUE && !defined(HAL_DISABLE_ADC_DRIVER)
static ChibiOS::AnalogIn analogIn;
#else
static Empty::AnalogIn analogIn;
#endif
#ifdef HAL_USE_EMPTY_STORAGE
static Empty::Storage storageDriver;
#else
static ChibiOS::Storage storageDriver;
#endif
static ChibiOS::GPIO gpioDriver;
static ChibiOS::RCInput rcinDriver;
#if HAL_USE_PWM == TRUE
static ChibiOS::RCOutput rcoutDriver;
#else
static Empty::RCOutput rcoutDriver;
#endif
static ChibiOS::Scheduler schedulerInstance;
static ChibiOS::Util utilInstance;
static Empty::OpticalFlow opticalFlowDriver;
#ifndef HAL_NO_FLASH_SUPPORT
static ChibiOS::Flash flashDriver;
#else
static Empty::Flash flashDriver;
#endif
#if HAL_WITH_IO_MCU
HAL_UART_IO_DRIVER;
#include <AP_IOMCU/AP_IOMCU.h>
AP_IOMCU iomcu(uart_io);
#endif
HAL_ChibiOS::HAL_ChibiOS() :
AP_HAL::HAL(
&uartADriver,
&uartBDriver,
&uartCDriver,
&uartDDriver,
&uartEDriver,
&uartFDriver,
&uartGDriver,
&uartHDriver,
&i2cDeviceManager,
&spiDeviceManager,
&analogIn,
&storageDriver,
&uartADriver,
&gpioDriver,
&rcinDriver,
&rcoutDriver,
&schedulerInstance,
&utilInstance,
&opticalFlowDriver,
&flashDriver,
nullptr
)
{}
static bool thread_running = false; /**< Daemon status flag */
static thread_t* daemon_task; /**< Handle of daemon task / thread */
extern const AP_HAL::HAL& hal;
/*
set the priority of the main APM task
*/
void hal_chibios_set_priority(uint8_t priority)
{
chSysLock();
#if CH_CFG_USE_MUTEXES == TRUE
if ((daemon_task->prio == daemon_task->realprio) || (priority > daemon_task->prio)) {
daemon_task->prio = priority;
}
daemon_task->realprio = priority;
#endif
chSchRescheduleS();
chSysUnlock();
}
thread_t* get_main_thread()
{
return daemon_task;
}
static AP_HAL::HAL::Callbacks* g_callbacks;
static AP_HAL::Util::PersistentData last_persistent_data;
static void main_loop()
{
daemon_task = chThdGetSelfX();
/*
switch to high priority for main loop
*/
chThdSetPriority(APM_MAIN_PRIORITY);
#ifdef HAL_I2C_CLEAR_BUS
// Clear all I2C Buses. This can be needed on some boards which
// can get a stuck I2C peripheral on boot
ChibiOS::I2CBus::clear_all();
#endif
ChibiOS::Shared_DMA::init();
peripheral_power_enable();
hal.uartA->begin(115200);
#ifdef HAL_SPI_CHECK_CLOCK_FREQ
// optional test of SPI clock frequencies
ChibiOS::SPIDevice::test_clock_freq();
#endif
hal.uartB->begin(38400);
hal.uartC->begin(57600);
hal.analogin->init();
hal.scheduler->init();
/*
run setup() at low priority to ensure CLI doesn't hang the
system, and to allow initial sensor read loops to run
*/
hal_chibios_set_priority(APM_STARTUP_PRIORITY);
if (stm32_was_watchdog_reset()) {
// load saved watchdog data
stm32_watchdog_load((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4);
last_persistent_data = utilInstance.persistent_data;
}
schedulerInstance.hal_initialized();
g_callbacks->setup();
#ifdef IOMCU_FW
stm32_watchdog_init();
#elif !defined(HAL_BOOTLOADER_BUILD)
// setup watchdog to reset if main loop stops
if (AP_BoardConfig::watchdog_enabled()) {
stm32_watchdog_init();
}
#ifndef HAL_NO_LOGGING
if (hal.util->was_watchdog_reset()) {
AP::internalerror().error(AP_InternalError::error_t::watchdog_reset);
const AP_HAL::Util::PersistentData &pd = last_persistent_data;
AP::logger().WriteCritical("WDOG", "TimeUS,Task,IErr,IErrCnt,MavMsg,MavCmd,SemLine,FL,FT,FA,FP,ICSR", "QbIIHHHHHIBI",
AP_HAL::micros64(),
pd.scheduler_task,
pd.internal_errors,
pd.internal_error_count,
pd.last_mavlink_msgid,
pd.last_mavlink_cmd,
pd.semaphore_line,
pd.fault_line,
pd.fault_type,
pd.fault_addr,
pd.fault_thd_prio,
pd.fault_icsr);
}
#endif // HAL_NO_LOGGING
#endif // IOMCU_FW
schedulerInstance.watchdog_pat();
hal.scheduler->system_initialized();
thread_running = true;
chRegSetThreadName(SKETCHNAME);
/*
switch to high priority for main loop
*/
chThdSetPriority(APM_MAIN_PRIORITY);
while (true) {
g_callbacks->loop();
/*
give up 50 microseconds of time if the INS loop hasn't
called delay_microseconds_boost(), to ensure low priority
drivers get a chance to run. Calling
delay_microseconds_boost() means we have already given up
time from the main loop, so we don't need to do it again
here
*/
#ifndef HAL_DISABLE_LOOP_DELAY
if (!schedulerInstance.check_called_boost()) {
hal.scheduler->delay_microseconds(50);
}
#endif
schedulerInstance.watchdog_pat();
}
thread_running = false;
}
void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
{
/*
* System initializations.
* - ChibiOS HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
#if HAL_USE_SERIAL_USB == TRUE
usb_initiailse();
#endif
#ifdef HAL_STDOUT_SERIAL
//STDOUT Initialistion
SerialConfig stdoutcfg =
{
HAL_STDOUT_BAUDRATE,
0,
USART_CR2_STOP1_BITS,
0
};
sdStart((SerialDriver*)&HAL_STDOUT_SERIAL, &stdoutcfg);
#endif
assert(callbacks);
g_callbacks = callbacks;
//Takeover main
main_loop();
}
const AP_HAL::HAL& AP_HAL::get_HAL() {
static const HAL_ChibiOS hal_chibios;
return hal_chibios;
}
#endif