mirror of https://github.com/ArduPilot/ardupilot
256 lines
6.8 KiB
C++
256 lines
6.8 KiB
C++
// Scheduler.cpp
|
|
//
|
|
// Flymaple Scheduler.
|
|
// We use systick interrupt for the 1kHz ordinary timers.
|
|
// We use a slightly higher priority HardwareTimer 2 for the failsafe callbacks
|
|
// so a hung timer wont prevent the failsafe timer interrupt running
|
|
//
|
|
// Use of noInterrupts()/interrupts() on FLymaple ARM processor.
|
|
// Please see the notes in FlymaplePortingNotes.txt in this directory for
|
|
// information about disabling interrupts on Flymaple
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
|
|
|
#include "Scheduler.h"
|
|
|
|
#include <stdarg.h>
|
|
|
|
#define millis libmaple_millis
|
|
#define micros libmaple_micros
|
|
#include "FlymapleWirish.h"
|
|
#undef millis
|
|
#undef micros
|
|
|
|
// Flymaple: Force init to be called *first*, i.e. before static object allocation.
|
|
// Otherwise, statically allocated objects (eg SerialUSB) that need libmaple may fail.
|
|
__attribute__((constructor)) void premain() {
|
|
init();
|
|
}
|
|
|
|
// Not declared in any libmaple headers :-(
|
|
extern "C"
|
|
{
|
|
void systick_attach_callback(void (*callback)(void));
|
|
};
|
|
|
|
// Use Maple hardware timer for 1khz failsafe timer
|
|
// Caution, this must agree with the interrupt number passed to
|
|
// nvic_irq_set_priority
|
|
static HardwareTimer _failsafe_timer(2);
|
|
|
|
using namespace AP_HAL_FLYMAPLE_NS;
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
AP_HAL::Proc FLYMAPLEScheduler::_failsafe = NULL;
|
|
volatile bool FLYMAPLEScheduler::_timer_suspended = false;
|
|
volatile bool FLYMAPLEScheduler::_timer_event_missed = false;
|
|
volatile bool FLYMAPLEScheduler::_in_timer_proc = false;
|
|
AP_HAL::MemberProc FLYMAPLEScheduler::_timer_proc[FLYMAPLE_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
|
|
uint8_t FLYMAPLEScheduler::_num_timer_procs = 0;
|
|
|
|
FLYMAPLEScheduler::FLYMAPLEScheduler() :
|
|
_delay_cb(NULL),
|
|
_min_delay_cb_ms(65535),
|
|
_initialized(false)
|
|
{}
|
|
|
|
void FLYMAPLEScheduler::init(void* machtnichts)
|
|
{
|
|
delay_us(2000000); // Wait for startup so we have time to connect a new USB console
|
|
// 1kHz interrupts from systick for normal timers
|
|
systick_attach_callback(_timer_procs_timer_event);
|
|
|
|
// Set up Maple hardware timer for 1khz failsafe timer
|
|
// ref: http://leaflabs.com/docs/lang/api/hardwaretimer.html#lang-hardwaretimer
|
|
_failsafe_timer.pause();
|
|
_failsafe_timer.setPeriod(1000); // 1000us = 1kHz
|
|
_failsafe_timer.setChannelMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);// Set up an interrupt on channel 1
|
|
_failsafe_timer.setCompare(TIMER_CH1, 1); // Interrupt 1 count after each update
|
|
_failsafe_timer.attachInterrupt(TIMER_CH1, _failsafe_timer_event);
|
|
_failsafe_timer.refresh();// Refresh the timer's count, prescale, and overflow
|
|
_failsafe_timer.resume(); // Start the timer counting
|
|
// We run this timer at a higher priority, so that a broken timer handler (ie one that hangs)
|
|
// will not prevent the failsafe timer interrupt.
|
|
// Caution: the timer number must agree with the HardwareTimer number
|
|
nvic_irq_set_priority(NVIC_TIMER2, 0x14);
|
|
}
|
|
|
|
// This function may calls the _delay_cb to use up time
|
|
void FLYMAPLEScheduler::delay(uint16_t ms)
|
|
{
|
|
uint32_t start = libmaple_micros();
|
|
|
|
while (ms > 0) {
|
|
while ((libmaple_micros() - start) >= 1000) {
|
|
ms--;
|
|
if (ms == 0) break;
|
|
start += 1000;
|
|
}
|
|
if (_min_delay_cb_ms <= ms) {
|
|
if (_delay_cb) {
|
|
_delay_cb();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
uint32_t FLYMAPLEScheduler::millis() {
|
|
return libmaple_millis();
|
|
}
|
|
|
|
uint32_t FLYMAPLEScheduler::micros() {
|
|
return libmaple_micros();
|
|
}
|
|
|
|
uint64_t FLYMAPLEScheduler::millis64() {
|
|
return millis();
|
|
}
|
|
|
|
uint64_t FLYMAPLEScheduler::micros64() {
|
|
// this is slow, but solves the problem with logging uint64_t timestamps
|
|
uint64_t ret = millis();
|
|
ret *= 1000ULL;
|
|
ret += micros() % 1000;
|
|
return ret;
|
|
}
|
|
|
|
void FLYMAPLEScheduler::delay_microseconds(uint16_t us)
|
|
{
|
|
delay_us(us);
|
|
}
|
|
|
|
void FLYMAPLEScheduler::register_delay_callback(AP_HAL::Proc proc, uint16_t min_time_ms)
|
|
{
|
|
_delay_cb = proc;
|
|
_min_delay_cb_ms = min_time_ms;
|
|
}
|
|
|
|
void FLYMAPLEScheduler::register_timer_process(AP_HAL::MemberProc proc)
|
|
{
|
|
for (int i = 0; i < _num_timer_procs; i++) {
|
|
if (_timer_proc[i] == proc) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (_num_timer_procs < FLYMAPLE_SCHEDULER_MAX_TIMER_PROCS) {
|
|
/* this write to _timer_proc can be outside the critical section
|
|
* because that memory won't be used until _num_timer_procs is
|
|
* incremented. */
|
|
_timer_proc[_num_timer_procs] = proc;
|
|
/* _num_timer_procs is used from interrupt, and multiple bytes long. */
|
|
noInterrupts();
|
|
_num_timer_procs++;
|
|
interrupts();
|
|
}
|
|
}
|
|
|
|
void FLYMAPLEScheduler::register_io_process(AP_HAL::MemberProc k)
|
|
{
|
|
// IO processes not supported on FLYMAPLE
|
|
}
|
|
|
|
void FLYMAPLEScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
|
|
{
|
|
/* XXX Assert period_us == 1000 */
|
|
_failsafe = failsafe;
|
|
}
|
|
|
|
void FLYMAPLEScheduler::suspend_timer_procs()
|
|
{
|
|
_timer_suspended = true;
|
|
}
|
|
|
|
void FLYMAPLEScheduler::resume_timer_procs()
|
|
{
|
|
_timer_suspended = false;
|
|
if (_timer_event_missed == true) {
|
|
_run_timer_procs(false);
|
|
_timer_event_missed = false;
|
|
}
|
|
}
|
|
|
|
bool FLYMAPLEScheduler::in_timerprocess() {
|
|
return _in_timer_proc;
|
|
}
|
|
|
|
void FLYMAPLEScheduler::_timer_procs_timer_event() {
|
|
_run_timer_procs(true);
|
|
}
|
|
|
|
// Called by HardwareTimer when a failsafe timer event occurs
|
|
void FLYMAPLEScheduler::_failsafe_timer_event()
|
|
{
|
|
// run the failsafe, if one is setup
|
|
if (_failsafe != NULL)
|
|
_failsafe();
|
|
}
|
|
|
|
void FLYMAPLEScheduler::begin_atomic()
|
|
{
|
|
noInterrupts();
|
|
}
|
|
|
|
void FLYMAPLEScheduler::end_atomic()
|
|
{
|
|
interrupts();
|
|
}
|
|
|
|
void FLYMAPLEScheduler::_run_timer_procs(bool called_from_isr)
|
|
{
|
|
_in_timer_proc = true;
|
|
|
|
if (!_timer_suspended) {
|
|
// now call the timer based drivers
|
|
for (int i = 0; i < _num_timer_procs; i++) {
|
|
if (_timer_proc[i]) {
|
|
_timer_proc[i]();
|
|
}
|
|
}
|
|
} else if (called_from_isr) {
|
|
_timer_event_missed = true;
|
|
}
|
|
|
|
_in_timer_proc = false;
|
|
}
|
|
|
|
bool FLYMAPLEScheduler::system_initializing() {
|
|
return !_initialized;
|
|
}
|
|
|
|
void FLYMAPLEScheduler::system_initialized()
|
|
{
|
|
if (_initialized) {
|
|
panic("PANIC: scheduler::system_initialized called"
|
|
"more than once");
|
|
}
|
|
_initialized = true;
|
|
}
|
|
|
|
void FLYMAPLEScheduler::panic(const char *errormsg, ...) {
|
|
/* Suspend timer processes. We still want the timer event to go off
|
|
* to run the _failsafe code, however. */
|
|
// REVISIT: not tested on FLYMAPLE
|
|
va_list ap;
|
|
|
|
_timer_suspended = true;
|
|
|
|
va_start(ap, errormsg);
|
|
hal.console->vprintf(errormsg, ap);
|
|
va_end(ap);
|
|
hal.console->printf("\n");
|
|
|
|
for(;;);
|
|
}
|
|
|
|
void FLYMAPLEScheduler::reboot(bool hold_in_bootloader) {
|
|
hal.uartA->println("GOING DOWN FOR A REBOOT\r\n");
|
|
hal.scheduler->delay(100);
|
|
nvic_sys_reset();
|
|
}
|
|
|
|
#endif
|