diff --git a/saint_os/firmware/shared/src/peripheral_manager.c b/saint_os/firmware/shared/src/peripheral_manager.cpp similarity index 100% rename from saint_os/firmware/shared/src/peripheral_manager.c rename to saint_os/firmware/shared/src/peripheral_manager.cpp diff --git a/saint_os/firmware/teensy41/src/fas100_driver.cpp b/saint_os/firmware/teensy41/src/fas100_driver.cpp index 1274e3a..f91828c 100644 --- a/saint_os/firmware/teensy41/src/fas100_driver.cpp +++ b/saint_os/firmware/teensy41/src/fas100_driver.cpp @@ -8,11 +8,9 @@ #include -extern "C" { #include "fas100_driver.h" #include "peripheral_driver.h" #include "flash_types.h" -} // ============================================================================= // Configuration @@ -51,11 +49,11 @@ static uint8_t rx_pos = 0; static void send_poll(void) { #ifndef SIMULATION - uint8_t frame[SPORT_POLL_FRAME_SIZE] = { + uint8_t frame[FAS100_POLL_FRAME_SIZE] = { SPORT_POLL_HEADER, SPORT_FAS100_PHYSICAL_ID }; - FAS100_SERIAL.write(frame, SPORT_POLL_FRAME_SIZE); + FAS100_SERIAL.write(frame, FAS100_POLL_FRAME_SIZE); #endif } diff --git a/saint_os/firmware/teensy41/src/maestro_driver.cpp b/saint_os/firmware/teensy41/src/maestro_driver.cpp index ea24acc..f69187e 100644 --- a/saint_os/firmware/teensy41/src/maestro_driver.cpp +++ b/saint_os/firmware/teensy41/src/maestro_driver.cpp @@ -11,10 +11,9 @@ #include #endif -extern "C" { +#include "flash_types.h" #include "maestro_driver.h" #include "peripheral_driver.h" -} // ============================================================================= // USB Host Objects (hardware only) @@ -23,7 +22,7 @@ extern "C" { #ifndef SIMULATION static USBHost myusb; static USBHub hub1(myusb); -static USBSerial_BigBuffer maestroSerial(myusb); +static USBSerial_BigBuffer maestroSerial(myusb, 1); #endif // ============================================================================= @@ -112,6 +111,17 @@ void maestro_init(void) maestro_initialized = true; } +void PrintDeviceListChanges() { + Serial.printf("*** Device %s %x:%x - connected ***\n", "Maestro", maestroSerial.idVendor(), maestroSerial.idProduct()); + + const uint8_t *psz = maestroSerial.manufacturer(); + if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz); + psz = maestroSerial.product(); + if (psz && *psz) Serial.printf(" product: %s\n", psz); + psz = maestroSerial.serialNumber(); + if (psz && *psz) Serial.printf(" Serial: %s\n", psz); +} + void maestro_update(void) { if (!maestro_initialized) return; @@ -125,10 +135,11 @@ void maestro_update(void) last_connect_check = now; bool was_connected = maestro_connected; - maestro_connected = (bool)maestroSerial; - + maestro_connected = maestroSerial; + if (maestro_connected && !was_connected) { Serial.printf("Maestro: device connected\n"); + PrintDeviceListChanges(); // Clear any pending errors maestro_get_errors(); } else if (!maestro_connected && was_connected) { diff --git a/saint_os/firmware/teensy41/src/main.cpp b/saint_os/firmware/teensy41/src/main.cpp index d54b558..43f1783 100644 --- a/saint_os/firmware/teensy41/src/main.cpp +++ b/saint_os/firmware/teensy41/src/main.cpp @@ -89,11 +89,11 @@ static rcl_timer_t state_timer; // Message buffers static std_msgs__msg__String announcement_msg; -static char announcement_buffer[256]; +static char announcement_buffer[512]; // NOTE: Current Msg is over 4K in size static std_msgs__msg__String capabilities_msg; -static char capabilities_buffer[8 *1024]; +static char capabilities_buffer[10 *1024]; static std_msgs__msg__String config_msg; static char config_buffer[2048]; @@ -387,6 +387,7 @@ static void announce_timer_callback(rcl_timer_t* timer, int64_t last_call_time) snprintf(announcement_buffer + ann_len, sizeof(announcement_buffer) - ann_len, "}}"); ann_len = strlen(announcement_buffer); + //Serial.printf("Announcement: %s, len=%d\n", announcement_buffer, ann_len); announcement_msg.data.data = announcement_buffer; announcement_msg.data.size = strlen(announcement_buffer); diff --git a/saint_os/firmware/teensy41/src/pin_config.cpp b/saint_os/firmware/teensy41/src/pin_config.cpp index 3ef6c95..73651af 100644 --- a/saint_os/firmware/teensy41/src/pin_config.cpp +++ b/saint_os/firmware/teensy41/src/pin_config.cpp @@ -389,12 +389,20 @@ int pin_config_capabilities_to_json(char* buffer, size_t buffer_size, const char // Write comma separator ret = snprintf(buffer + written, buffer_size - written, "%s", first_pin ? "" : ","); - if (ret < 0 || (size_t)ret >= buffer_size - written) return -1; + if (ret < 0 || (size_t)ret >= buffer_size - written) + { + Serial.printf("Pin config: snprintf failed for comma separator, ret=%d\n", ret); + return -1; + } written += ret; int frag = drv->capabilities_to_json(ch, buffer + written, buffer_size - written); - if (frag < 0) return -1; + if (frag < 0) + { + Serial.printf("Pin config: driver capabilities_to_json failed for channel %d\n", ch); + return -1; + } written += frag; } else { // Generic virtual pin entry @@ -405,7 +413,11 @@ int pin_config_capabilities_to_json(char* buffer, size_t buffer_size, const char ret = snprintf(buffer + written, buffer_size - written, "%s{\"gpio\":%d,\"name\":\"%s\",\"capabilities\":[\"%s\"]}", first_pin ? "" : ",", gpio, pin_name, drv->mode_string); - if (ret < 0 || (size_t)ret >= buffer_size - written) return -1; + if (ret < 0 || (size_t)ret >= buffer_size - written) + { + Serial.printf("Pin config: snprintf failed for virtual pin JSON, ret=%d\n", ret); + return -1; + } written += ret; } first_pin = false;