From 252ab82c97c6cc729989b147a3d6dbe9fce3a48a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 1 Aug 2018 18:56:50 +1000
Subject: [PATCH] HAL_PX4: update to match plane3.9.0beta6

---
 libraries/AP_HAL_PX4/GPIO.cpp          | 11 ----
 libraries/AP_HAL_PX4/GPIO.h            |  1 -
 libraries/AP_HAL_PX4/HAL_PX4_Class.cpp |  7 +++
 libraries/AP_HAL_PX4/Scheduler.cpp     | 70 ++++++++++++++++++++++++--
 libraries/AP_HAL_PX4/Scheduler.h       |  7 +++
 libraries/AP_HAL_PX4/Util.cpp          |  3 +-
 6 files changed, 81 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_HAL_PX4/GPIO.cpp b/libraries/AP_HAL_PX4/GPIO.cpp
index ad78f4bb5e..56756505ca 100644
--- a/libraries/AP_HAL_PX4/GPIO.cpp
+++ b/libraries/AP_HAL_PX4/GPIO.cpp
@@ -91,17 +91,6 @@ void PX4GPIO::pinMode(uint8_t pin, uint8_t output)
     }
 }
 
-int8_t PX4GPIO::analogPinToDigitalPin(uint8_t pin)
-{
-    switch (pin) {
-    case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5):
-        // the only pins that can be mapped are the FMU servo rail pins */
-        return pin;
-    }
-    return -1;
-}
-
-
 uint8_t PX4GPIO::read(uint8_t pin) {
     switch (pin) {
 
diff --git a/libraries/AP_HAL_PX4/GPIO.h b/libraries/AP_HAL_PX4/GPIO.h
index 82ebadec4c..9a752b2edb 100644
--- a/libraries/AP_HAL_PX4/GPIO.h
+++ b/libraries/AP_HAL_PX4/GPIO.h
@@ -37,7 +37,6 @@ public:
     PX4GPIO();
     void    init() override;
     void    pinMode(uint8_t pin, uint8_t output) override;
-    int8_t  analogPinToDigitalPin(uint8_t pin) override;
     uint8_t read(uint8_t pin) override;
     void    write(uint8_t pin, uint8_t value) override;
     void    toggle(uint8_t pin) override;
diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
index ee3622f56f..0c904966de 100644
--- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
+++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
@@ -76,6 +76,13 @@ static PX4::SPIDeviceManager spi_mgr_instance;
 #define UARTD_DEFAULT_DEVICE "/dev/null"
 #define UARTE_DEFAULT_DEVICE "/dev/null"
 #define UARTF_DEFAULT_DEVICE "/dev/null"
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
+#define UARTB_DEFAULT_DEVICE "/dev/ttyS3"
+#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
+#define UARTD_DEFAULT_DEVICE "/dev/ttyS1"
+#define UARTE_DEFAULT_DEVICE "/dev/null"
+#define UARTF_DEFAULT_DEVICE "/dev/null"
 #else
 #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
 #define UARTB_DEFAULT_DEVICE "/dev/ttyS3"
diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp
index 668d141c81..400f9bf018 100644
--- a/libraries/AP_HAL_PX4/Scheduler.cpp
+++ b/libraries/AP_HAL_PX4/Scheduler.cpp
@@ -172,17 +172,13 @@ void PX4Scheduler::delay_microseconds_boost(uint16_t usec)
 
 void PX4Scheduler::delay(uint16_t ms)
 {
-    if (!in_main_thread()) {
-        ::printf("ERROR: delay() from timer process\n");
-        return;
-    }
     perf_begin(_perf_delay);
     uint64_t start = AP_HAL::micros64();
 
     while ((AP_HAL::micros64() - start)/1000 < ms &&
            !_px4_thread_should_exit) {
         delay_microseconds_semaphore(1000);
-        if (_min_delay_cb_ms <= ms) {
+        if (in_main_thread() && _min_delay_cb_ms <= ms) {
             call_delay_cb();
         }
     }
@@ -446,4 +442,68 @@ void PX4Scheduler::restore_interrupts(void *state)
     irqrestore((irqstate_t)(uintptr_t)state);
 }
 
+/*
+  trampoline for thread create
+*/
+void *PX4Scheduler::thread_create_trampoline(void *ctx)
+{
+    AP_HAL::MemberProc *t = (AP_HAL::MemberProc *)ctx;
+    (*t)();
+    free(t);
+    return nullptr;
+}
+
+/*
+  create a new thread
+*/
+bool PX4Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
+{
+    // take a copy of the MemberProc, it is freed after thread exits
+    AP_HAL::MemberProc *tproc = (AP_HAL::MemberProc *)malloc(sizeof(proc));
+    if (!tproc) {
+        return false;
+    }
+    *tproc = proc;
+
+    uint8_t thread_priority = APM_IO_PRIORITY;
+    static const struct {
+        priority_base base;
+        uint8_t p;
+    } priority_map[] = {
+        { PRIORITY_BOOST, APM_MAIN_PRIORITY_BOOST},
+        { PRIORITY_MAIN, APM_MAIN_PRIORITY},
+        { PRIORITY_SPI, APM_SPI_PRIORITY},
+        { PRIORITY_I2C, APM_I2C_PRIORITY},
+        { PRIORITY_CAN, APM_CAN_PRIORITY},
+        { PRIORITY_TIMER, APM_TIMER_PRIORITY},
+        { PRIORITY_RCIN, APM_TIMER_PRIORITY},
+        { PRIORITY_IO, APM_IO_PRIORITY},
+        { PRIORITY_UART, APM_UART_PRIORITY},
+        { PRIORITY_STORAGE, APM_STORAGE_PRIORITY},
+    };
+    for (uint8_t i=0; i<ARRAY_SIZE(priority_map); i++) {
+        if (priority_map[i].base == base) {
+            thread_priority = constrain_int16(priority_map[i].p + priority, 1, APM_MAX_PRIORITY);
+            break;
+        }
+    }
+    pthread_t thread;
+    pthread_attr_t thread_attr;
+    struct sched_param param;
+
+    pthread_attr_init(&thread_attr);
+    pthread_attr_setstacksize(&thread_attr, stack_size);
+
+    param.sched_priority = thread_priority;
+    (void)pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+
+    if (pthread_create(&thread, &thread_attr, thread_create_trampoline, tproc) != 0) {
+        free(tproc);
+        return false;
+    }
+    pthread_setname_np(thread, name);
+    return true;
+}
+
 #endif
diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h
index e9c0352763..58667a2ed2 100644
--- a/libraries/AP_HAL_PX4/Scheduler.h
+++ b/libraries/AP_HAL_PX4/Scheduler.h
@@ -10,6 +10,7 @@
 
 #define PX4_SCHEDULER_MAX_TIMER_PROCS 8
 
+#define APM_MAX_PRIORITY        243
 #define APM_MAIN_PRIORITY_BOOST 241
 #define APM_MAIN_PRIORITY       180
 #define APM_TIMER_PRIORITY      181
@@ -72,6 +73,11 @@ public:
       restore interrupt state from disable_interrupts_save()
      */
     void restore_interrupts(void *) override;
+
+    /*
+      create a new thread
+     */
+    bool thread_create(AP_HAL::MemberProc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) override;
     
 private:
     bool _initialized;
@@ -115,5 +121,6 @@ private:
     perf_counter_t  _perf_io_timers;
     perf_counter_t  _perf_storage_timer;
     perf_counter_t  _perf_delay;
+    static void *thread_create_trampoline(void *ctx);    
 };
 #endif
diff --git a/libraries/AP_HAL_PX4/Util.cpp b/libraries/AP_HAL_PX4/Util.cpp
index 34896d74c0..b07ecea056 100644
--- a/libraries/AP_HAL_PX4/Util.cpp
+++ b/libraries/AP_HAL_PX4/Util.cpp
@@ -126,7 +126,8 @@ bool PX4Util::get_system_id(char buf[40])
              board_type,
              (unsigned)serialid[0], (unsigned)serialid[1], (unsigned)serialid[2], (unsigned)serialid[3], 
              (unsigned)serialid[4], (unsigned)serialid[5], (unsigned)serialid[6], (unsigned)serialid[7], 
-             (unsigned)serialid[8], (unsigned)serialid[9], (unsigned)serialid[10],(unsigned)serialid[11]); 
+             (unsigned)serialid[8], (unsigned)serialid[9], (unsigned)serialid[10],(unsigned)serialid[11]);
+    buf[39] = 0;
     return true;
 }