Added support for console printf

modalai · May 23, 2024 · 4dd4be2 · 4dd4be2
1 parent 7669363
commit 4dd4be2
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 5 deletions.
diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp
@@ -30,7 +30,7 @@
 
 using namespace QURT;
 
-static Empty::UARTDriver serial0Driver;
+static UARTDriver serial0Driver("/dev/console");
 static UARTDriver serial1Driver("/dev/tty-4");
 static UARTDriver serial2Driver("/dev/tty-2");
 
@@ -87,9 +87,9 @@ void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const
     serial0Driver.begin(115200);
     rcinDriver.init();
 	// Runs okay up to here. Next line causes a crash!
-    // callbacks->setup();
-    // scheduler->set_system_initialized();
-	// 
+    callbacks->setup();
+    scheduler->set_system_initialized();
+
     // for (;;) {
     //     callbacks->loop();
     // }

diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp
@@ -2,7 +2,27 @@
 #include "UARTDriver.h"
 #include <AP_Common/ExpandingString.h>
 
-QURT::UARTDriver::UARTDriver(const char *name) {}
+extern const AP_HAL::HAL& hal;
+
+QURT::UARTDriver::UARTDriver(const char *name)
+{ 
+	if (strcmp(name, "/dev/console") == 0) {
+		_is_console = true;
+		HAP_PRINTF("UART console created");
+	}
+}
+
+void QURT::UARTDriver::printf(const char *fmt, ...)
+{
+	if (_is_console) {
+		va_list ap;
+		char buf[300];
+		va_start(ap, fmt);
+		vsnprintf(buf, sizeof(buf), fmt, ap);
+		va_end(ap);
+		HAP_PRINTF(buf);
+	}
+}
 
 /* QURT implementations of virtual methods */
 void QURT::UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) {}

diff --git a/libraries/AP_HAL_QURT/UARTDriver.h b/libraries/AP_HAL_QURT/UARTDriver.h
@@ -19,6 +19,8 @@
 #include "Semaphores.h"
 #include <AP_HAL/utility/RingBuffer.h>
 
+#define CONSOLE_BUFFER_SIZE 64
+
 class QURT::UARTDriver : public AP_HAL::UARTDriver {
 public:
     UARTDriver(const char *name);
@@ -29,6 +31,8 @@ class QURT::UARTDriver : public AP_HAL::UARTDriver {
     /* Empty implementations of Stream virtual methods */
     uint32_t txspace() override;
 
+	void printf(const char *fmt, ...) override;
+
 protected:
     void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
     size_t _write(const uint8_t *buffer, size_t size) override;
@@ -37,4 +41,6 @@ class QURT::UARTDriver : public AP_HAL::UARTDriver {
     void _flush() override;
     uint32_t _available() override;
     bool _discard_input() override;
+	bool _is_console{ false };
+	char _console_buffer[CONSOLE_BUFFER_SIZE];
 };
diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -398,6 +398,9 @@ void AP_Vehicle::setup()
     can_mgr.init();
 #endif
 
+// Crashes if we go farther!
+return;
+
 #if HAL_LOGGING_ENABLED
     logger.init(get_log_bitmask(), get_log_structures(), get_num_log_structures());
 #endif