From f6b10998961a9e2e067c03b7ddfb2daed9056648 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Thu, 18 May 2017 17:24:02 -0700
Subject: [PATCH] AP_HAL: move in tap esc implementation

This can be shared with Linux.
---
 .../utility}/RCOutput_Tap.cpp                 | 22 +++++++++----------
 .../utility}/RCOutput_Tap.h                   |  8 +++----
 libraries/AP_HAL_PX4/HAL_PX4_Class.cpp        |  3 ++-
 3 files changed, 15 insertions(+), 18 deletions(-)
 rename libraries/{AP_HAL_PX4 => AP_HAL/utility}/RCOutput_Tap.cpp (97%)
 rename libraries/{AP_HAL_PX4 => AP_HAL/utility}/RCOutput_Tap.h (95%)

diff --git a/libraries/AP_HAL_PX4/RCOutput_Tap.cpp b/libraries/AP_HAL/utility/RCOutput_Tap.cpp
similarity index 97%
rename from libraries/AP_HAL_PX4/RCOutput_Tap.cpp
rename to libraries/AP_HAL/utility/RCOutput_Tap.cpp
index bb47401a9a..23091b486d 100644
--- a/libraries/AP_HAL_PX4/RCOutput_Tap.cpp
+++ b/libraries/AP_HAL/utility/RCOutput_Tap.cpp
@@ -54,6 +54,7 @@
 
 #include "RCOutput_Tap.h"
 
+#include <errno.h>
 #include <fcntl.h>
 #include <stdio.h>
 #include <sys/stat.h>
@@ -61,8 +62,6 @@
 #include <termios.h>
 #include <unistd.h>
 
-#include <drivers/drv_hrt.h>
-
 #include <AP_Math/AP_Math.h>
 
 #define DEBUG 0
@@ -79,7 +78,6 @@ extern const AP_HAL::HAL &hal;
 /****** ESC data types ******/
 
 #define ESC_HAVE_CURRENT_SENSOR
-#define MIN_BOOT_TIME_USEC (550 * AP_USEC_PER_MSEC)
 
 static const uint8_t crcTable[256] = {
     0x00, 0xE7, 0x29, 0xCE, 0x52, 0xB5, 0x7B, 0x9C, 0xA4, 0x43, 0x8D, 0x6A,
@@ -137,7 +135,7 @@ static const uint8_t device_dir_map[] = {0, 1, 0, 1, 0, 1, 0, 1};
 
 #define MIN_BOOT_TIME_MSEC (550) // Minimum time to wait after Power on before sending commands
 
-namespace PX4 {
+namespace ap {
 
 /****** Run ***********/
 
@@ -304,7 +302,7 @@ enum PARSR_ESC_STATE {
 /****************************/
 }
 
-using namespace PX4;
+using namespace ap;
 
 void RCOutput_Tap::_uart_close()
 {
@@ -322,7 +320,7 @@ bool RCOutput_Tap::_uart_open()
     int termios_state = -1;
 
     if (_uart_fd < 0) {
-        PX4_ERR("failed to open uart device!");
+        ::fprintf(stderr, "failed to open uart device! %s\n", UART_DEVICE_PATH);
         return -1;
     }
 
@@ -353,16 +351,16 @@ bool RCOutput_Tap::_uart_open()
 
 void RCOutput_Tap::init()
 {
-    _perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout");
+    _perf_rcout = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "APM_rcout");
 
     if (!_uart_open()) {
         AP_HAL::panic("Unable to open " UART_DEVICE_PATH);
         return;
     }
 
-    hrt_abstime uptime_usec = hrt_absolute_time();
-    if (uptime_usec < MIN_BOOT_TIME_USEC) {
-        hal.scheduler->delay((MIN_BOOT_TIME_USEC - uptime_usec) / AP_USEC_PER_MSEC);
+    uint32_t now = AP_HAL::millis();
+    if (now < MIN_BOOT_TIME_MSEC) {
+        hal.scheduler->delay(MIN_BOOT_TIME_MSEC - now);
     }
 
     /* Issue Basic Config */
@@ -504,7 +502,7 @@ void RCOutput_Tap::push()
 {
     _corking = false;
 
-    perf_begin(_perf_rcout);
+    hal.util->perf_begin(_perf_rcout);
 
     uint16_t out[TAP_ESC_MAX_MOTOR_NUM];
     uint8_t motor_cnt = _channels_count;
@@ -578,7 +576,7 @@ void RCOutput_Tap::push()
 
     _next_channel_reply = (_next_channel_reply + 1) % _channels_count;
 
-    perf_end(_perf_rcout);
+    hal.util->perf_end(_perf_rcout);
 }
 
 #endif
diff --git a/libraries/AP_HAL_PX4/RCOutput_Tap.h b/libraries/AP_HAL/utility/RCOutput_Tap.h
similarity index 95%
rename from libraries/AP_HAL_PX4/RCOutput_Tap.h
rename to libraries/AP_HAL/utility/RCOutput_Tap.h
index a8b5c893c8..b101a40c5d 100644
--- a/libraries/AP_HAL_PX4/RCOutput_Tap.h
+++ b/libraries/AP_HAL/utility/RCOutput_Tap.h
@@ -49,11 +49,9 @@
  */
 #pragma once
 
-#include "AP_HAL_PX4.h"
-#include <systemlib/perf_counter.h>
-#include <uORB/topics/actuator_armed.h>
+#include <AP_HAL/Util.h>
 
-namespace PX4 {
+namespace ap {
 
 struct EscPacket;
 
@@ -86,7 +84,7 @@ private:
     bool _uart_open();
     void _uart_close();
 
-    perf_counter_t _perf_rcout;
+    AP_HAL::Util::perf_counter_t _perf_rcout;
 
     uint8_t _enabled_channels;
     bool _corking;
diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
index 03249681a6..e0fa502dde 100644
--- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
+++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
@@ -2,6 +2,7 @@
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
 
+#include <AP_HAL/utility/RCOutput_Tap.h>
 #include <AP_HAL_Empty/AP_HAL_Empty.h>
 #include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
 
@@ -13,7 +14,6 @@
 #include "Storage.h"
 #include "RCInput.h"
 #include "RCOutput.h"
-#include "RCOutput_Tap.h"
 #include "AnalogIn.h"
 #include "Util.h"
 #include "GPIO.h"
@@ -34,6 +34,7 @@
 #include <drivers/drv_hrt.h>
 
 using namespace PX4;
+using namespace ap;
 
 //static Empty::GPIO gpioDriver;