From d0b3b926c081890c215315d1b4936f38dc55bbbe Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 19 Mar 2021 12:40:56 +1100
Subject: [PATCH] AP_HAL_Linux: split out a calculate_thread_priority method

---
 libraries/AP_HAL_Linux/Scheduler.cpp | 26 +++++++++++++++++---------
 libraries/AP_HAL_Linux/Scheduler.h   |  4 ++++
 2 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp
index 362defab77..f2780d852e 100644
--- a/libraries/AP_HAL_Linux/Scheduler.cpp
+++ b/libraries/AP_HAL_Linux/Scheduler.cpp
@@ -348,16 +348,9 @@ void Scheduler::teardown()
     _uart_thread.join();
 }
 
-/*
-  create a new thread
-*/
-bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
+// calculates an integer to be used as the priority for a newly-created thread
+uint8_t Scheduler::calculate_thread_priority(priority_base base, int8_t priority) const
 {
-    Thread *thread = new Thread{(Thread::task_t)proc};
-    if (!thread) {
-        return false;
-    }
-
     uint8_t thread_priority = APM_LINUX_IO_PRIORITY;
     static const struct {
         priority_base base;
@@ -382,6 +375,21 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_
         }
     }
 
+    return thread_priority;
+}
+
+/*
+  create a new thread
+*/
+bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
+{
+    Thread *thread = new Thread{(Thread::task_t)proc};
+    if (!thread) {
+        return false;
+    }
+
+    const uint8_t thread_priority = calculate_thread_priority(base, priority);
+
     // Add 256k to HAL-independent requested stack size
     thread->set_stack_size(256 * 1024 + stack_size);
 
diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h
index f78cf97c95..4e0f756476 100644
--- a/libraries/AP_HAL_Linux/Scheduler.h
+++ b/libraries/AP_HAL_Linux/Scheduler.h
@@ -86,6 +86,10 @@ private:
     AP_HAL::MemberProc _io_proc[LINUX_SCHEDULER_MAX_IO_PROCS];
     uint8_t _num_io_procs;
 
+    // calculates an integer to be used as the priority for a
+    // newly-created thread
+    uint8_t calculate_thread_priority(priority_base base, int8_t priority) const;
+
     SchedulerThread _timer_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_timer_task, void), *this};
     SchedulerThread _io_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_io_task, void), *this};
     SchedulerThread _rcin_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_rcin_task, void), *this};