From 646fa46f6bbd1b811bd005975b3b3319256a0322 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Sun, 17 Mar 2024 02:29:37 +0300
Subject: [PATCH] Use FlixPeriph library for SBUS

---
 Makefile      |  1 -
 docs/build.md |  5 ++---
 flix/flix.ino |  2 +-
 flix/rc.ino   | 10 ++++------
 gazebo/SBUS.h | 15 +++++++++++----
 gazebo/flix.h |  2 +-
 6 files changed, 19 insertions(+), 16 deletions(-)

diff --git a/Makefile b/Makefile
index f1434a8..45a1edf 100644
--- a/Makefile
+++ b/Makefile
@@ -15,7 +15,6 @@ dependencies .dependencies:
 	arduino-cli core update-index --config-file arduino-cli.yaml
 	arduino-cli core install esp32:esp32@2.0.11 --config-file arduino-cli.yaml
 	arduino-cli lib update-index
-	arduino-cli lib install "Bolder Flight Systems SBUS"@1.0.1
 	arduino-cli lib install "FlixPeriph"
 	arduino-cli lib install "MAVLink"@2.0.1
 	touch .dependencies
diff --git a/docs/build.md b/docs/build.md
index 12e1bfd..a970289 100644
--- a/docs/build.md
+++ b/docs/build.md
@@ -81,9 +81,8 @@ Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* set
 
 1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
 2. Install ESP32 core using [Boards Manager](https://docs.arduino.cc/learn/starting-guide/cores).
-3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library) (**versions are significant**):
-   * `Bolder Flight Systems SBUS`, version 1.0.1.
-   * `FlixPeriph`, version 1.0.0.
+3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
+   * `FlixPeriph`.
    * `MAVLink`, version 2.0.1.
 4. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master).
 5. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE.
diff --git a/flix/flix.ino b/flix/flix.ino
index 5e4358b..5ab7ffb 100644
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -26,7 +26,7 @@
 float t = NAN; // current step time, s
 float dt; // time delta from previous step, s
 float loopFreq; // loop frequency, Hz
-uint16_t channels[16]; // raw rc channels
+int16_t channels[16]; // raw rc channels
 float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)
 Vector rates; // angular rates, rad/s
 Vector acc; // accelerometer data, m/s/s
diff --git a/flix/rc.ino b/flix/rc.ino
index 8927a21..69f7574 100644
--- a/flix/rc.ino
+++ b/flix/rc.ino
@@ -5,7 +5,7 @@
 
 #include <SBUS.h>
 
-// NOTE: use `cr` command and replace with the actual values
+// NOTE: use 'cr' command to calibrate the RC and put the values here
 int channelNeutral[] = {995, 883, 200, 972, 512, 512};
 int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
 
@@ -14,14 +14,12 @@ SBUS RC(Serial2);
 void setupRC() {
 	Serial.println("Setup RC");
 	RC.begin();
-	Serial2.setRxInvert(true); // SBUS uses inverted signal
 }
 
 void readRC() {
-	bool failsafe, lostFrame;
-	if (RC.read(channels, &failsafe, &lostFrame)) {
-		if (failsafe) { return; } // TODO:
-		if (lostFrame) { return; }
+	if (RC.read()) {
+		SBUSData data = RC.data();
+		memcpy(channels, data.ch, sizeof(channels)); // copy channels data
 		normalizeRC();
 	}
 }
diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h
index afc7559..ca9c9ec 100644
--- a/gazebo/SBUS.h
+++ b/gazebo/SBUS.h
@@ -5,13 +5,20 @@
 
 #include "joystick.h"
 
+struct SBUSData {
+	int16_t ch[16];
+};
+
 class SBUS {
 public:
 	SBUS(HardwareSerial& bus) {};
 	void begin() {};
-	bool read(int16_t *channels, bool *failsafe, bool *lostFrame) { // NOTE: on the hardware channels is uint16_t
-		*failsafe = false;
-		*lostFrame = false;
-		return joystickGet();
+	bool read() { return joystickGet(); };
+	SBUSData data() {
+		SBUSData data;
+		for (uint8_t i = 0; i < 16; i++) {
+			data.ch[i] = channels[i];
+		}
+		return data;
 	};
 };
diff --git a/gazebo/flix.h b/gazebo/flix.h
index a8ce214..193e751 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -23,7 +23,7 @@ float t = NAN;
 float dt;
 float loopFreq;
 float motors[4];
-int16_t channels[16]; // raw rc channels WARNING: unsigned on hardware
+int16_t channels[16]; // raw rc channels
 float controls[RC_CHANNELS];
 Vector acc;
 Vector rates;