#pragma once
//#pragma GCC optimize ("O2")
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_F4Light_Namespace.h"
#include "handler.h"
#include "Config.h"
#include "Semaphores.h"
#include "GPIO.h"
#include <delay.h>
#include <systick.h>
#include <boards.h>
#include <timer.h>
//#include <setjmp.h>
#define F4Light_SCHEDULER_MAX_IO_PROCS 10
#define MAIN_PRIORITY 100 // priority for main task
#define DRIVER_PRIORITY 98 // priority for drivers, speed of main will be 1/4 of this
#define IO_PRIORITY 115 // main task has 100 so IO tasks will use 1/16 of CPU
#define SHED_FREQ 10000 // timer's freq in Hz
#define TIMER_PERIOD 100 // task timeslice period in uS
#define MAIN_STACK_SIZE 4096U+1024U // measured use of stack is only 1.5K - but it grows up to 3K when using FatFs, also this includes 1K stack for ISR
#define IO_STACK_SIZE 4096U // IO_tasks stack size - io_thread can do work with filesystem, stack overflows if 2K
#define DEFAULT_STACK_SIZE 1024U // Default tasks stack size
#define SMALL_TASK_STACK 1024U // small stack for sensors
#define STACK_MAX 65536U
#if 1
#define EnterCriticalSection __set_BASEPRI(SVC_INT_PRIORITY << (8 - __NVIC_PRIO_BITS))
#define LeaveCriticalSection __set_BASEPRI(0)
#else
#define EnterCriticalSection noInterrupts()
#define LeaveCriticalSection interrupts()
#endif
/*
* Task run-time structure (Task control block AKA TCB)
*/
struct task_t {
const uint8_t* sp; // Task stack pointer, should be first to access from context switcher
task_t* next; // Next task (double linked list)
task_t* prev; // Previous task
Handler handle; // loop() in Revo_handler - to allow to change task, called via revo_call_handler
const uint8_t* stack; // Task stack bottom
uint8_t id; // id of task
uint8_t priority; // base priority of task
uint8_t curr_prio; // current priority of task, usually higher than priority
bool active; // task still not ended
bool f_yield; // task gives its quant voluntary (to not call it again)
uint32_t ttw; // time to wait - for delays and IO
uint32_t t_yield; // time when task loose control
uint32_t period; // if set then task will start on time basis
uint32_t time_start; // start time of task (for periodic tasks)
F4Light::Semaphore *sem; // task should start after owning this semaphore
F4Light::Semaphore *sem_wait; // task is waiting this semaphore
uint32_t sem_time; // max time to wait semaphore
uint32_t sem_start_wait; // time when waiting starts (can use t_yield but stays for clarity)
#if defined(MTASK_PROF)
uint32_t start; // microseconds of timeslice start
uint32_t in_isr; // time in ISR when task runs
uint32_t def_ttw; // default TTW - not as hard as period
uint8_t sw_type; // type of task switch
uint64_t time; // full time
uint32_t max_time; // maximal execution time of task - to show
uint32_t count; // call count to calc mean
uint32_t work_time; // max time of full task
uint32_t sem_max_wait; // max time of semaphore waiting
uint32_t quants; // count of ticks
uint32_t quants_time; // sum of quatn's times
uint32_t t_paused; // time task was paused on IO
uint32_t count_paused; // count task was paused on IO
uint32_t max_paused; // max time task was paused on IO
uint32_t max_c_paused; // count task was paused on IO
uint32_t stack_free; // free stack
#endif
uint32_t guard; // stack guard to check TCB corruption
};
extern "C" {
extern unsigned _estack; // defined by link script
extern uint32_t us_ticks;
extern void *_sdata;
extern void *_edata;
extern void *_sccm; // start of CCM
extern void *_eccm; // end of CCM vars
void revo_call_handler(Handler hh, uint32_t arg); // universal caller for all type handlers - memberProc and Proc
extern voidFuncPtr boardEmergencyHandler; // will be called on any fault or panic() before halt
void PendSV_Handler();
void SVC_Handler();
void getNextTask();
void switchContext();
void __do_context_switch();
void hal_try_kill_task_or_reboot(uint8_t n);
void hal_go_next_task();
void hal_stop_multitask();
extern task_t *s_running; // running task
extern task_t *next_task; // task to run next
extern caddr_t stack_bottom; // for SBRK check
bool hal_is_armed();
// publish to low-level functions
void hal_yield(uint16_t ttw);
void hal_delay(uint16_t t);
void hal_delay_microseconds(uint16_t t);
uint32_t hal_micros();
void hal_isr_time(uint32_t t);
// task management for USB MSC mode
void hal_set_task_active(void * handle);
void hal_context_switch_isr();
void * hal_register_task(voidFuncPtr task, uint32_t stack);
void hal_set_task_priority(void * handle, uint8_t prio);
void enqueue_flash_erase(uint32_t from, uint32_t to);
}
#define RAMEND ((size_t)&_estack)
#ifdef SHED_DEBUG
typedef struct RevoSchedLog {
uint32_t start;
uint32_t end;
uint32_t ttw;
uint32_t time_start;
uint32_t quant;
uint32_t in_isr;
task_t *want_tail;
uint8_t task_id;
uint8_t prio;
uint8_t active;
uint8_t sw_type;
} revo_sched_log;
#define SHED_DEBUG_SIZE 512
#endif
enum Revo_IO_Flags {
IO_PERIODIC= 0,
IO_ONCE = 1,
};
typedef struct REVO_IO {
Handler h;
Revo_IO_Flags flags;
} Revo_IO;
class F4Light::Scheduler : public AP_HAL::Scheduler {
public:
typedef struct IO_COMPLETION {
Handler handler;
bool request;
#ifdef SHED_PROF
uint64_t time;
uint32_t count;
uint32_t max_time;
#endif
} IO_Completion;
Scheduler();
void init();
inline void delay(uint16_t ms) { _delay(ms); } // uses internal static methods
inline void delay_microseconds(uint16_t us) { _delay_microseconds(us); }
inline void delay_microseconds_boost(uint16_t us) override { _delay_microseconds_boost(us); }
inline uint32_t millis() { return AP_HAL::millis(); }
inline uint32_t micros() { return _micros(); }
inline void register_timer_process(AP_HAL::MemberProc proc) { _register_timer_process(proc, 1000); }
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms) override;
static void _register_io_process(Handler h, Revo_IO_Flags flags);
void register_io_process(AP_HAL::MemberProc proc) { Revo_handler h = { .mp=proc }; _register_io_process(h.h, IO_PERIODIC); }
static inline void _register_timer_process(AP_HAL::MemberProc proc, uint32_t period) {
Revo_handler r = { .mp=proc };
_register_timer_task(period, r.h, NULL);
}
inline bool in_timerprocess() { return false; } // we don't calls anything in ISR
void inline register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) { _failsafe = failsafe; }
void system_initialized();
static void _reboot(bool hold_in_bootloader);
void reboot(bool hold_in_bootloader);
inline bool in_main_thread() const override { return _in_main_thread(); }
// drivers are not the best place for its own sheduler so let do it here
static inline AP_HAL::Device::PeriodicHandle register_timer_task(uint32_t period_us, AP_HAL::Device::PeriodicCb proc, F4Light::Semaphore *sem) {
Revo_handler r = { .pcb=proc };
return _register_timer_task(period_us, r.h, sem);
}
static void _delay(uint16_t ms);
static void _delay_microseconds(uint16_t us);
static void _delay_microseconds_boost(uint16_t us);
static void _delay_us_ny(uint16_t us); // no yield delay
static inline uint32_t _millis() { return systick_uptime(); } //systick_uptime returns 64-bit time
static inline uint64_t _millis64() { return systick_uptime(); }
static inline uint32_t _micros() { return timer_get_count32(TIMER5); }
static uint64_t _micros64();
static bool adjust_timer_task(AP_HAL::Device::PeriodicHandle h, uint32_t period_us);
static bool unregister_timer_task(AP_HAL::Device::PeriodicHandle h);
void loop(); // to add ability to print out scheduler's stats in main thread
static inline bool in_interrupt(){ return (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) /* || (__get_BASEPRI()) */; }
//{ this functions do a preemptive multitask and inspired by Arduino-Scheduler (Mikael Patel), and scmrtos
/**
* Initiate scheduler and main task with given stack size. Should
* be called before start of any tasks if the main task requires a
* stack size other than the default size. Returns true if
* successful otherwise false.
* @param[in] stackSize in bytes.
* @return bool.
*/
static inline bool adjust_stack(size_t stackSize) { s_top = stackSize; return true; }
/**
* Start a task with given function and stack size. Should be
* called from main task. The functions are executed by the
* task. The taskLoop function is repeatedly called. Returns
* not-NULL if successful otherwise NULL (no memory for new task).
* @param[in] taskLoop function to call.
* @param[in] stackSize in bytes.
* @return address of TCB.
*/
static void * _start_task(Handler h, size_t stackSize);
static inline void * start_task(voidFuncPtr taskLoop, size_t stackSize = DEFAULT_STACK_SIZE){
Revo_handler r = { .vp=taskLoop };
return _start_task(r.h, stackSize);
}
static inline void * start_task(AP_HAL::MemberProc proc, size_t stackSize = DEFAULT_STACK_SIZE){
Revo_handler r = { .mp=proc };
return _start_task(r.h, stackSize);
}
// not used - tasks are never stopped
static void stop_task(void * h);
// functions to alter task's properties
//[ this functions called only at task start
static void set_task_period(void *h, uint32_t period); // task will be auto-activated by this period
static inline void set_task_priority(void *h, uint8_t prio){ // priority is a relative speed of task
task_t *task = (task_t *)h;
task->curr_prio= prio;
task->priority = prio;
}
// task wants to run only with this semaphore owned
static inline void set_task_semaphore(void *h, F4Light::Semaphore *sem){ // taskLoop function will be called owning this semaphore
task_t *task = (task_t *)h;
task->sem = sem;
}
//]
// this functions are atomic so don't need to disable interrupts
static inline void *get_current_task() { // get task handler or 0 if called from ISR
if(in_interrupt()) return NULL;
return s_running;
}
static inline void *get_current_task_isr() { // get current task handler even if called from ISR
return s_running;
}
static inline void set_task_active(void *h) { // tasks are created in stopped state
task_t * task = (task_t*)h;
task->active=true;
}
// do context switch after return from interrupt
static inline void context_switch_isr(){ timer_generate_update(TIMER14); }
#if defined(MTASK_PROF)
static void inline task_pause(uint32_t t) { // called from task when it starts transfer
s_running->ttw=t;
s_running->sem_start_wait=_micros();
s_running->count_paused++;
}
static void inline task_resume(void *h) { // called from IO_Complete ISR to resume task
#if defined(USE_MPU)
mpu_disable(); // we need access to write
#endif
task_t * task = (task_t*)h;
task->ttw=0;
task->active=true;
_forced_task = task; // force it. Thus we exclude loop to select task
context_switch_isr();
uint32_t dt= _micros() - task->sem_start_wait;
task->t_paused += dt;
}
#else
static void inline task_pause(uint16_t t) { s_running->ttw=t; } // called from task when it starts IO transfer
static void inline task_resume(void *h) { // called from IO_Complete ISR to resume task, and will get 1st quant 100%
#if defined(USE_MPU)
mpu_disable(); // we need access to write
#endif
task_t * task = (task_t*)h; // called from ISR so don't need disabling interrupts when writes to TCB
task->ttw=0;
task->active=true;
_forced_task = task; // force it, to not spent time for search by priority
context_switch_isr();
}
#endif
//]
/*
task scheduler. Gives task ready to run with highest priority
*/
static task_t *get_next_task();
/*
* finish current tick and schedule new task excluding this
*/
static void yield(uint16_t ttw=0); // optional time to wait
/**
* Return current task stack size.
* @return bytes
*/
static size_t task_stack();
// check from what task it called
static inline bool _in_main_thread() { return s_running == &s_main; }
// resume task that called delay_boost()
static void resume_boost(){
if(boost_task) {
task_t *task = (task_t *) boost_task;
boost_task=NULL;
if(task->ttw){ // task now in wait
#if defined(USE_MPU)
mpu_disable(); // we need access to write
#endif
uint32_t now = _micros();
uint32_t timeFromLast = now - task->t_yield; // time since that moment
if(task->ttw<=100 || timeFromLast > task->ttw*3/2){ // gone 2/3 of time?
task->ttw=0; // stop waiting
task->active=true;
_forced_task = task; // force it
}
} else {
_forced_task = task; // just force it
}
}
}
static inline void plan_context_switch(){
need_switch_task = true; // require context switch
SCB->ICSR = SCB_ICSR_PENDSVSET_Msk; // PENDSVSET
}
static void SVC_Handler(uint32_t * svc_args); // many functions called via SVC for hardware serialization
static void _try_kill_task_or_reboot(uint8_t n); // exception occures in armed state - try to kill current task
static void _go_next_task();
static void _stop_multitask();
static volatile bool need_switch_task; // should be public for access from C code
//}
//{ IO completion routines, allows to move out time consuming parts from ISR
#define MAX_IO_COMPLETION 8
typedef voidFuncPtr ioc_proc;
static uint8_t register_io_completion(Handler handle);
static inline uint8_t register_io_completion(ioc_proc cb) {
Revo_handler r = { .vp=cb };
return register_io_completion(r.h);
}
static inline uint8_t register_io_completion(AP_HAL::MemberProc proc) {
Revo_handler r = { .mp=proc };
return register_io_completion(r.h);
}
static inline void do_io_completion(uint8_t id){ // schedule selected IO completion
if(id) {
io_completion[id-1].request = true;
timer_generate_update(TIMER13);
}
}
static void exec_io_completion();
static volatile bool need_io_completion;
//}
// helpers
static inline Handler get_handler(AP_HAL::MemberProc proc){
Revo_handler h = { .mp = proc };
return h.h;
}
static inline Handler get_handler(AP_HAL::Proc proc){
Revo_handler h = { .hp = proc };
return h.h;
}
static inline void setEmergencyHandler(voidFuncPtr handler) { boardEmergencyHandler = handler; }
#ifdef MPU_DEBUG
static inline void MPU_buffer_overflow(){ MPU_overflow_cnt++; }
static inline void MPU_restarted() { MPU_restart_cnt++; }
static inline void MPU_stats(uint16_t count, uint32_t time) {
if(count>MPU_count) {
MPU_count=count;
MPU_Time=time;
}
}
#endif
static inline void arming_state_changed(bool v){ if(!v && on_disarm_handler) revo_call_handler(on_disarm_handler, 0); }
static inline void register_on_disarm(Handler h){ on_disarm_handler=h; }
static void start_stats_task(); // it interferes with CONNECT_COM and CONNECT_ESC so should be started last
protected:
//{ multitask
// executor for task's handler, never called but used when task context formed
static void do_task(task_t * task);
// gves first deleted task or NULL - not used because tasks are never finished
static task_t* get_empty_task();
/*
* Initiate a task with the given functions and stack. When control
* is yield to the task then the loop function is repeatedly called.
* @param[in] h task handler (may be NULL)
* @param[in] stack top reference.
*/
static void *init_task(uint64_t h, const uint8_t* stack);
static uint32_t fill_task(task_t &tp); // prepares task's TCB
static void enqueue_task(task_t &tp); // add new task to run queue
static void dequeue_task(task_t &tp); // remove task from run queue
// plan context switch
static void switch_task();
static void _switch_task();
static task_t s_main; // main task TCB
static size_t s_top; // Task stack allocation top.
static uint16_t task_n; // counter of tasks
static task_t *_idle_task; // remember TCB of idle task
static task_t *_forced_task; // task activated from ISR so should be called without prioritization
static void *boost_task; // task that called delay_boost()
static void check_stack(uint32_t sp);
#define await(cond) while(!(cond)) yield()
//} end of multitask
private:
static AP_HAL::Device::PeriodicHandle _register_timer_task(uint32_t period_us, Handler proc, F4Light::Semaphore *sem);
static void * _delay_cb_handle;
static bool _initialized;
// ISR functions
static void _timer_isr_event(uint32_t v /*TIM_TypeDef *tim */);
static void _timer5_ovf(uint32_t v /*TIM_TypeDef *tim */ );
static void _tail_timer_event(uint32_t v /*TIM_TypeDef *tim */);
static void _ioc_timer_event(uint32_t v /*TIM_TypeDef *tim */);
static void _delay_timer_event(uint32_t v /*TIM_TypeDef *tim */);
static void _run_timer_procs(bool called_from_isr);
static uint32_t timer5_ovf_cnt; // high part of 64-bit time
static AP_HAL::Proc _failsafe; // periodically called from ISR
static Revo_IO _io_proc[F4Light_SCHEDULER_MAX_IO_PROCS]; // low priority tasks for IO thread
static void _run_io(void);
static uint8_t _num_io_proc;
static bool _in_io_proc;
static Handler on_disarm_handler;
static void _print_stats();
static uint32_t lowest_stack;
static struct IO_COMPLETION io_completion[MAX_IO_COMPLETION];
static uint8_t num_io_completion;
#ifdef SHED_PROF
static uint64_t shed_time;
static uint64_t task_time;
static bool flag_10s;
static uint64_t delay_time;
static uint64_t delay_int_time;
static uint32_t max_loop_time;
static void _set_10s_flag();
static uint64_t ioc_time;
static uint64_t sleep_time;
static uint32_t max_delay_err;
static uint32_t tick_micros; // max exec time
static uint32_t tick_count; // number of calls
static uint64_t tick_fulltime; // full consumed time to calc mean
#endif
#ifdef MTASK_PROF
static uint32_t max_wfe_time;
static uint32_t tsched_count;
static uint32_t tsched_sw_count;
static uint32_t tsched_count_y;
static uint32_t tsched_sw_count_y;
static uint32_t tsched_count_t;
static uint32_t tsched_sw_count_t;
#ifdef SHED_DEBUG
static revo_sched_log logbuf[SHED_DEBUG_SIZE];
static uint16_t sched_log_ptr;
#endif
#endif
#ifdef MPU_DEBUG
static uint32_t MPU_overflow_cnt;
static uint32_t MPU_restart_cnt;
static uint32_t MPU_count;
static uint32_t MPU_Time;
#endif
};
void revo_call_handler(Handler h, uint32_t arg);