mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_FLYMAPLE: implement new AP_HAL functions
Implement the new AP_HAL functions and use them in the Scheduler when possible. Because the functions are in a namespace, there's no need to do the define/undef trick and avoid the globals millis() and micros() provided by libmaple.
This commit is contained in:
parent
8db8b9b355
commit
79721feb14
|
@ -17,11 +17,7 @@
|
|||
|
||||
#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.
|
||||
|
@ -81,10 +77,10 @@ void FLYMAPLEScheduler::init(void* machtnichts)
|
|||
// This function may calls the _delay_cb to use up time
|
||||
void FLYMAPLEScheduler::delay(uint16_t ms)
|
||||
{
|
||||
uint32_t start = libmaple_micros();
|
||||
uint32_t start = AP_HAL::micros();
|
||||
|
||||
while (ms > 0) {
|
||||
while ((libmaple_micros() - start) >= 1000) {
|
||||
while ((AP_HAL::micros() - start) >= 1000) {
|
||||
ms--;
|
||||
if (ms == 0) break;
|
||||
start += 1000;
|
||||
|
@ -98,23 +94,19 @@ void FLYMAPLEScheduler::delay(uint16_t ms)
|
|||
}
|
||||
|
||||
uint32_t FLYMAPLEScheduler::millis() {
|
||||
return libmaple_millis();
|
||||
return AP_HAL::millis();
|
||||
}
|
||||
|
||||
uint32_t FLYMAPLEScheduler::micros() {
|
||||
return libmaple_micros();
|
||||
return AP_HAL::micros();
|
||||
}
|
||||
|
||||
uint64_t FLYMAPLEScheduler::millis64() {
|
||||
return millis();
|
||||
return AP_HAL::millis64();
|
||||
}
|
||||
|
||||
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;
|
||||
return AP_HAL::micros64();
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::delay_microseconds(uint16_t us)
|
||||
|
@ -224,7 +216,7 @@ bool FLYMAPLEScheduler::system_initializing() {
|
|||
void FLYMAPLEScheduler::system_initialized()
|
||||
{
|
||||
if (_initialized) {
|
||||
panic("PANIC: scheduler::system_initialized called"
|
||||
AP_HAL::panic("PANIC: scheduler::system_initialized called"
|
||||
"more than once");
|
||||
}
|
||||
_initialized = true;
|
||||
|
|
|
@ -1,4 +1,13 @@
|
|||
#include <stdarg.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/system.h>
|
||||
#include <AP_HAL_FLYMAPLE/Scheduler.h>
|
||||
|
||||
#include "FlymapleWirish.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
namespace AP_HAL {
|
||||
|
||||
|
@ -6,4 +15,47 @@ void init()
|
|||
{
|
||||
}
|
||||
|
||||
void 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;
|
||||
|
||||
hal.scheduler->suspend_timer_procs();
|
||||
|
||||
va_start(ap, errormsg);
|
||||
hal.console->vprintf(errormsg, ap);
|
||||
va_end(ap);
|
||||
hal.console->printf("\n");
|
||||
|
||||
for(;;);
|
||||
}
|
||||
|
||||
uint32_t micros()
|
||||
{
|
||||
// Use function provided by libmaple.
|
||||
return ::micros();
|
||||
}
|
||||
|
||||
uint32_t millis()
|
||||
{
|
||||
// Use function provided by libmaple.
|
||||
return ::millis();
|
||||
}
|
||||
|
||||
uint64_t millis64()
|
||||
{
|
||||
return millis();
|
||||
}
|
||||
|
||||
uint64_t micros64()
|
||||
{
|
||||
// this is slow, but solves the problem with logging uint64_t timestamps
|
||||
uint64_t ret = millis();
|
||||
ret *= 1000ULL;
|
||||
ret += micros() % 1000;
|
||||
return ret;
|
||||
}
|
||||
|
||||
} // namespace AP_HAL
|
||||
|
|
Loading…
Reference in New Issue