Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <pthread.h>
       4             : 
       5             : #include "AP_HAL_Linux.h"
       6             : 
       7             : #include "Semaphores.h"
       8             : #include "Thread.h"
       9             : 
      10             : #define LINUX_SCHEDULER_MAX_TIMER_PROCS 10
      11             : #define LINUX_SCHEDULER_MAX_TIMESLICED_PROCS 10
      12             : #define LINUX_SCHEDULER_MAX_IO_PROCS 10
      13             : 
      14             : #define AP_LINUX_SENSORS_STACK_SIZE  256 * 1024
      15             : #define AP_LINUX_SENSORS_SCHED_POLICY  SCHED_FIFO
      16             : #define AP_LINUX_SENSORS_SCHED_PRIO 12
      17             : 
      18             : namespace Linux {
      19             : 
      20             : class Scheduler : public AP_HAL::Scheduler {
      21             : public:
      22             :     Scheduler();
      23             : 
      24        2337 :     static Scheduler *from(AP_HAL::Scheduler *scheduler) {
      25        2337 :         return static_cast<Scheduler*>(scheduler);
      26             :     }
      27             : 
      28             :     void     init() override;
      29             :     void     delay(uint16_t ms) override;
      30             :     void     delay_microseconds(uint16_t us) override;
      31             : 
      32             :     void     register_timer_process(AP_HAL::MemberProc) override;
      33             :     void     register_io_process(AP_HAL::MemberProc) override;
      34             : 
      35             :     bool     in_main_thread() const override;
      36             : 
      37             :     void     register_timer_failsafe(AP_HAL::Proc, uint32_t period_us) override;
      38             : 
      39             :     void     set_system_initialized() override;
      40           0 :     bool     is_system_initialized() override { return _initialized; };
      41             : 
      42             :     void     reboot(bool hold_in_bootloader) override;
      43             : 
      44             :     void     stop_clock(uint64_t time_usec) override;
      45             : 
      46        2328 :     uint64_t stopped_clock_usec() const { return _stopped_clock_usec; }
      47             : 
      48             :     void microsleep(uint32_t usec);
      49             : 
      50             :     void teardown();
      51             : 
      52             :     /*
      53             :       create a new thread
      54             :      */
      55             :     bool thread_create(AP_HAL::MemberProc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) override;
      56             :     
      57             :     /*
      58             :       set cpu affinity mask to be applied on initialization - setting it
      59             :       later has no effect.
      60             :      */
      61           0 :     void set_cpu_affinity(const cpu_set_t &cpu_affinity) { _cpu_affinity = cpu_affinity; }
      62             : 
      63             : private:
      64             :     class SchedulerThread : public PeriodicThread {
      65             :     public:
      66         192 :         SchedulerThread(Thread::task_t t, Scheduler &sched)
      67         192 :             : PeriodicThread(t)
      68         192 :             , _sched(sched)
      69         192 :         { }
      70             : 
      71             :     protected:
      72             :         bool _run() override;
      73             : 
      74             :         Scheduler &_sched;
      75             :     };
      76             : 
      77             :     void     init_realtime();
      78             : 
      79             :     void     init_cpu_affinity();
      80             : 
      81             :     void _wait_all_threads();
      82             : 
      83             :     void     _debug_stack();
      84             : 
      85             :     AP_HAL::Proc _failsafe;
      86             : 
      87             :     bool _initialized;
      88             :     pthread_barrier_t _initialized_barrier;
      89             : 
      90             :     AP_HAL::MemberProc _timer_proc[LINUX_SCHEDULER_MAX_TIMER_PROCS];
      91             :     uint8_t _num_timer_procs;
      92             :     volatile bool _in_timer_proc;
      93             : 
      94             :     AP_HAL::MemberProc _io_proc[LINUX_SCHEDULER_MAX_IO_PROCS];
      95             :     uint8_t _num_io_procs;
      96             : 
      97             :     // calculates an integer to be used as the priority for a
      98             :     // newly-created thread
      99             :     uint8_t calculate_thread_priority(priority_base base, int8_t priority) const;
     100             : 
     101             :     SchedulerThread _timer_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_timer_task, void), *this};
     102             :     SchedulerThread _io_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_io_task, void), *this};
     103             :     SchedulerThread _rcin_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_rcin_task, void), *this};
     104             :     SchedulerThread _uart_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_uart_task, void), *this};
     105             : 
     106             :     void _timer_task();
     107             :     void _io_task();
     108             :     void _rcin_task();
     109             :     void _uart_task();
     110             : 
     111             :     void _run_io();
     112             :     void _run_uarts();
     113             : 
     114             :     uint64_t _stopped_clock_usec;
     115             :     uint64_t _last_stack_debug_msec;
     116             :     pthread_t _main_ctx;
     117             : 
     118             :     Semaphore _io_semaphore;
     119             :     cpu_set_t _cpu_affinity;
     120             : };
     121             : 
     122             : }