From 43be27c43dc309e7efa9b7ccddbd061fed8ebbfe Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Fri, 27 Dec 2024 15:34:33 +0300
Subject: [PATCH] Fix joystick work in simulation Logic was broken as
 joystickGet never got called

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

diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h
index c687239..f57db7f 100644
--- a/gazebo/SBUS.h
+++ b/gazebo/SBUS.h
@@ -14,7 +14,7 @@ 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 joystickInitialized; };
+	bool read() { return joystickInit(); };
 	SBUSData data() {
 		SBUSData data;
 		joystickGet(data.ch);
diff --git a/gazebo/joystick.h b/gazebo/joystick.h
index 8c8ba84..3833340 100644
--- a/gazebo/joystick.h
+++ b/gazebo/joystick.h
@@ -15,9 +15,12 @@
 #define RC_CHANNEL_MODE 4
 
 SDL_Joystick *joystick;
-bool joystickInitialized = false, warnShown = false;
 
-void joystickInit() {
+bool joystickInit() {
+	static bool joystickInitialized = false;
+	static bool warnShown = false;
+	if (joystickInitialized) return true;
+
 	SDL_Init(SDL_INIT_JOYSTICK);
 	joystick = SDL_JoystickOpen(0);
 	if (joystick != NULL) {
@@ -27,14 +30,10 @@ void joystickInit() {
 		gzwarn << "Joystick not found, begin waiting for joystick..." << std::endl;
 		warnShown = true;
 	}
+	return joystickInitialized;
 }
 
 bool joystickGet(int16_t ch[16]) {
-	if (!joystickInitialized) {
-		joystickInit();
-		return false;
-	}
-
 	SDL_JoystickUpdate();
 
 	for (uint8_t i = 0; i < sizeof(channels) / sizeof(channels[0]); i++) {