GS USB Streaming (#374)

- Stream telemetry data over USB. - In single mode, stream whichever link updated faster. - In dual mode, stream both links. - Put back all warning messages. Added Debug flag to remove the log output if in debug mode.
catsystems · Jun 29, 2024 · a135223 · a135223
1 parent ad89954
commit a135223
Show file tree

Hide file tree

Showing 12 changed files with 100 additions and 73 deletions.
diff --git a/ground_station/platformio.ini b/ground_station/platformio.ini
@@ -37,7 +37,7 @@ lib_deps =
 
 build_flags =
   -DCFG_TUSB_CONFIG_FILE='"sdkconfig.h"' ; Use default TinyUSB configuration
-  -DFIRMWARE_VERSION='"1.1.1"'           ; Enter Firmware Version here
+  -DFIRMWARE_VERSION='"1.1.2"'           ; Enter Firmware Version here
   -DUSB_MANUFACTURER='"CATS"'            ; USB Manufacturer string
   -DUSB_PRODUCT='"CATS Ground Station"'   ; USB Product String
   -D USB_SERIAL="0"                      ; Enter Device Serial Number here
@@ -70,6 +70,7 @@ build_flags =
   -D CONFIG_SPIRAM_CACHE_WORKAROUND
   -D DISABLE_ALL_LIBRARY_WARNINGS
   -DBOARD_HAS_PSRAM
+  #-D CATS_DEBUG
 
 build_unflags =
   -std=gnu++11

diff --git a/ground_station/src/config.cpp b/ground_station/src/config.cpp
@@ -27,42 +27,38 @@ void Config::save() {
 
 void Config::load() {
   systemParser.loadFile("/config.json");
-  console.log.println("Load config file");
   bool mode{false};
   bool stop{false};
   if (!systemParser.getTestingPhrase(config.testingPhrase)) {
     strncpy(config.testingPhrase, "", 1);
-    console.error.println("Failed");
-  } else {
-    console.log.println(config.testingPhrase);
+    console.warning.println("Testing phrase loading failed");
   }
+
   if (!systemParser.getLinkPhrase1(config.linkPhrase1)) {
     strncpy(config.linkPhrase1, "", 1);
-    console.error.println("Failed");
-  } else {
-    console.log.println(config.linkPhrase1);
+    console.warning.println("Link phrase 1 loading failed");
   }
+
   if (!systemParser.getLinkPhrase2(config.linkPhrase2)) {
     strncpy(config.linkPhrase2, "", 1);
-    console.error.println("Failed");
-  } else {
-    console.log.println(config.linkPhrase2);
+    console.warning.println("Link phrase 2 loading failed");
   }
+
   if (!systemParser.getTelemetryMode(mode)) {
     mode = false;
-  } else {
-    console.log.println(static_cast<uint32_t>(mode));
+    console.warning.println("Telemetry mode loading failed");
   }
+
   if (!systemParser.getNeverStopLoggingFlag(stop)) {
     config.neverStopLogging = false;
-  } else {
-    console.log.println(static_cast<uint32_t>(config.neverStopLogging));
+    console.warning.println("Logging flag loading failed");
   }
+
   if (!systemParser.getTimeZone(config.timeZoneOffset)) {
     config.timeZoneOffset = 0;
-  } else {
-    console.log.println(config.timeZoneOffset);
+    console.warning.println("Timezone loading failed");
   }
+
   if (!systemParser.getMagCalib(config.mag_calib)) {
     config.mag_calib.mag_offset_x = 0;
     config.mag_calib.mag_offset_y = 0;
@@ -71,8 +67,7 @@ void Config::load() {
     config.mag_calib.mag_scale_x = 1000;
     config.mag_calib.mag_scale_y = 1000;
     config.mag_calib.mag_scale_z = 1000;
-  } else {
-    console.log.println(config.timeZoneOffset);
+    console.warning.println("Mag Calibration loading failed");
   }
   if (!systemParser.getUnitSystem(config.unitSystem)) {
     config.unitSystem = UnitSystem::kMetric;

diff --git a/ground_station/src/hmi/hmi.cpp b/ground_station/src/hmi/hmi.cpp
@@ -544,7 +544,6 @@ void Hmi::settings() {
 
         link1.setTestingPhrase(systemConfig.config.testingPhrase, kMaxPhraseLen);
         systemConfig.save();
-        console.log.println("Save config");
       }
       window.initMenu(menuIndex);
     }

diff --git a/ground_station/src/hmi/window.cpp b/ground_station/src/hmi/window.cpp
@@ -1264,9 +1264,7 @@ void Window::updateKeyboard(char *text, int32_t keyHighlight, bool keyPressed) {
   display.setTextColor(BLACK);
 
   if (keyPressed) {
-    console.error.println("[KEYBOARD] Key pressed");
     if (keyHighlight == kShiftIdx) {  // SHIFT
-      console.error.println("[KEYBOARD] Shift");
       upperCase = !upperCase;
       initKeyboard(text);
     } else if (keyHighlight == -1) {  // BACKSPACE

diff --git a/ground_station/src/logging/recorder.cpp b/ground_station/src/logging/recorder.cpp
@@ -8,12 +8,11 @@ bool Recorder::begin() {
   int32_t number = 0;
 
   if (!fatfs.chdir(directory)) {
-    console.error.print("[REC] Open directory failed");
-    console.error.println(directory);
+    console.warning.print("[REC] Open directory failed");
+    console.warning.println(directory);
     fatfs.mkdir(&directory[1]);
-    console.log.println("[REC] Crating directory");
     if (!fatfs.chdir(directory)) {
-      console.error.println("[REC] Open directory failed");
+      console.warning.println("[REC] Open directory failed");
       return false;
     }
   }
@@ -31,9 +30,8 @@ bool Recorder::begin() {
 
 void Recorder::createFile() {
   file = fatfs.open(fileName, FILE_WRITE);
-  console.log.println(fileName);
   if (!file) {
-    console.error.println("[REC] Open file failed");
+    console.warning.println("[REC] Open file failed");
     return;
   }
   fileCreated = true;

diff --git a/ground_station/src/main.cpp b/ground_station/src/main.cpp
@@ -25,9 +25,14 @@ void setup() {
   pinMode(21, INPUT);
 
   console.begin();
+#ifdef CATS_DEBUG
+  console.setLevel(Console::ConsoleLevel::LEVEL_OK);
+#else
+  console.setLevel(Console::ConsoleLevel::LEVEL_LOG);
+#endif
 
   if (!utils.begin(0, "DRIVE")) {
-    console.error.println("[MAIN] Could not initialize utilities");
+    console.warning.println("[MAIN] Could not initialize utilities");
   }
 
   systemConfig.load();
@@ -43,6 +48,9 @@ void setup() {
 
 void loop() {
   static bool ini{false};
+  static uint16_t link1LastTs{0};
+  static uint16_t link2LastTs{0};
+
   if (millis() > 5000 && !ini) {
     ini = true;
     if (systemConfig.config.receiverMode == SINGLE) {
@@ -67,29 +75,51 @@ void loop() {
 
   // In single mode, both antennas track the same rocket
   if (systemConfig.config.receiverMode == SINGLE) {
-    const bool link1DataValid = (link1.data.lat() != 0) && (link1.data.lon() != 0);
-    const bool link2DataValid = (link2.data.lat() != 0) && (link2.data.lon() != 0);
+    const bool link1GpsValid = (link1.data.lat() != 0) && (link1.data.lon() != 0);
+    const bool link2GpsValid = (link2.data.lat() != 0) && (link2.data.lon() != 0);
     // Check if data from link 1 is newer than link 2
     if (link1.data.getLastUpdateTime() > link2.data.getLastUpdateTime()) {
+      // Stream data from Link 1
+      if (link1LastTs != link1.data.ts()) {
+        Utils::streamUsb(&link1, 1);
+      }
+
       // Take data from link 1 with higher priority
-      if (link1DataValid) {
+      if (link1GpsValid) {
         navigation.setPointB(link1.data.lat(), link1.data.lon());
-      } else if (link2DataValid) {
+      } else if (link2GpsValid) {
         navigation.setPointB(link2.data.lat(), link2.data.lon());
       }
     } else {
+      // Stream data from Link 2
+      if (link2LastTs != link2.data.ts()) {
+        Utils::streamUsb(&link2, 2);
+      }
+
       // Take data from link 2 with higher priority
-      if (link2DataValid) {
+      if (link2GpsValid) {
         navigation.setPointB(link2.data.lat(), link2.data.lon());
-      } else if (link1DataValid) {
+      } else if (link1GpsValid) {
         navigation.setPointB(link1.data.lat(), link1.data.lon());
       }
     }
   } else {
+    // Stream both Links as they track different FC's
+    if (link1LastTs != link1.data.ts()) {
+      Utils::streamUsb(&link1, 1);
+    }
+    if (link2LastTs != link2.data.ts()) {
+      Utils::streamUsb(&link2, 2);
+    }
+
     if (link1.data.lat() != 0 && link1.data.lon() != 0) {
       navigation.setPointB(link1.data.lat(), link1.data.lon());
     }
   }
 
+  // Update last timestamp from the link
+  link1LastTs = link1.data.ts();
+  link2LastTs = link2.data.ts();
+
   delay(100);
 }
diff --git a/ground_station/src/navigation.cpp b/ground_station/src/navigation.cpp
@@ -13,8 +13,9 @@ bool Navigation::begin() {
   initialized = true;
 
   compass.init();
+
   if (imu.begin(Wire, 0x6A) != 1) {
-    console.error.println("IMU init failed!");
+    console.warning.println("IMU init failed!");
   }
 
   filter.begin(NAVIGATION_TASK_FREQUENCY);
@@ -173,12 +174,6 @@ void Navigation::navigationTask(void *pvParameter) {
 
     ref->filter.getQuaternion(&ref->q0, &ref->q1, &ref->q2, &ref->q3);
 
-    // console.log.print(ref->ax);
-    // console.log.print("; ");
-    // console.log.print(ref->ay);
-    // console.log.print("; ");
-    // console.log.println(ref->az);
-
     if (ref->calibration == CALIB_ONGOING) {
       ref->calibrate(ref->raw_m);
       ref->check_rotation();

diff --git a/ground_station/src/systemParser.cpp b/ground_station/src/systemParser.cpp
@@ -28,14 +28,14 @@ bool SystemParser::loadFile(const char* path) {
   File file = fatfs.open(filePath);
 
   if (!file) {
-    console.error.println("[PARSER] Open file failed");
+    console.warning.println("[PARSER] Open file failed");
     return false;
   }
 
   DeserializationError error = deserializeJson(doc, file);
   if (error) {
     file.close();
-    console.error.printf("[PARSER] Failed to read file, using default configuration: %s\n", error.c_str());
+    console.warning.printf("[PARSER] Failed to read file, using default configuration: %s\n", error.c_str());
     return false;
   }
 
@@ -186,25 +186,24 @@ bool SystemParser::getUnitSystem(UnitSystem& unit_system) {
  * @return false on error
  */
 bool SystemParser::saveFile(const char* path) {
-  console.log.println("[PARSER] Store file");
   if (path != nullptr) {
     filePath = path;
   }
   if (fatfs.exists(filePath)) {
     if (!fatfs.remove(filePath)) {
-      console.error.println("[PARSER] Could not remove file");
+      console.warning.println("[PARSER] Could not remove file");
       return false;
     }
   }
   // NOLINTNEXTLINE(cppcoreguidelines-init-variables) something is wrong with this 'File' type
   File file = fatfs.open(filePath, FILE_WRITE);
   if (!file) {
-    console.error.println("[PARSER] Open file failed");
+    console.warning.println("[PARSER] Open file failed");
     return false;
   }
   if (serializeJson(doc, file) == 0) {
     file.close();
-    console.error.println("[PARSER] Failed to write to file");
+    console.warning.println("[PARSER] Failed to write to file");
     return false;
   }
   file.close();

diff --git a/ground_station/src/telemetry/parser.cpp b/ground_station/src/telemetry/parser.cpp
@@ -56,7 +56,6 @@ void Parser::process(uint8_t ch) {
       if (crc == ch) {
         parse();
       } else {
-        console.error.println("[PARSER] CRC Failed");
         reset();
       }
     } break;
@@ -83,5 +82,5 @@ void Parser::cmdGNSSTime(uint8_t *args, uint32_t length) {
 
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static) function ptr doesn't work if it's static
 void Parser::cmdGNSSInfo(uint8_t *args [[maybe_unused]], uint32_t length [[maybe_unused]]) {
-  console.log.println("GNSS Info Received");
+  // TODO: Do Something
 }
diff --git a/ground_station/src/telemetry/telemetry.cpp b/ground_station/src/telemetry/telemetry.cpp
@@ -65,11 +65,9 @@ void Telemetry::initLink() {
   if (linkPhrase[0] != 0) {
     // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) uint8 to char is OK
     const uint32_t phraseCrc = crc32(linkPhrase, strlen(reinterpret_cast<const char*>(linkPhrase)));
-    console.error.printf("[TELEMETRY] Sending link phrase: %s (CRC: %lu)\n", linkPhrase, phraseCrc);
     sendLinkPhraseCrc(phraseCrc, 4);
     vTaskDelay(100);
     sendEnable();
-    console.warning.println("[TELEMETRY] Link Enabled");
   }
 
   if (testingPhrase[0] != 0) {
@@ -149,8 +147,6 @@ void Telemetry::update(void* pvParameter) {
 
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static) uses serial
 void Telemetry::sendLinkPhraseCrc(uint32_t crc, uint32_t length) {
-  console.log.println(crc);
-  console.log.println(length);
   uint8_t out[7];  // 1 OP + 1 LEN + 4 DATA + 1 CRC
   out[0] = CMD_LINK_PHRASE;
   out[1] = static_cast<uint8_t>(length);