From 62523e374af216e1ed49ae24d08c94293492dfd5 Mon Sep 17 00:00:00 2001
From: mattp256 <11513260+mattp256@users.noreply.github.com>
Date: Thu, 6 Feb 2025 23:54:41 -0600
Subject: [PATCH] AR_Motors: fix brushed motor support for omni vehicles

Fixes an issue where omni motors (BrushedWithRelay/BrushedBiplor)
are not configured correctly due to initialization order. setup_omni()
must execute first so that _motors_num is set correctly when
setup_pwm_type() uses it to determine which motors are in use.
---
 libraries/AR_Motors/AP_MotorsUGV.cpp | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp
index 71536a7ad51ec9..599e2816c5312c 100644
--- a/libraries/AR_Motors/AP_MotorsUGV.cpp
+++ b/libraries/AR_Motors/AP_MotorsUGV.cpp
@@ -132,6 +132,11 @@ void AP_MotorsUGV::init(uint8_t frtype)
 {
     _frame_type = frame_type(frtype);
 
+    // setup for omni vehicles
+    if (_frame_type != FRAME_TYPE_UNDEFINED) {
+        setup_omni();
+    }
+    
     // setup servo output
     setup_servo_output();
 
@@ -141,10 +146,6 @@ void AP_MotorsUGV::init(uint8_t frtype)
     // set safety output
     setup_safety_output();
 
-    // setup for omni vehicles
-    if (_frame_type != FRAME_TYPE_UNDEFINED) {
-        setup_omni();
-    }
 }
 
 bool AP_MotorsUGV::get_legacy_relay_index(int8_t &index1, int8_t &index2, int8_t &index3, int8_t &index4) const