diff --git a/HansonServo.ino b/HansonServo.ino index ae3b799..ea435a2 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -16,6 +16,7 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static #define CMD_LOAD_FILE 0x03 #define CMD_DELETE_FILE 0x04 #define CMD_SAVE_FILE 0x05 +#define CMD_MESSAGE 0x06 // ESP32 S2 PINOUT @@ -172,6 +173,24 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) { } } +void sendMessage(const String& payload) { + uint16_t length = payload.length(); + + uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF); + for (int i = 0; i < length; i++) { + checksum ^= payload[i]; + } + + Serial.write(HEADER1); + Serial.write(HEADER2); + Serial.write(CMD_MESSAGE); + Serial.write((length >> 8) & 0xFF); + Serial.write(length & 0xFF); + Serial.write((const uint8_t*)payload.c_str(), length); + Serial.write(checksum); +} + + void handleIdRequest() { String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION; uint16_t length = payload.length(); @@ -340,41 +359,70 @@ void handleSaveFile(const uint8_t* payload, uint16_t length) { bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& animation) { - if (length < 17) { - Serial.println("Payload too short for header"); + sendMessage("SAVING FILE"); + if (length < 1) { + Serial.println("Payload too short for filename length"); return false; } - // Parse header - memcpy(animation.header.magic, payload, 4); + // 🔹 Parse filename + uint16_t filenameLength = payload[0] | (payload[1] << 8); + if (length < 2 + filenameLength + 18) { + Serial.println("Payload too short for filename and header"); + return false; + } + + char filename[filenameLength+1]; + + //strcpy(filename, "/test1.anim"); + + + memcpy(filename, payload + 2, filenameLength); + filename[filenameLength] = '\0'; + + + const uint8_t* ptr = payload + 2 + filenameLength; + + String msg = "SAVING FILE: "; + msg += filename; + sendMessage(msg); + // 🔹 Parse header + memcpy(animation.header.magic, ptr, 4); if (strncmp(animation.header.magic, "ANIM", 4) != 0) { Serial.println("Invalid magic header"); + + sendMessage("invalid magic header"); return false; } - animation.header.frameCount = (payload[4] << 8) | payload[5]; - animation.header.version = payload[6]; - animation.header.frameRate = payload[7]; - memcpy(animation.header.reserved, payload + 8, 8); + animation.header.frameCount = (ptr[4] << 8) | ptr[5]; + animation.header.version = ptr[6]; + animation.header.frameRate = ptr[7]; + memcpy(animation.header.reserved, ptr + 8, 8); - uint16_t keyframeCount = payload[16] | (payload[17] << 8); + uint16_t keyframeCount = ptr[16] | (ptr[17] << 8); + ptr += 18; - if (length < 17 + keyframeCount * sizeof(Keyframe)) { + if (length < (ptr - payload) + keyframeCount * 5) { Serial.println("Payload too short for keyframes"); + sendMessage("Payload too short"); return false; } - // Parse keyframes + // 🔹 Parse keyframes animation.clear(); - const uint8_t* ptr = payload + 18; - for (uint8_t i = 0; i < keyframeCount; i++) { + for (uint16_t i = 0; i < keyframeCount; i++) { uint8_t motorId = ptr[0]; uint16_t frame = ptr[1] | (ptr[2] << 8); uint16_t position = ptr[3] | (ptr[4] << 8); animation.addKeyframe(motorId, frame, position); ptr += 5; } - animation.saveToFile("/saved.anim"); + + // 🔹 Save using received filename + animation.saveToFile(("/" + String(filename)).c_str()); + + sendMessage("SAVED"); return true; } @@ -384,6 +432,7 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a + unsigned long lastSend = 0; void loop() { HandleSerialRequests();