From 292b10197f58bd17d894472ca193071b9050050f Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Mon, 23 Dec 2024 02:04:22 +0300
Subject: [PATCH] Improve logic of passing channels data in simulated SBUS
 Return the data the same way as on the real drone without touching channels
 global vairable

---
 gazebo/SBUS.h     | 6 ++----
 gazebo/joystick.h | 6 +++---
 2 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h
index f97fa12..cc32a2c 100644
--- a/gazebo/SBUS.h
+++ b/gazebo/SBUS.h
@@ -14,12 +14,10 @@ public:
 	SBUS(HardwareSerial& bus, const bool inv = true) {};
 	SBUS(HardwareSerial& bus, const int8_t rxpin, const int8_t txpin, const bool inv = true) {};
 	void begin() {};
-	bool read() { return joystickGet(); };
+	bool read() { return true; };
 	SBUSData data() {
 		SBUSData data;
-		for (uint8_t i = 0; i < 16; i++) {
-			data.ch[i] = channels[i];
-		}
+		joystickGet(data.ch);
 		return data;
 	};
 };
diff --git a/gazebo/joystick.h b/gazebo/joystick.h
index 5294d77..088e6d4 100644
--- a/gazebo/joystick.h
+++ b/gazebo/joystick.h
@@ -41,7 +41,7 @@ void joystickInit() {
 	memcpy(channelMax, channelMaxOverride, sizeof(channelMaxOverride));
 }
 
-bool joystickGet() {
+bool joystickGet(int16_t ch[16]) {
 	if (!joystickInitialized) {
 		joystickInit();
 		return false;
@@ -49,8 +49,8 @@ bool joystickGet() {
 
 	SDL_JoystickUpdate();
 
-	for (uint8_t i = 0; i < 8; i++) {
-		channels[i] = SDL_JoystickGetAxis(joystick, i);
+	for (uint8_t i = 0; i < sizeof(channels) / sizeof(channels[0]); i++) {
+		ch[i] = SDL_JoystickGetAxis(joystick, i);
 	}
 	return true;
 }