From 2e658ff4a94811f878ad79ae3921e6f8485ca019 Mon Sep 17 00:00:00 2001
From: Douglas Clowes <dcl@ansto.gov.au>
Date: Fri, 12 Nov 2010 14:56:28 +1100
Subject: [PATCH] Add Floating point read/write commands r3029 | dcl |
 2010-11-12 14:56:28 +1100 (Fri, 12 Nov 2010) | 1 line

---
 site_ansto/hardsup/sct_modbusprot.c | 68 ++++++++++++++++++++++-------
 1 file changed, 53 insertions(+), 15 deletions(-)

diff --git a/site_ansto/hardsup/sct_modbusprot.c b/site_ansto/hardsup/sct_modbusprot.c
index 2a81a35f..bb0a7376 100644
--- a/site_ansto/hardsup/sct_modbusprot.c
+++ b/site_ansto/hardsup/sct_modbusprot.c
@@ -5,10 +5,11 @@
 #include <ascon.h>
 #include <ascon.i>
 #include <dynstring.h>
+#include <stdbool.h>
 
 static int dbgprintf(char* fmtstr, ...);
 static int dbgprintx(const char* msg, const unsigned char* cp, int blen);
-static unsigned int debug_modbus = 0;
+static unsigned int debug_modbus = 1;
 
 /** @brief encode modbus request
 * TODO: clean me up
@@ -20,6 +21,7 @@ int ModbusWriteStart(Ascon *a) {
   char temp[32];
   char* endp = NULL;
   int idx = 6;
+  bool do_float = false;
   temp[0] = 0; /* transaction id */
   temp[1] = 0;
   temp[2] = 0; /* protocol id */
@@ -38,16 +40,22 @@ int ModbusWriteStart(Ascon *a) {
   temp[idx++] = dev;
   buff = endp + 1;
   cmd = strtoul(buff, &endp, 10);
-  if (endp == buff || (cmd != 3 && cmd != 16)) { /* read/write registers */
+  if (endp == buff || (cmd != 3 && cmd != 16 && cmd != 1003 && cmd != 1016)) { /* read/write registers */
     dbgprintf("modbus-er: Bad command id: %d from %s\n", cmd, buff);
     a->state = AsconIdle;
     AsconError(a, "Bad command id", 0);
     return 0;
   }
+  if (cmd > 1000) {
+    cmd %= 1000;
+    do_float = true;
+  } else {
+    do_float = false;
+  }
   temp[idx++] = cmd;
   buff = endp + 1;
   reg = strtoul(buff, &endp, 10);
-  if (endp == buff || reg < 1 || reg > 65535) {
+  if (endp == buff || reg > 65535) {
     dbgprintf("modbus-er: Bad register id: %d from %s\n", reg, buff);
     a->state = AsconIdle;
     AsconError(a, "Bad register id", 0);
@@ -56,20 +64,39 @@ int ModbusWriteStart(Ascon *a) {
   temp[idx++] = (reg >> 8) & 0xFF;
   temp[idx++] = reg & 0xFF;
   temp[idx++] = 0; /* word count msbyte */
-  temp[idx++] = 1; /* word count lsbyte */
+  if (do_float)
+    temp[idx++] = 2; /* word count lsbyte */
+  else
+    temp[idx++] = 1; /* word count lsbyte */
   if (cmd == 16) { /* write registers */
     buff = endp + 1;
-    unsigned int val;
-    val = strtoul(buff, &endp, 10);
-    if (endp == buff || val > 65535) {
-      dbgprintf("modbus-er: Bad value: %d from %s\n", val, buff);
-      a->state = AsconIdle;
-      AsconError(a, "Bad value", 0);
-      return 0;
+    if (do_float) {
+      union { unsigned char v[4]; float val; } u;
+      u.val = strtof(buff, &endp);
+      if (endp == buff) {
+        dbgprintf("modbus-er: Bad value: %f from %s\n", u.val, buff);
+        a->state = AsconIdle;
+        AsconError(a, "Bad value", 0);
+        return 0;
+      }
+      temp[idx++] = 4; /* byte count */
+      temp[idx++] = u.v[1];
+      temp[idx++] = u.v[0];
+      temp[idx++] = u.v[3];
+      temp[idx++] = u.v[2];
+    } else {
+      unsigned int val;
+      val = strtoul(buff, &endp, 10);
+      if (endp == buff || val > 65535) {
+        dbgprintf("modbus-er: Bad value: %d from %s\n", val, buff);
+        a->state = AsconIdle;
+        AsconError(a, "Bad value", 0);
+        return 0;
+      }
+      temp[idx++] = 2; /* byte count */
+      temp[idx++] = (val >> 8) & 0xFF;
+      temp[idx++] = val & 0xFF;
     }
-    temp[idx++] = 2; /* byte count */
-    temp[idx++] = (val >> 8) & 0xFF;
-    temp[idx++] = val & 0xFF;
   }
   len = idx - 6;
   temp[4] = len >> 8;   /* length msbyte */
@@ -109,8 +136,19 @@ int ModbusReading(Ascon *a) {
         if (cp[7] == 3 && cp[8] == 2) {
           rlen = snprintf(temp, 64, "%d", (((unsigned int)cp[9]) << 8) + (unsigned int)cp[10]);
         }
+        else if (cp[7] == 3 && cp[8] == 4) {
+          union { unsigned char v[4]; float val; } u;
+          u.v[1] = cp[9];
+          u.v[0] = cp[10];
+          u.v[3] = cp[11];
+          u.v[2] = cp[12];
+          rlen = snprintf(temp, 64, "%g", u.val);
+        }
         else if (cp[7] == 16 && cp[11] == 1) {
-          rlen = snprintf(temp, 64, "OK");
+          rlen = snprintf(temp, 64, "OK int");
+        }
+        else if (cp[7] == 16 && cp[11] == 2) {
+          rlen = snprintf(temp, 64, "OK float");
         }
         else if (((unsigned int)cp[7]) == 0x83) {
           rlen = snprintf(temp, 64, "ASCERR:%02x:%d", cp[7], cp[8]);