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

AP_HAL_Linux: added scheduler hooks for raspilot

Browse Source
This commit is contained in:
raspilot 2015-08-18 12:39:02 +10:00 committed by Andrew Tridgell
parent 25cddbcbaa
commit 30a2fe0857
1 changed files with 31 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/AP_HAL_Linux/Scheduler.cpp
View File

@ -8,6 +8,7 @@
#include "UARTDriver.h" #include "UARTDriver.h"
#include "Util.h" #include "Util.h"
#include "SPIUARTDriver.h" #include "SPIUARTDriver.h"
#include "RPIOUARTDriver.h"
#include <sys/time.h> #include <sys/time.h>
#include <poll.h> #include <poll.h>
#include <unistd.h> #include <unistd.h>
@ -29,7 +30,7 @@ extern const AP_HAL::HAL& hal;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
#define APM_LINUX_UART_PERIOD 10000 #define APM_LINUX_UART_PERIOD 10000
#define APM_LINUX_RCIN_PERIOD 500 #define APM_LINUX_RCIN_PERIOD 500
#define APM_LINUX_TONEALARM_PERIOD 10000 #define APM_LINUX_TONEALARM_PERIOD 10000
#define APM_LINUX_IO_PERIOD 20000 #define APM_LINUX_IO_PERIOD 20000
#else #else
@ -143,7 +144,7 @@ void LinuxScheduler::delay(uint16_t ms)
return; return;
} }
uint64_t start = millis64(); uint64_t start = millis64();
while ((millis64() - start) < ms) { while ((millis64() - start) < ms) {
// this yields the CPU to other apps // this yields the CPU to other apps
_microsleep(1000); _microsleep(1000);
@ -155,36 +156,36 @@ void LinuxScheduler::delay(uint16_t ms)
} }
} }
uint64_t LinuxScheduler::millis64() uint64_t LinuxScheduler::millis64()
{ {
if (stopped_clock_usec) { if (stopped_clock_usec) {
return stopped_clock_usec/1000; return stopped_clock_usec/1000;
} }
struct timespec ts; struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts); clock_gettime(CLOCK_MONOTONIC, &ts);
return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
(_sketch_start_time.tv_sec + (_sketch_start_time.tv_sec +
(_sketch_start_time.tv_nsec*1.0e-9))); (_sketch_start_time.tv_nsec*1.0e-9)));
} }
uint64_t LinuxScheduler::micros64() uint64_t LinuxScheduler::micros64()
{ {
if (stopped_clock_usec) { if (stopped_clock_usec) {
return stopped_clock_usec; return stopped_clock_usec;
} }
struct timespec ts; struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts); clock_gettime(CLOCK_MONOTONIC, &ts);
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
(_sketch_start_time.tv_sec + (_sketch_start_time.tv_sec +
(_sketch_start_time.tv_nsec*1.0e-9))); (_sketch_start_time.tv_nsec*1.0e-9)));
} }
uint32_t LinuxScheduler::millis() uint32_t LinuxScheduler::millis()
{ {
return millis64() & 0xFFFFFFFF; return millis64() & 0xFFFFFFFF;
} }
uint32_t LinuxScheduler::micros() uint32_t LinuxScheduler::micros()
{ {
return micros64() & 0xFFFFFFFF; return micros64() & 0xFFFFFFFF;
} }
@ -204,7 +205,7 @@ void LinuxScheduler::register_delay_callback(AP_HAL::Proc proc,
_min_delay_cb_ms = min_time_ms; _min_delay_cb_ms = min_time_ms;
} }
void LinuxScheduler::register_timer_process(AP_HAL::MemberProc proc) void LinuxScheduler::register_timer_process(AP_HAL::MemberProc proc)
{ {
for (uint8_t i = 0; i < _num_timer_procs; i++) { for (uint8_t i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i] == proc) { if (_timer_proc[i] == proc) {
@ -220,7 +221,7 @@ void LinuxScheduler::register_timer_process(AP_HAL::MemberProc proc)
} }
} }
void LinuxScheduler::register_io_process(AP_HAL::MemberProc proc) void LinuxScheduler::register_io_process(AP_HAL::MemberProc proc)
{ {
for (uint8_t i = 0; i < _num_io_procs; i++) { for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) { if (_io_proc[i] == proc) {
@ -269,6 +270,15 @@ void LinuxScheduler::_run_timers(bool called_from_timer_thread)
_timer_proc[i](); _timer_proc[i]();
} }
} }
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
//SPI UART use SPI
if (!((LinuxRPIOUARTDriver *)hal.uartC)->isExternal() )
{
((LinuxRPIOUARTDriver *)hal.uartC)->_timer_tick();
}
#endif
_timer_semaphore.give(); _timer_semaphore.give();
// and the failsafe, if one is setup // and the failsafe, if one is setup
@ -349,7 +359,15 @@ void *LinuxScheduler::_uart_thread(void* arg)
// process any pending serial bytes // process any pending serial bytes
((LinuxUARTDriver *)hal.uartA)->_timer_tick(); ((LinuxUARTDriver *)hal.uartA)->_timer_tick();
((LinuxUARTDriver *)hal.uartB)->_timer_tick(); ((LinuxUARTDriver *)hal.uartB)->_timer_tick();
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
//SPI UART not use SPI
if ( ((LinuxRPIOUARTDriver *)hal.uartC)->isExternal() )
{
((LinuxRPIOUARTDriver *)hal.uartC)->_timer_tick();
}
#else
((LinuxUARTDriver *)hal.uartC)->_timer_tick(); ((LinuxUARTDriver *)hal.uartC)->_timer_tick();
#endif
((LinuxUARTDriver *)hal.uartE)->_timer_tick(); ((LinuxUARTDriver *)hal.uartE)->_timer_tick();
} }
return NULL; return NULL;
@ -390,7 +408,7 @@ void *LinuxScheduler::_io_thread(void* arg)
return NULL; return NULL;
} }
void LinuxScheduler::panic(const prog_char_t *errormsg) void LinuxScheduler::panic(const prog_char_t *errormsg)
{ {
write(1, errormsg, strlen(errormsg)); write(1, errormsg, strlen(errormsg));
write(1, "\n", 1); write(1, "\n", 1);
@ -399,7 +417,7 @@ void LinuxScheduler::panic(const prog_char_t *errormsg)
exit(1); exit(1);
} }
bool LinuxScheduler::in_timerprocess() bool LinuxScheduler::in_timerprocess()
{ {
return _in_timer_proc; return _in_timer_proc;
} }
@ -422,7 +440,7 @@ void LinuxScheduler::system_initialized()
_initialized = true; _initialized = true;
} }
void LinuxScheduler::reboot(bool hold_in_bootloader) void LinuxScheduler::reboot(bool hold_in_bootloader)
{ {
exit(1); exit(1);
} }