Diff: modbus.cpp

Revision:

1:0007712e84a8

Parent:

0:f306cb0263a6

--- a/modbus.cpp	Wed Mar 14 06:17:48 2018 +0000
+++ b/modbus.cpp	Fri Mar 16 04:34:08 2018 +0000
@@ -24,7 +24,8 @@
         for (i = 0; i < 6; i++) {
             sum += frame[i];
         }
-        if ((sum + lrc) == 0) {
+        sum = sum + lrc;
+        if (sum == 0) {
             status = 1;
         }
     }
@@ -34,15 +35,20 @@
 
 void modbus_init(uint8_t id)
 {
+    int i;
+    
     nodeId = id;
+    for (i = 0; i < ADDR_RANGE; i++) {
+        regValue[i] = 0;
+    }
 }
 
 uint16_t modbus_read(uint16_t offset)
-{
+{  
     if (offset < ADDR_RANGE) {
         return regValue[offset];
     } 
-    return 0xFFFF;
+    return 0;
 }
 
 uint16_t modbus_update(uint8_t offset, uint16_t val)
@@ -54,7 +60,23 @@
         regValue[offset] = val;
         return tmp;
     } 
-    return 0xFFFF;
+    return 0;
+}
+
+uint8_t modbus_check(uint16_t offset, uint16_t count) 
+{
+    uint8_t reg_map = 0x00;
+    int i, j;
+    
+    for (i = 0; i < ADDR_RANGE; i++) {
+        for (j = offset; j < (offset+count); j++) {
+            if (j == (ADDR_BASE + i)) {
+                reg_map |= (1 << i);
+            } 
+        }
+    }
+    
+    return reg_map;
 }
 
 int modbus_parser(char ch, uint8_t *frame)
@@ -72,6 +94,7 @@
             }
             break;
         case RECEPTION:
+            printf("%c", ch);
             if ((ch >= '0') && (ch <= '9')) {
                 buf[idx++] = ch;
             } else if ((ch >= 'a') && (ch <= 'f')) {
@@ -101,3 +124,66 @@
     
     return status;
 }
+
+uint8_t calc_lrc(uint8_t *frame, int count)
+{
+    int8_t lrc = 0;
+    int i;
+    
+    for (i = 0; i < count; i++) {
+        lrc += frame[i];
+    }
+    return -lrc;
+}
+
+void modbus_response(uint8_t reg_map) 
+{
+    char fmt00[] = ":%02X%02X%02X\r\n";
+    char fmt01[] = ":%02X%02X%02X%02X%02X\r\n";
+    char fmt02[] = ":%02X%02X%02X%02X%02X\r\n";
+    char fmt03[] = ":%02X%02X%02X%02X%02X%02X%02X\r\n";
+    char *fmtstr;
+    char str[20];
+    uint8_t frame[ADDR_RANGE*2 + 3];
+    uint16_t tmp;
+    
+    frame[0] = nodeId;
+    frame[1] = FUNC_CODE;
+    switch(reg_map) {
+        case 1:
+            tmp = modbus_read(0);
+            frame[2] = tmp >> 8;
+            frame[3] = tmp & 0xFF;
+            frame[4] = calc_lrc(frame, 4);
+            fmtstr = fmt01;
+            sprintf(str, fmtstr, frame[0], frame[1], frame[2], frame[3], frame[4]);
+            printf("Response: %s", str);
+            break;
+        case 2:
+            tmp = modbus_read(1);
+            frame[2] = tmp >> 8;
+            frame[3] = tmp & 0xFF;
+            frame[4] = calc_lrc(frame, 4);
+            fmtstr = fmt02;
+            sprintf(str, fmtstr, frame[0], frame[1], frame[2], frame[3], frame[4]);
+            printf("Response: %s", str);        
+            break;
+        case 3:
+            tmp = modbus_read(0);
+            frame[2] = tmp >> 8;
+            frame[3] = tmp & 0xFF;
+            tmp = modbus_read(1);
+            frame[4] = tmp >> 8;
+            frame[5] = tmp & 0xFF;                                
+            frame[6] = calc_lrc(frame, 6);
+            fmtstr = fmt03;
+            sprintf(str, fmtstr, frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6]);
+            printf("Response: %s", str);              
+            break;
+        default:
+            fmtstr = fmt00;
+            frame[2] = calc_lrc(frame, 2);
+            sprintf(str, fmtstr, frame[0], frame[1], frame[2]);
+            printf("Response: %s", str);      
+    }
+}