From 48ea797a4790b43cde914aa96370dd062f23ccd9 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Sat, 6 Jan 2024 00:09:29 +0300
Subject: [PATCH] Make simulator read RC through real drone code

---
 gazebo/SBUS.h        | 5 ++++-
 gazebo/joystick.h    | 2 --
 gazebo/simulator.cpp | 2 +-
 3 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h
index cd5eb3c..b522574 100644
--- a/gazebo/SBUS.h
+++ b/gazebo/SBUS.h
@@ -9,5 +9,8 @@ class SBUS {
 public:
 	SBUS(HardwareSerial& bus) {};
 	void begin() {};
-	bool read(int16_t* channels, bool* failsafe, bool* lostFrame) { joystickGet(); return true; }; // NOTE: on the hardware channels is uint16_t
+	bool read(int16_t* channels, bool* failsafe, bool* lostFrame) { // NOTE: on the hardware channels is uint16_t
+		joystickGet();
+		return true;
+	};
 };
diff --git a/gazebo/joystick.h b/gazebo/joystick.h
index 84288a2..a5ade19 100644
--- a/gazebo/joystick.h
+++ b/gazebo/joystick.h
@@ -54,6 +54,4 @@ void joystickGet() {
 	}
 	channels[RC_CHANNEL_MODE] = SDL_JoystickGetButton(joystick, 0) ? 1 : 0;
 	controls[RC_CHANNEL_MODE] = channels[RC_CHANNEL_MODE];
-
-	normalizeRC();
 }
diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp
index e624ff9..5c6aa99 100644
--- a/gazebo/simulator.cpp
+++ b/gazebo/simulator.cpp
@@ -67,7 +67,7 @@ public:
 		acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));
 
 		// read rc
-		joystickGet();
+		readRC();
 		controls[RC_CHANNEL_MODE] = 1; // 0 acro, 1 stab
 		controls[RC_CHANNEL_AUX] = 1; // armed