You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
251 lines
6.4 KiB
251 lines
6.4 KiB
/* |
|
* 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.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; |
|
#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; |
|
#endif |
|
|
|
#if HAL_USE_I2C == TRUE |
|
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 |
|
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; |
|
|
|
|
|
#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, |
|
&i2cDeviceManager, |
|
&spiDeviceManager, |
|
&analogIn, |
|
&storageDriver, |
|
&uartADriver, |
|
&gpioDriver, |
|
&rcinDriver, |
|
&rcoutDriver, |
|
&schedulerInstance, |
|
&utilInstance, |
|
&opticalFlowDriver, |
|
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 THD_FUNCTION(main_loop,arg) |
|
{ |
|
daemon_task = chThdGetSelfX(); |
|
|
|
#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(); |
|
|
|
hal.uartA->begin(115200); |
|
|
|
#ifdef HAL_SPI_CHECK_CLOCK_FREQ |
|
// optional test of SPI clock frequencies |
|
ChibiOS::SPIDevice::test_clock_freq(); |
|
#endif |
|
|
|
//Setup SD Card and Initialise FATFS bindings |
|
sdcard_init(); |
|
|
|
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); |
|
|
|
schedulerInstance.hal_initialized(); |
|
|
|
g_callbacks->setup(); |
|
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 250 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 |
|
*/ |
|
if (!schedulerInstance.check_called_boost()) { |
|
hal.scheduler->delay_microseconds(250); |
|
} |
|
} |
|
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. |
|
*/ |
|
|
|
#ifdef HAL_USB_PRODUCT_ID |
|
setup_usb_strings(); |
|
#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; |
|
|
|
void *main_thread_wa = hal.util->malloc_type(THD_WORKING_AREA_SIZE(APM_MAIN_THREAD_STACK_SIZE), AP_HAL::Util::MEM_FAST); |
|
chThdCreateStatic(main_thread_wa, |
|
APM_MAIN_THREAD_STACK_SIZE, |
|
APM_MAIN_PRIORITY, /* Initial priority. */ |
|
main_loop, /* Thread function. */ |
|
nullptr); /* Thread parameter. */ |
|
chThdExit(0); |
|
} |
|
|
|
const AP_HAL::HAL& AP_HAL::get_HAL() { |
|
static const HAL_ChibiOS hal_chibios; |
|
return hal_chibios; |
|
} |
|
|
|
#endif
|
|
|