Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: add uavcan thread

Browse Source
This commit is contained in:
Siddharth Purohit 2018-02-02 00:51:18 +05:30 committed by Andrew Tridgell
parent 3c009cf02c
commit f2e77ab773
2 changed files with 40 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_HAL_ChibiOS/Scheduler.cpp
View File

@ -25,6 +25,7 @@
#include <AP_HAL_ChibiOS/Storage.h> #include <AP_HAL_ChibiOS/Storage.h>
#include <AP_HAL_ChibiOS/RCOutput.h> #include <AP_HAL_ChibiOS/RCOutput.h>
#include <AP_HAL_ChibiOS/RCInput.h> #include <AP_HAL_ChibiOS/RCInput.h>
#include <AP_HAL_ChibiOS/CAN.h>
#include <AP_Scheduler/AP_Scheduler.h> #include <AP_Scheduler/AP_Scheduler.h>
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
@ -37,7 +38,9 @@ THD_WORKING_AREA(_rcin_thread_wa, 512);
THD_WORKING_AREA(_io_thread_wa, 2048); THD_WORKING_AREA(_io_thread_wa, 2048);
THD_WORKING_AREA(_storage_thread_wa, 2048); THD_WORKING_AREA(_storage_thread_wa, 2048);
THD_WORKING_AREA(_uart_thread_wa, 2048); THD_WORKING_AREA(_uart_thread_wa, 2048);
#ifdef HAL_WITH_UAVCAN
THD_WORKING_AREA(_uavcan_thread_wa, 2048);
#endif
#if HAL_WITH_IO_MCU #if HAL_WITH_IO_MCU
extern ChibiOS::UARTDriver uart_io; extern ChibiOS::UARTDriver uart_io;
#endif #endif
@ -54,6 +57,14 @@ void Scheduler::init()
_timer_thread, /* Thread function. */ _timer_thread, /* Thread function. */
this); /* Thread parameter. */ this); /* Thread parameter. */
// setup the timer thread - this will call tasks at 1kHz
#ifdef HAL_WITH_UAVCAN
_uavcan_thread_ctx = chThdCreateStatic(_uavcan_thread_wa,
sizeof(_uavcan_thread_wa),
APM_UAVCAN_PRIORITY, /* Initial priority. */
_uavcan_thread, /* Thread function. */
this); /* Thread parameter. */
#endif
// setup the RCIN thread - this will call tasks at 1kHz // setup the RCIN thread - this will call tasks at 1kHz
_rcin_thread_ctx = chThdCreateStatic(_rcin_thread_wa, _rcin_thread_ctx = chThdCreateStatic(_rcin_thread_wa,
sizeof(_rcin_thread_wa), sizeof(_rcin_thread_wa),
@ -264,6 +275,24 @@ void Scheduler::_timer_thread(void *arg)
hal.rcout->timer_tick(); hal.rcout->timer_tick();
} }
} }
#if HAL_WITH_UAVCAN
void Scheduler::_uavcan_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
sched->_rcin_thread_ctx->name = "apm_uavcan";
while (!sched->_hal_initialized) {
sched->delay_microseconds(20000);
}
while (true) {
sched->delay_microseconds(1000);
for (int i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) {
if(hal.can_mgr[i] != nullptr) {
CANManager::from(hal.can_mgr[i])->_timer_tick();
}
}
}
}
#endif
void Scheduler::_rcin_thread(void *arg) void Scheduler::_rcin_thread(void *arg)
{ {

11
libraries/AP_HAL_ChibiOS/Scheduler.h
View File

@ -36,6 +36,10 @@
#define APM_SPI_PRIORITY 179 #define APM_SPI_PRIORITY 179
#endif #endif
#ifndef APM_UAVCAN_PRIORITY
#define APM_UAVCAN_PRIORITY 178
#endif
#ifndef APM_CAN_PRIORITY #ifndef APM_CAN_PRIORITY
#define APM_CAN_PRIORITY 177 #define APM_CAN_PRIORITY 177
#endif #endif
@ -108,12 +112,17 @@ private:
thread_t* _io_thread_ctx; thread_t* _io_thread_ctx;
thread_t* _storage_thread_ctx; thread_t* _storage_thread_ctx;
thread_t* _uart_thread_ctx; thread_t* _uart_thread_ctx;
#if HAL_WITH_UAVCAN
thread_t* _uavcan_thread_ctx;
#endif
static void _timer_thread(void *arg); static void _timer_thread(void *arg);
static void _rcin_thread(void *arg); static void _rcin_thread(void *arg);
static void _io_thread(void *arg); static void _io_thread(void *arg);
static void _storage_thread(void *arg); static void _storage_thread(void *arg);
static void _uart_thread(void *arg); static void _uart_thread(void *arg);
#if HAL_WITH_UAVCAN
static void _uavcan_thread(void *arg);
#endif
void _run_timers(bool called_from_timer_thread); void _run_timers(bool called_from_timer_thread);
void _run_io(void); void _run_io(void);
}; };