ardupilot/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp

329 lines
8.1 KiB
C++

/*
* 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 <AP_Vehicle/AP_Vehicle_Type.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;
static HAL_UARTH_DRIVER;
static HAL_UARTI_DRIVER;
static HAL_UARTJ_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;
static Empty::UARTDriver uartIDriver;
static Empty::UARTDriver uartJDriver;
#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;
#if HAL_WITH_DSP
static ChibiOS::DSP dspDriver;
#else
static Empty::DSP dspDriver;
#endif
#ifndef HAL_NO_FLASH_SUPPORT
static ChibiOS::Flash flashDriver;
#else
static Empty::Flash flashDriver;
#endif
#if HAL_NUM_CAN_IFACES > 0
static ChibiOS::CANIface* canDrivers[HAL_NUM_CAN_IFACES];
#endif
#if HAL_USE_WSPI == TRUE && defined(HAL_QSPI_DEVICE_LIST)
static ChibiOS::QSPIDeviceManager qspiDeviceManager;
#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,
&uartIDriver,
&uartJDriver,
&i2cDeviceManager,
&spiDeviceManager,
#if HAL_USE_WSPI == TRUE && defined(HAL_QSPI_DEVICE_LIST)
&qspiDeviceManager,
#else
nullptr,
#endif
&analogIn,
&storageDriver,
&uartADriver,
&gpioDriver,
&rcinDriver,
&rcoutDriver,
&schedulerInstance,
&utilInstance,
&opticalFlowDriver,
&flashDriver,
&dspDriver,
#if HAL_NUM_CAN_IFACES
(AP_HAL::CANIface**)canDrivers
#else
nullptr
#endif
)
{}
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->hdr.pqueue.prio == daemon_task->realprio) || (priority > daemon_task->hdr.pqueue.prio)) {
daemon_task->hdr.pqueue.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 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
#ifndef HAL_NO_SHARED_DMA
ChibiOS::Shared_DMA::init();
#endif
peripheral_power_enable();
hal.serial(0)->begin(115200);
#ifdef HAL_SPI_CHECK_CLOCK_FREQ
// optional test of SPI clock frequencies
ChibiOS::SPIDevice::test_clock_freq();
#endif
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);
utilInstance.last_persistent_data = utilInstance.persistent_data;
}
schedulerInstance.hal_initialized();
g_callbacks->setup();
#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
utilInstance.apply_persistent_params();
#endif
#if !defined(DISABLE_WATCHDOG)
#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();
}
if (hal.util->was_watchdog_reset()) {
INTERNAL_ERROR(AP_InternalError::error_t::watchdog_reset);
}
#endif // IOMCU_FW
#endif // DISABLE_WATCHDOG
schedulerInstance.watchdog_pat();
hal.scheduler->set_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
*/
#if !defined(HAL_DISABLE_LOOP_DELAY) && !APM_BUILD_TYPE(APM_BUILD_Replay)
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_initialise();
#endif
#ifdef HAL_STDOUT_SERIAL
//STDOUT Initialisation
SerialConfig stdoutcfg =
{
HAL_STDOUT_BAUDRATE,
0,
USART_CR2_STOP1_BITS,
0
};
sdStart((SerialDriver*)&HAL_STDOUT_SERIAL, &stdoutcfg);
#endif
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