From 79721feb14ca9eb99205b5cd8d97d72076af3abf Mon Sep 17 00:00:00 2001
From: Caio Marcelo de Oliveira Filho <caio.oliveira@intel.com>
Date: Wed, 11 Nov 2015 12:22:49 -0200
Subject: [PATCH] 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.
---
 libraries/AP_HAL_FLYMAPLE/Scheduler.cpp | 22 ++++-------
 libraries/AP_HAL_FLYMAPLE/system.cpp    | 52 +++++++++++++++++++++++++
 2 files changed, 59 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp
index 5d94274ece..29950a3fb8 100644
--- a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp
+++ b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp
@@ -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;
diff --git a/libraries/AP_HAL_FLYMAPLE/system.cpp b/libraries/AP_HAL_FLYMAPLE/system.cpp
index 65467f3997..e115460d46 100644
--- a/libraries/AP_HAL_FLYMAPLE/system.cpp
+++ b/libraries/AP_HAL_FLYMAPLE/system.cpp
@@ -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