Diff: Picaso_4DGL-32PTU_Media.cpp

Revision:

14:561cb06a2739

Parent:

13:1a0800957412

Child:

15:86bdf382e6f7

--- a/Picaso_4DGL-32PTU_Media.cpp	Thu Sep 15 08:04:37 2016 +0000
+++ b/Picaso_4DGL-32PTU_Media.cpp	Thu Sep 15 09:48:59 2016 +0000
@@ -113,17 +113,50 @@
 bool PICASO_4DGL :: media_WrSector(char *block) {
 
     char command[514] = "";
+    bool success = false;
     
     command[0] = (MEDIA_WRITE_SECTOR >> (8*1)) & 0xff;
     command[1] = (MEDIA_WRITE_SECTOR >> (8*0)) & 0xff;
     
-    int j = 2;
-    int k = 0;
-    //writeCOMMAND(command, 2);
-    bool success = readSectorResponse(515);
-#ifdef DEBUGMODE
-    pc.printf("\n\r DEBUG: Set sector address: %i\n\r", success);
+    if (strlen(block) <= 512) {
+        int j = 513 - strlen(block);
+        for (int i = 0; i < strlen(block); i++) {
+            command[j++] = block[i];
+        }
+        success = media_WrData(command, 514);
+    }
+    else { // data is bigger than one block
+        int j = 513 - (strlen(block) % 512); // set the first block pointer
+        for (int i = 0; i < strlen(block); i++) {
+            if (j == 513) {
+                success = media_WrData(command, 514);
+                #if DEBUGMODE
+                    pc.printf("\n\r     DEBUG: Block send = %i", success);
+                #endif
+                j = 2;
+                command[j++] = block[i];
+            }
+            else if (i == (strlen(block) - 1)) {
+                command[j] = block[i];
+                success = media_WrData(command, 514);
+            }
+            else command[j++] = block[i];
+        }
+    }
+    return success;
+}
+
+bool PICASO_4DGL :: media_WrData(char *block, int size) {
+
+#if DEBUGMODE
+    pc.printf("\n\r     DEBUG: Write block =");
+    for (int k = 2; k < size; k++) {
+        pc.printf(" %02X", block[k]);
+    }
+    pc.printf("\n\r");
 #endif
+    writeCOMMAND(block, size);
+    bool success = writeSectorResponse(3);
     return success;
 }
 
@@ -157,4 +190,3 @@
 
 
 
-