Still early alpha, controller works, stupidremote working

libecmbind/ecmbind.h

@@ -27,7 +27,7 @@ typedef struct {
 } dto_servicedescriptor_t;
 
 typedef void (*cb_enum_indeces_t)(int slave, int index);
-typedef void (*cb_enum_sdo_descriptors_t)(int slave, int index, uint16_t dataType, uint16_t objectCode, int sub, char *name);
+typedef void (*cb_enum_sdo_descriptors_t)(int slave, int index, uint16_t dataType, uint16_t objectCode, int sub, ushort bitlength, char *name);
 typedef void (*cb_enum_pdo)(uint16_t slave, uint16_t index, char subindex, int addr_offset, int addr_bit, int bitlength);
 
 extern ecd_pdo_entry_t ecd_pdo_map[1024];

libecmbind/libecmbind.c

@@ -181,6 +181,18 @@ int ecmbind_sdo_write(int slave, int index, int subindex, char* buffer, int size
     return ec_SDOwrite(slave, index, subindex, FALSE, size, buffer, 250000L);
 }
 
+int ecmbind_sdo_read_ca(int slave, int index, int subindex, char* buffer, int size)
+{
+    int wkc = ec_SDOread(slave, index, subindex, TRUE, &size, buffer, 250000L);
+    if (wkc <= 0)
+        return wkc;
+    return size;
+}
+int ecmbind_sdo_write_ca(int slave, int index, int subindex, char* buffer, int size)
+{
+    return ec_SDOwrite(slave, index, subindex, TRUE, size, buffer, 250000L);
+}
+
 /*
 typedef void (*cb_enum_pdo)(int slave, int index, int subindex, int addr_offset, int addr_bit, int bitlength);
 */

libecmbind/libecmbind_descriptors.c

@@ -189,7 +189,7 @@ int ecmbind_read_objectdescription(int Slave, int index, cb_enum_sdo_descriptors
             strncpy(temp , (char *)&aSDOp->bdata[6], n);
             temp[n] = 0x00;
 
-            cb(Slave, index, etohs(aSDOp->wdata[1]), aSDOp->bdata[5], aSDOp->bdata[4], temp);
+            cb(Slave, index, etohs(aSDOp->wdata[1]), aSDOp->bdata[5], aSDOp->bdata[4], 0, temp);
          }
          /* got unexpected response from slave */
          else
@@ -265,7 +265,7 @@ int ecmbind_read_objectdescription_entry(uint16_t slave, uint16_t index, uint16_
             strncpy(temp , (char *)&aSDOp->wdata[5], n);
             temp[n] = 0x00; /* string terminator */
 
-            cb(slave, index, (ushort)etohs(aSDOp->wdata[2]), 0, sub, temp);
+            cb(slave, index, (ushort)etohs(aSDOp->wdata[2]), 0, sub, (ushort)etohs(aSDOp->wdata[3]), temp);
 
 /*
             pOElist->ValueInfo[sub] = aSDOp->bdata[3];

ln.ethercat.service/EthercatService.cs

@@ -17,8 +17,6 @@ namespace ln.ethercat.service
     {
 
         public ECMaster ECMaster { get; private set; }
-        public Controller Controller { get; private set; }
-
 
         [StaticArgument(Option = 'i', LongOption = "interface")]
         public string EthercatInterfaceName { get; set; }
@@ -72,14 +70,9 @@ namespace ln.ethercat.service
             httpServer = new HTTPServer(httpLoggingRouter);
             httpServer.AddEndpoint(new Endpoint(IPv6.ANY, 7676));
 
-            Controller = new Controller();
-            Controller.Add(new CIA402Controller(ECMaster, 1));
-            Controller.Add(new CIA402Controller(ECMaster, 2));
-
-            controllerApiController = new ControllerApiController(Controller);
+            controllerApiController = new ControllerApiController(ECMaster.Controller);
             httpRouter.AddSimpleRoute("/api/v1/*", controllerApiController);
 
-
             timerWebsockets = new System.Timers.Timer(250);
             timerWebsockets.Elapsed += WebSocketTimerMethod;
         }
@@ -97,6 +90,7 @@ namespace ln.ethercat.service
         {
             try{
                 EthercatWebSocket.SendProcessData(ECMaster);
+                ControllerWebSocket.SendUpdates(ECMaster.Controller);
             } catch (Exception ex)
             {
                 Logging.Log(ex);
@@ -108,10 +102,10 @@ namespace ln.ethercat.service
             switch (newState)
             {
                 case ECSlaveState.OPERATIONAL:
-                    Controller.Start();
+                    //Controller.Start();
                     break;
                 default:
-                    Controller.Stop();
+                    //Controller.Stop();
                     break;
             }
         }

ln.ethercat.service/MainAxFeederControllerLogic.cs

@@ -0,0 +1,63 @@
+
+using System;
+using System.IO;
+using ln.ethercat.controller;
+using ln.json;
+using ln.json.mapping;
+
+namespace ln.ethercat.service
+{
+
+    public class MainAxFeederControllerLogic
+    {
+        public MyParameters Parameters { get; set; }
+
+        ECMaster ECMaster;
+
+        SDOValue svRelais;
+        SDOValue svEnable;
+
+        public MainAxFeederControllerLogic(ECMaster ecMaster)
+        {
+            ECMaster = ecMaster;
+
+            Parameters = new MyParameters();
+            if (File.Exists("mafcl.json"))
+            {
+                JSONValue configValue = JSONParser.ParseFile("mafcl.json");
+                JSONMapper.DefaultMapper.Apply(configValue as JSONObject, Parameters);
+            }
+        }
+
+        public void Save()
+        {
+            if (JSONMapper.DefaultMapper.Serialize(Parameters, out JSONValue configObject))
+            {
+                using (StreamWriter sw = new StreamWriter("mafcl.json"))
+                {
+                    sw.Write(configObject.ToString());
+                    sw.Flush();
+                }
+            }
+        }
+
+        public void Initialize()
+        {
+            if (!(
+                ECMaster.GetSDOValue(1, 0x2012, 31, out svRelais) &&
+                ECMaster.GetSDOValue(1, 0x2012, 32, out svEnable)
+            ))
+                throw new Exception("could not retrieve needed SDOValues");
+        }
+
+        public void ControllerLogic(Controller contreller)
+        {
+        }
+
+
+        public class MyParameters {
+
+        }
+
+    }
+}

ln.ethercat.service/Program.cs

@@ -1,9 +1,11 @@
 using System;
+using System.Data;
 using System.Text;
 using System.Threading;
 using ln.application;
 using ln.ethercat.controller;
 using ln.ethercat.controller.drives;
+using ln.ethercat.controller.remote;
 using ln.logging;
 using ln.type;
 
@@ -11,6 +13,9 @@ namespace ln.ethercat.service
 {
     class Program
     {
+        [StaticArgument(LongOption = "serial-remote")]
+        static string SerialRemotePort { get; set; }
+
         static void Main(string[] args)
         {
             Logging.Log(LogLevel.INFO, ".NET EtherCAT service host");
@@ -23,11 +28,52 @@ namespace ln.ethercat.service
             EthercatService ethercatService = new EthercatService(args[0]);
 
             ArgumentContainer argumentContainer = new ArgumentContainer();
+            argumentContainer.AddStaticOptions<Program>();
             argumentContainer.AddOptions(ethercatService);
             argumentContainer.Parse(ref args);
 
             ethercatService.Initialize();
+            ethercatService.ECMaster.OnStateChange += (ECMaster ECMaster, ECSlaveState newState) => {
+                if (newState == ECSlaveState.OPERATIONAL)
+                {
+                    ECMaster.DriveControllers[1].IgnoredByController = true;
+
+                    if (ECMaster.GetSDOValue(1, 0x2012, 32, out SDOValue svEnableDrives))
+                    {
+                        svEnableDrives.SetValue((byte)0x00);
+                    }
+
+                } else if (newState == ECSlaveState.PRE_OP)
+                {
+                    ECMaster.RequestPDOMapping(1, 0x2012, 31, true);
+                    ECMaster.RequestPDOMapping(1, 0x2012, 32, true);
+                }
+            };
+
+            ethercatService.ECMaster.Controller.OnStateChanged += (controller, state) => {
+                Logging.Log(LogLevel.DEBUG, "ControllerState=={0}", state);
+
+                if (state == ControllerStates.NOTREADY)
+                {
+                    if (controller.ECMaster.GetSDOValue(1, 0x2012, 32, out SDOValue svEnableDrives))
+                    {
+                        Logging.Log(LogLevel.DEBUG, "ControllerState=={0} powering up enable signals", state);
+                        svEnableDrives.SetValue((byte)0x01);
+                    }
+                } else if (state == ControllerStates.READY)
+                {
+                    controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.TORQUE;
+                }
+            };
+
             ethercatService.Start();
+
+
+            if (SerialRemotePort != null)
+            {
+                StupidSerialRemote stupidSerialRemote = new StupidSerialRemote(ethercatService.ECMaster.Controller, SerialRemotePort);
+                stupidSerialRemote.Start();
+            }
         }
     }
 }

ln.ethercat.service/api/v1/ControllerApiController.cs

@@ -1,5 +1,3 @@
-
-
 using System;
 using System.Globalization;
 using System.Linq;
@@ -30,93 +28,61 @@ namespace ln.ethercat.service.api.v1
 
 
         [GET("/sockets/controller")]
-        public HttpResponse GetControllerSocket()
-        {
-            Timer timer = new Timer(250);
-            JSONWebSocketResponse websocket = new JSONWebSocketResponse();
-            websocket.OnWebSocketStateChanged += (Socket, newstate) => {
-                if (newstate == WebSocketState.CLOSED)
-                {
-                    timer.Stop();
-                    timer.Dispose();
-                }
-            };
+        public HttpResponse GetControllerSocket() => new ControllerWebSocket(Controller);
 
-            timer.Elapsed += (s,e) => {
-                try{
-                    JSONObject controllerState = new JSONObject()
-                        .Add("DriveControllers", new JSONArray().Add(Controller.DriveControllers.Select((dc=> new JSONObject()
-                                                                                                                    .Add("slave", dc.Slave)
-                                                                                                                    .Add("DriveController", dc.GetType().Name)
-                                                                                                                    .Add("DriveState", dc.DriveState.ToString())
-                                                                                                                    ))))
-                        .Add("DrivesState", Controller.DrivesState.ToString())
-                        .Add("IsRunning", Controller.IsRunning)
-                    ;
-                    websocket.Send(controllerState);
-                } catch (Exception ex)
-                {
-                    Logging.Log(ex);
-                }
-            };
+        // [GET("/sockets/controller/drives/:drive")]
+        // public HttpResponse GetDriveControllerSocket(int drive)
+        // {
+        //     DriveController driveController = Controller.DriveControllers[drive];
 
-            timer.Start();
-            return websocket;
-        }
+        //     Timer timer = new Timer(250);
+        //     JSONWebSocketResponse websocket = new JSONWebSocketResponse();
+        //     websocket.OnWebSocketStateChanged += (Socket, newstate) => {
+        //         if (newstate == WebSocketState.CLOSED)
+        //         {
+        //             timer.Stop();
+        //             timer.Dispose();
+        //         }
+        //     };
 
-        [GET("/sockets/controller/drives/:drive")]
-        public HttpResponse GetDriveControllerSocket(int drive)
-        {
-            DriveController driveController = Controller.DriveControllers[drive];
+        //     websocket.OnWebSocketReceivedText += (s,text) => {
+        //         JSONObject message = (JSONObject)JSONParser.Parse(text);
+        //         switch (message["event"].ToNative().ToString())
+        //         {
+        //             case "action":
+        //             Logging.Log(LogLevel.DEBUG, "DriveControllerSocket: action: {0}", message["value"].ToNative().ToString());
+        //                 driveController.GetType().GetMethod(message["value"].ToNative().ToString()).Invoke(driveController, new object[0]{});
+        //                 break;
+        //             case "set":
+        //                 JSONObject jsonSet = (message["value"] as JSONObject);
+        //                 foreach (string key in jsonSet.Keys)
+        //                 {
+        //                     PropertyInfo propertyInfo = driveController.GetType().GetProperty(key);
+        //                     propertyInfo.SetValue(driveController, Cast.To(jsonSet[key].ToNative(), propertyInfo.PropertyType));
+        //                 }
+        //                 break;
+        //         }
+        //     };
 
-            Timer timer = new Timer(250);
-            JSONWebSocketResponse websocket = new JSONWebSocketResponse();
-            websocket.OnWebSocketStateChanged += (Socket, newstate) => {
-                if (newstate == WebSocketState.CLOSED)
-                {
-                    timer.Stop();
-                    timer.Dispose();
-                }
-            };
+        //     timer.Elapsed += (s,e) => {
+        //         try{
+        //             JSONObject driveControllerState = new JSONObject()
+        //                 .Add("id", drive)
+        //                 .Add("DriveState", driveController.DriveState.ToString())
+        //                 .Add("OEMDriveState", driveController.OEMDriveState)
+        //                 .Add("DriveMode", driveController.DriveMode)
+        //                 .Add("OEMDriveMode", driveController.OEMDriveMode)
+        //                 ;
+        //             websocket.Send(driveControllerState);
+        //         } catch (Exception ex)
+        //         {
+        //             Logging.Log(ex);
+        //         }
+        //     };
 
-            websocket.OnWebSocketReceivedText += (s,text) => {
-                JSONObject message = (JSONObject)JSONParser.Parse(text);
-                switch (message["event"].ToNative().ToString())
-                {
-                    case "action":
-                    Logging.Log(LogLevel.DEBUG, "DriveControllerSocket: action: {0}", message["value"].ToNative().ToString());
-                        driveController.GetType().GetMethod(message["value"].ToNative().ToString()).Invoke(driveController, new object[0]{});
-                        break;
-                    case "set":
-                        JSONObject jsonSet = (message["value"] as JSONObject);
-                        foreach (string key in jsonSet.Keys)
-                        {
-                            PropertyInfo propertyInfo = driveController.GetType().GetProperty(key);
-                            propertyInfo.SetValue(driveController, Cast.To(jsonSet[key].ToNative(), propertyInfo.PropertyType));
-                        }
-                        break;
-                }
-            };
-
-            timer.Elapsed += (s,e) => {
-                try{
-                    JSONObject driveControllerState = new JSONObject()
-                        .Add("id", drive)
-                        .Add("DriveState", driveController.DriveState.ToString())
-                        .Add("OEMDriveState", driveController.OEMDriveState)
-                        .Add("DriveMode", driveController.DriveMode)
-                        .Add("OEMDriveMode", driveController.OEMDriveMode)
-                        ;
-                    websocket.Send(driveControllerState);
-                } catch (Exception ex)
-                {
-                    Logging.Log(ex);
-                }
-            };
-
-            timer.Start();
-            return websocket;
-        }
+        //     timer.Start();
+        //     return websocket;
+        // }
         
 
     }

ln.ethercat.service/api/v1/ControllerWebSocket.cs

@@ -0,0 +1,115 @@
+using System;
+using System.Collections.Concurrent;
+using System.Collections.Generic;
+using ln.ethercat.controller;
+using ln.ethercat.controller.drives;
+using ln.http.websocket;
+using ln.json;
+using ln.logging;
+
+namespace ln.ethercat.service.api.v1
+{
+    public class ControllerWebSocket : WebSocketResponse
+    {
+        static List<ControllerWebSocket> webSockets = new List<ControllerWebSocket>();
+        static bool sendingUpdates;
+        public static void SendUpdates(Controller controller)
+        {
+            lock (webSockets)
+            {
+                if (sendingUpdates)
+                    return;
+                sendingUpdates = true;
+            }
+
+            try
+            {
+
+                JSONArray jsonDriveControllers = new JSONArray();
+
+                foreach (DriveController driveController in controller.ECMaster.DriveControllers)
+                {
+                    JSONObject jsonDriveController =
+                        new JSONObject()
+                        .Add("DriveState", driveController.DriveState.ToString())
+                        .Add("OEMState", driveController.OEMDriveState)
+                        .Add("DriveMode", driveController.DriveMode.ToString())
+                        .Add("TargetValue", driveController.TargetValue)
+                        .Add("ActualSpeed", driveController.ActualSpeed)
+                        .Add("ActualTorque", driveController.ActualTorque)
+                        .Add("ActualLoad", driveController.ActualLoad)
+                        ;
+                    
+                    jsonDriveControllers.Add(jsonDriveController);
+                }
+
+                JSONObject jsonController = 
+                    new JSONObject()
+                    .Add("State", controller.ControllerState.ToString())
+                    .Add("Drives", jsonDriveControllers)
+                    .Add("CycleCounter", controller.CycleCounter)
+                    ;
+
+
+                JSONObject jsonMessage =
+                    new JSONObject()
+                    .Add("event", "update")
+                    .Add("value", jsonController)
+                    ;
+
+                string jsonMessageText = jsonMessage.ToString();
+
+                ControllerWebSocket[] currentSockets;
+                lock (webSockets)
+                    currentSockets = webSockets.ToArray();
+
+                foreach (ControllerWebSocket webSocket in currentSockets)
+                {
+                    if (webSocket.Controller.Equals(controller) && (webSocket.State == WebSocketState.OPEN))
+                        webSocket.Send(jsonMessageText);
+                }
+            } catch (Exception e)
+            {
+                Logging.Log(e);
+            } finally
+            {
+                sendingUpdates = false;
+            }
+        }
+
+        Controller Controller;
+
+        public ControllerWebSocket(Controller controller)
+        {
+            Controller = controller;
+            OnWebSocketStateChanged += (WebSocketResponse webSocket, WebSocketState newState) => {
+                switch (newState)
+                {
+                    case WebSocketState.OPEN:
+                        lock (webSockets)
+                            webSockets.Add(this);
+                        break;
+                    default:
+                        lock (webSockets)
+                            webSockets.Remove(this);
+                        break;
+                }
+            };
+        }
+
+
+        public override void Received(string textMessage)
+        {
+            base.Received(textMessage);
+
+            JSONValue jsonTextValue = JSONParser.Parse(textMessage);
+            if (jsonTextValue is JSONObject jsonMessage)
+            {
+                string eventName = jsonMessage["event"].ToNative().ToString();
+            }
+        }
+
+
+
+    }
+}

ln.ethercat.service/api/v1/EthercatApiController.cs

@@ -1,18 +1,11 @@
 using System;
-using System.Collections.Generic;
-using System.Linq;
-using System.Timers;
 using ln.http;
 using ln.http.api;
 using ln.http.api.attributes;
-using ln.http.websocket;
 using ln.json;
-using ln.json.mapping;
-using ln.logging;
 
 namespace ln.ethercat.service.api.v1
 {
-
     public class EthercatApiController : WebApiController
     {
         ECMaster ECMaster => EthercatService.ECMaster;
@@ -51,14 +44,6 @@ namespace ln.ethercat.service.api.v1
         [GET("/slaves/:slave/state")]
         public HttpResponse GetEthercatState(int slave) => HttpResponse.OK().Content(ECMaster.ReadSlaveState(slave).ToString());
 
-        [POST("/slaves/:slave/state")]
-        public HttpResponse RequestEthercatState(int slave, ECSlaveState state)
-        {
-            if (ECMaster.RequestSlaveState(slave, state) > 0)
-                return HttpResponse.NoContent();
-            return HttpResponse.GatewayTimeout();
-        }
-
 
         [GET("/master/state")]
         public HttpResponse GetFullState()

ln.ethercat.service/api/v1/EthercatWebSocket.cs

@@ -10,6 +10,7 @@ using ln.http.websocket;
 using ln.json;
 using ln.json.mapping;
 using ln.logging;
+using ln.type;
 
 namespace ln.ethercat.service.api.v1
 {
@@ -17,9 +18,19 @@ namespace ln.ethercat.service.api.v1
     {
         /************** Static ***************************************************************************/
         static List<EthercatWebSocket> activeWebSockets = new List<EthercatWebSocket>();
+        static bool broadcastActive;
 
         public static void SendProcessData(ECMaster ecMaster)
         {
+            lock (activeWebSockets)
+            {
+                if (broadcastActive)
+                    return;
+                broadcastActive = true;
+            }
+
+            try{
+
             JSONArray jsonPDOList = new JSONArray();
 
             foreach (PDO pdo in ecMaster.GetPDOMap())
@@ -49,11 +60,29 @@ namespace ln.ethercat.service.api.v1
                 );
             string stateMessageText = stateMessage.ToString();
 
-            foreach (EthercatWebSocket webSocket in activeWebSockets)
+            EthercatWebSocket[] sockets;
+            lock(activeWebSockets)
+                sockets = activeWebSockets.ToArray();
+
+            foreach (EthercatWebSocket webSocket in sockets)
             {
-                webSocket.Send(messageText);
-                webSocket.Send(stateMessageText);
-                webSocket.SendSubscribedSDOs();
+                if (webSocket.State != WebSocketState.OPEN)
+                {
+                    lock (activeWebSockets)
+                        activeWebSockets.Remove(webSocket);
+                } else 
+                {
+                    webSocket.Send(messageText);
+                    webSocket.Send(stateMessageText);
+                    webSocket.SendSubscribedSDOs();
+                }
+            }
+            } catch (Exception e)
+            {
+                Logging.Log(e);
+            } finally
+            {
+                broadcastActive = false;
             }
         }
 
@@ -93,11 +122,13 @@ namespace ln.ethercat.service.api.v1
             {
                 case WebSocketState.OPEN:
                     Logging.Log(LogLevel.DEBUG, "EthercatWebSocket -> active");
-                    activeWebSockets.Add(this);
+                    lock(activeWebSockets)
+                        activeWebSockets.Add(this);
                     break;
-                case WebSocketState.CLOSED:
+                default:
                     Logging.Log(LogLevel.DEBUG, "EthercatWebSocket -> inactive");
-                    activeWebSockets.Remove(this);
+                    lock(activeWebSockets)
+                        activeWebSockets.Remove(this);
                     break;
             }
         }
@@ -127,6 +158,28 @@ namespace ln.ethercat.service.api.v1
                         break;
                     case "action":
 
+                        break;
+                    case "sdowrite":
+                        if (jsonMessage["value"] is JSONObject jsonValue)
+                        {
+                            UInt16 slave_id, index;
+                            byte subindex;
+                            object value;
+
+                            slave_id = Cast.To<UInt16>(jsonValue["Slave"].ToNative());
+                            index = Cast.To<UInt16>(jsonValue["Index"].ToNative());
+                            subindex = Cast.To<byte>(jsonValue["SubIndex"].ToNative());
+                            value = jsonValue["Value"].ToNative();
+
+                            if (EthercatService.ECMaster.GetSDOValue(slave_id, index, subindex, out SDOValue sdoValue))
+                            {
+                                sdoValue.SetValue(value);
+                            } else
+                            {
+                                Logging.Log(LogLevel.DEBUG, "EthercatWebSocket: event sdowrite: could not find SDOValue {0}:{1:X4}.{2}", slave_id, index, subindex);
+                            }
+                        }
+
                         break;
                 }
             }

ln.ethercat.service/www/html/spa.html

@@ -50,7 +50,11 @@
                     <router-view></router-view>
                 </div>
                 <aside class="right">
-                    <pdo-panel id="pdopanel" :processdata="processdata"></pdo-panel>
+                    <pdo-panel
+                        id="pdopanel"
+                        :processdata="processdata"
+                        @write="$ECAPP.sendMessage('sdowrite', $event);"
+                        ></pdo-panel>
                 </aside>
             </main>
             <footer>

ln.ethercat.service/www/static/css/spa.css

@@ -49,13 +49,13 @@ fieldset {
     display: flex;
 }
 
-fieldset > input {
-    flex-grow: 1;
-}
 fieldset > label {
     flex-grow: 0;
 }
 
+fieldset > input, fieldset > .value {
+    flex-grow: 1;
+}
 
 .panel {
     padding: 6px;

ln.ethercat.service/www/static/js/ecapp.js

@@ -24,6 +24,10 @@ class EthercatApplication
         this.base_url = scheme + "//" + host;
 
         this.connectMainSocket();
+
+        window.onunload = () => {
+            this.closeWebSockets();
+        };
     }
 
     connectMainSocket(){
@@ -49,6 +53,7 @@ class EthercatApplication
         this.socket.onerror = (evt)=>{
             console.log("weboscket error: ", evt);
             this.socket.close();
+            this.socket = null;
         };
         this.socket.onclose = (evt)=>{
             console.log("weboscket closed: ", evt);
@@ -72,6 +77,7 @@ class EthercatApplication
         {
             let ws = this.ac_sockets.pop();
             console.log("disposing:", ws);
+            ws.onclose = null;
             ws.close();
         }
     }

ln.ethercat.service/www/static/js/pdopanel.js

@@ -12,12 +12,26 @@
         data: function(){
             return {}
         },
+        methods: {
+            promptNewValue: function(slave, index, subindex, datatype, initialValue){
+                console.log(index, subindex, datatype, initialValue);
+                let newValue = window.prompt(`new value for ${slave}:${index.toString(16)}.${subindex}`, initialValue);
+                console.log(newValue);
+                if (newValue)
+                {
+                    this.$emit("write", { Slave: slave, Index: index, SubIndex: subindex, Value: newValue});
+                }
+            }
+        },
         template: `
 <div class="panel">
     <h2>Prozessdaten</h2>
     <article v-for="sdo,key in processdata">
-        <div>{{sdo.Descriptor.Index.toString(16).toUpperCase()}}.{{sdo.SubIndex}} {{ sdo.Name }}</div>
-        <div class="value">{{ sdo.Value }}</div>
+        <div>{{sdo.Descriptor.SlaveId}}:{{sdo.Descriptor.Index.toString(16).toUpperCase()}}.{{sdo.SubIndex}} {{ sdo.Name }} ({{sdo.DataType}})</div>
+        <div
+            class="value"
+            @dblclick="promptNewValue(sdo.Descriptor.SlaveId, sdo.Descriptor.Index, sdo.SubIndex, sdo.DataType, sdo.Value);"
+            >{{ sdo.Value }}</div>
     </article>
 </div>
     `,

ln.ethercat.service/www/static/js/sdopanel.js

@@ -13,18 +13,32 @@
         data: function(){
             return {}
         },
+        methods: {
+            promptNewValue: function(slave, index, subindex, datatype, initialValue){
+                console.log(index, subindex, datatype, initialValue);
+                let newValue = window.prompt(`new value for ${slave}:${index.toString(16)}.${subindex}`, initialValue);
+                console.log(newValue);
+                if (newValue)
+                {
+                    this.$emit("write", { Slave: slave, Index: index, SubIndex: subindex, Value: newValue});
+                }
+            }
+        },
         template: `
 <div class="panel">
     <h2>SDO Panel</h2>
     <article v-for="sdo,key in subscribed">
         <h2>{{sdo.Index.toString(16).toUpperCase()}} {{ sdo.Name }} ({{ sdo.DataType}} / M:{{ sdo.MaxSubIndex }})</h2>
         <ul>
-            <div
+            <fieldset
                 v-for="sdovalue in sdo.Values"
                 >
-                <span>.{{sdovalue.SubIndex}} {{ sdovalue.Name }} ({{ sdo.DataType}})</span>
-                <input :disabled="(sdovalue.DataType == 'NONE')" type="text" :value="sdovalue.Value">
-            </div>
+                <span>.{{sdovalue.SubIndex}} {{ sdovalue.Name }} ({{ sdovalue.DataType}})</span>
+                <div
+                    class="value"
+                    @dblclick="promptNewValue(sdo.SlaveId, sdo.Index, sdovalue.SubIndex, sdovalue.DataType, sdovalue.Value);"
+                    >{{ sdovalue.Value }}</div>
+            </fieldset>
         </ul>
     </article>
 </div>

ln.ethercat.service/www/static/js/sdoselect.js

@@ -16,20 +16,35 @@
                     datatype: null,
                 },
                 selectedIndex: null,
+                manualIndex: "",
             };
         },
         methods: {
             getFilteredDescriptors: function(){
-                return this.descriptors.filter((sdo)=>
-                    (!this.filters.name || sdo.name.toLowerCase().includes(this.filters.name.toLowerCase())) ); // &&
+                if (this.filters.name)
+                    return this.descriptors.filter((sdo)=>(sdo.name.toLowerCase().includes(this.filters.name.toLowerCase())) );
+                else
+                    return this.descriptors;
+                    
+                    // &&
                     // (!this.filters.index || sdo.index.toString(16).toLowerCase().includes(this.filters.index.toLowerCase())) &&
                     // (!this.filters.datatype || sdo.datatype.toLowerCase().includes(this.filters.datatype.toLowerCase())) &&
                     // (!this.filters.slave || sdo.address.slave.toLowerCase().includes(this.filters.slave.toLowerCase()))
                     // );
             },
+            addManualIndex: function(){
+                if (this.manualIndex)
+                {
+                    this.$emit("selected", parseInt(this.manualIndex, 16));
+                }
+            }
         },
         template: `
 <sdoselect class="flex" style="flex-direction: column;">
+    <fieldset>
+        <input type="text" pattern="[a-fA-F0-9]{4}" v-model="manualIndex">
+        <button @click="addManualIndex();">+</button>
+    </fieldset>
     <input type="text" v-model="filters.name">
     <select size="15" v-model="selectedIndex" @dblclick="if (selectedIndex>=0) $emit('selected', selectedIndex);">
         <option

ln.ethercat.service/www/static/pages/controller.html

@@ -3,11 +3,6 @@
     <p>
         {{ controller }}
     </p>
-    <div>
-        <router-link
-            v-for="dc,idx in controller.DriveControllers"
-            :to="'/controller/drives/' + idx">Drive Controller {{ idx }}</router-link>
-    </div>
     <div v-if="drive_controller">
         <h2>Drive Controller</h2>
         <button @click="drive_poweroff">Power Off</button>
@@ -27,9 +22,9 @@
         <div>
             <fieldset>
                 <label>Target Torque:</label>
-                <input type="range" min="-1000" max="1000" v-model="targetTorque" id="targetTorque" @input="setProperty('TargetTorque', targetTorque);">
-                <input type="number" v-model="targetTorque" @change="setProperty('TargetTorque', targetTorque);">
-                <button @click="targetTorque = 0; setProperty('TargetTorque', targetTorque);">ZERO</button>
+                <input type="range" min="-1000" max="1000" v-model="targetTorque" id="targetTorque" @input="setProperty('TargetValue', targetTorque);">
+                <input type="number" v-model="targetTorque" @change="setProperty('TargetValue', targetTorque);">
+                <button @click="targetTorque = 0; setProperty('TargetValue', targetTorque);">ZERO</button>
             </fieldset>
         </div>
 

ln.ethercat.service/www/static/pages/pdo.html

@@ -1,4 +1,6 @@
 <div>
     <h1>Prozessdaten</h1>
-    <sdo-table :sdolist="this.processdata"></sdo-table>
+    <sdo-table
+        :sdolist="this.processdata"
+        ></sdo-table>
 </div>

ln.ethercat.service/www/static/pages/slave.html

@@ -7,6 +7,9 @@
                 @selected="$ECAPP.subscribe(slave_id, $event);"
                 ></sdo-select>
         </div>
-        <sdo-panel :subscribed="$ECAPP.subscribed.sdo"></sdo-panel>
+        <sdo-panel
+            :subscribed="$ECAPP.subscribed.sdo"
+            @write="$ECAPP.sendMessage('sdowrite', $event);"
+            ></sdo-panel>
     </div>
 </div>

ln.ethercat/ECDataTypeConverter.cs

@@ -159,7 +159,7 @@ namespace ln.ethercat
                 ));
 
             AddConverter(ECDataTypes.NONE, new ECValueConverterAdapter(
-                typeof(byte[]),
+                typeof(string),
                 (object value, out byte[] ethercatBytes) => { ethercatBytes = Extensions.BytesFromHexString(value as string); return true; },
                 (byte[] ethercatBytes, out object value) => { value = ethercatBytes.ToHexString(); return true; }
                 ));

ln.ethercat/ECMBind.cs

@@ -41,7 +41,7 @@ namespace ln.ethercat
     }
 
     public delegate void cb_enum_indeces(int slave, int index);
-    public delegate void cb_enum_sdo_descriptors(int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, String name);
+    public delegate void cb_enum_sdo_descriptors(int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, ushort bitlength, String name);
     public delegate void cb_enum_pdo(UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength);
 
 
@@ -106,6 +106,12 @@ namespace ln.ethercat
         [DllImport("lib/libecmbind.so")]
         public static extern int ecmbind_sdo_write(int slave, int index, int subindex, byte[] buffer, int size);
 
+        [DllImport("lib/libecmbind.so")]
+        public static extern int ecmbind_sdo_read_ca(int slave, int index, int subindex, byte[] buffer, int size);
+
+        [DllImport("lib/libecmbind.so")]
+        public static extern int ecmbind_sdo_write_ca(int slave, int index, int subindex, byte[] buffer, int size);
+
 
 
         [DllImport("lib/libecmbind.so")]

ln.ethercat/ECMaster.cs

@@ -1,13 +1,13 @@
 
 
 using System;
-using System.Buffers;
 using System.Collections.Generic;
+using System.Diagnostics;
 using System.IO;
 using System.Linq;
-using System.Reflection.Metadata;
-using System.Security.Cryptography.X509Certificates;
 using System.Threading;
+using ln.ethercat.controller;
+using ln.ethercat.controller.drives;
 using ln.logging;
 using ln.type;
 
@@ -21,6 +21,23 @@ namespace ln.ethercat
         RUNNING,
     }
 
+    public struct PDOMappingRequest
+    {
+        public ushort Slave;
+        public ushort Index;
+        public byte SubIndex;
+        public bool RxPDO;
+
+        public PDOMappingRequest(ushort slave,ushort index,byte subIndex,bool rxpdo)
+        {
+            Slave = slave;
+            Index = index;
+            SubIndex = subIndex;
+            RxPDO = rxpdo;
+        }
+
+    }
+
     public delegate void ECStateChange(ECMaster sender,ECSlaveState newState);
 
     public class ECMaster
@@ -47,8 +64,8 @@ namespace ln.ethercat
 
         ECSlaveState ethercatState = ECSlaveState.NONE;
         public ECSlaveState EthercatState { 
-                get => UpdateEthercatState();
-                set {
+                get =>ethercatState;
+                private set {
                     if (value != ethercatState)
                     {
                         OnStateChange?.Invoke(this, value);
@@ -73,113 +90,168 @@ namespace ln.ethercat
             Logging.Log(LogLevel.INFO, "ecmbind_initialize({0}) = {1}", interfaceName, result);
             if (result<=0)
                 throw new Exception("ecmbind_initialize failed");
+
+            Controller = new Controller(this);
         }
 
-        bool stopProcessing;
-        public bool StopProccessing {
-            get => stopProcessing;
-            set => stopProcessing = value;
-        }
+        bool stopWatchdog;
+        bool stopProcessData;
 
         Thread threadProcessData;
         Thread threadWatchdog;
 
-        public bool Start()
+        public Controller Controller { get; }
+        List<DriveController> driveControllers = new List<DriveController>();
+        public DriveController[] DriveControllers => driveControllers.ToArray();
+
+
+        public void Start()
         {
-            if ((threadProcessData?.IsAlive ?? false) || (threadWatchdog?.IsAlive ?? false))
-                throw new Exception("already started");
-            
-            stopProcessing = false;
-
-            EthercatState = ECSlaveState.BOOT;
-            ExpectedWorkCounter = 0;
-
-            EthercatState = ECSlaveState.INIT;
             lock (this)
             {
-                CountSlaves = ECMBind.ecmbind_config_init();
-                if (CountSlaves <= 0)
-                {
-                    Logging.Log(LogLevel.DEBUG, "ECMaster: no slaves connected");
-                    return false;
-                }
+                if (threadWatchdog?.IsAlive ?? false)
+                    throw new Exception("already started");
 
-                if (!CheckState(ECSlaveState.PRE_OP, TIMEOUT_PREOP))
-                    return false;
+                ethercatState = ECSlaveState.NONE;
+                ExpectedWorkCounter = 0;
 
-                IOMapSize = ECMBind.ecmbind_config_map();
-                Logging.Log(LogLevel.DEBUG, "ECMaster: IOMapSize={0}", IOMapSize);
-
-                UpdatePDOMap();
-                
-                threadProcessData = new Thread(MasterThread);
-                threadProcessData.Start();
-
-                Thread.Sleep(20);
-
-                if (!RequestState(ECSlaveState.SAFE_OP, out ECSlaveState slaveState, TIMEOUT_SAFEOP))
-                {
-                    Stop();
-                    return false;
-                }
+                stopWatchdog = false;
 
                 threadWatchdog = new Thread(Watchdog);
                 threadWatchdog.Start();
-
             }
-            return true;
         }
 
         public void Stop()
         {                
-            if (threadProcessData?.IsAlive ?? false)
+            lock (this)
             {
-                stopProcessing = true;
-                ECMBind.ecmbind_request_state(0, ECSlaveState.SAFE_OP, TIMEOUT_BACKTO_SAFEOP);
-
-                threadProcessData.Join();
-                threadProcessData = null;
-
-                if (!Thread.CurrentThread.Equals(threadWatchdog))
+                if (threadWatchdog?.IsAlive ?? false)
                 {
-                    threadWatchdog?.Join();
-                    threadWatchdog = null;
+                    stopWatchdog = true;
+
+                    lock (this)
+                        Monitor.PulseAll(this);
+
+                    threadWatchdog.Join();
                 }
             }
         }
-        
-
-        object lockIOMap = new object();
-        byte[] iomap = new byte[8192];
 
         void Watchdog()
         {
-                // if (!ReadSDOIndeces())
-                // {
-                //     Logging.Log(LogLevel.WARNING, "ECMaster: could not read SDO indeces");
-                // }
-
-                if (!RequestState(ECSlaveState.OPERATIONAL, out ECSlaveState slaveState, TIMEOUT_SAFEOP))
+            while (!stopWatchdog)
+            {
+                IOMapSize = 0;
+                if (threadProcessData?.IsAlive ?? false)
                 {
-                    if (slaveState < ECSlaveState.SAFE_OP)
-                        Stop();
+                    stopProcessData = true;
+                    while (threadProcessData.IsAlive)
+                        threadProcessData.Join(250);
+                }
+                stopProcessData = false;
+
+                EthercatState = ECSlaveState.INIT;
+
+                CountSlaves = ECMBind.ecmbind_config_init();
+                if (CountSlaves <= 0)
+                {
+                    Logging.Log(LogLevel.DEBUG, "ECMaster: no slaves connected, scheduling restart...");
+                    Thread.Sleep(2500);
+                    continue;
                 }
 
-                while (!stopProcessing)
+
+                if (!WaitForState(ECSlaveState.PRE_OP, TIMEOUT_PREOP))
                 {
-                    UpdateEthercatState();
-                    Thread.Sleep(250);
+                    Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach PRE_OP. restart...");
+                    Thread.Sleep(2500);
+                    continue;
                 }
+
+
+                /* Bus State PRE_OP */
+                ScanControllers();
+
+                EthercatState = ECSlaveState.PRE_OP;
+
+                ConfigureProcessDataMappings();
+                SetupPDOMapping();
+                UpdatePDOMap();
+                
+                threadProcessData = new Thread(ProcessData);
+                threadProcessData.Start();
+
+                Thread.Sleep(20);
+                RequestSlaveState(0, ECSlaveState.SAFE_OP);
+
+                if (!WaitForState(ECSlaveState.SAFE_OP, TIMEOUT_SAFEOP))
+                {
+                    Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach SAFE_OP. restart...");
+                    Thread.Sleep(2500);
+                    continue;
+                }
+
+                EthercatState = ECSlaveState.SAFE_OP;
+
+                Thread.Sleep(20);
+                RequestSlaveState(0, ECSlaveState.OPERATIONAL);
+                
+                Thread.Sleep(250);
+                if (!WaitForState(ECSlaveState.OPERATIONAL, TIMEOUT_SAFEOP))
+                {
+                    Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach OPERATIONAL. restart...");
+                    Thread.Sleep(2500);
+                    continue;
+                }
+
+                EthercatState = ECSlaveState.OPERATIONAL;
+
+                /*****  we handle the controller cycle from here on *****/
+
+                Controller.Initialize();
+
+                DateTime nextControllerRun = DateTime.Now;
+                while (!stopWatchdog)
+                {
+                    EthercatState = IOLocked(()=>ECMBind.ecmbind_read_state());
+                    if (EthercatState != ECSlaveState.OPERATIONAL)
+                        break;
+
+                    DateTime currentTime = DateTime.Now;
+                    nextControllerRun += TimeSpan.FromSeconds(Controller.ControllerLoopInterval);
+                    if (currentTime > nextControllerRun)
+                    {
+                        Logging.Log(LogLevel.WARNING, "missed controller cycle");
+                        while (currentTime > nextControllerRun)
+                            nextControllerRun += TimeSpan.FromSeconds(Controller.ControllerLoopInterval);
+                    }
+                    TimeSpan timeToNextCycle = nextControllerRun - currentTime;
+                    try
+                    {
+                        Thread.Sleep(timeToNextCycle);
+                        Controller.Cycle();
+                    } catch (Exception e)
+                    {
+                        Logging.Log(e);
+                    }
+                }
+
+            }
+
         }
 
-        void MasterThread()
+        void ProcessData()
         {
-            ExpectedWorkCounter = ECMBind.ecmbind_get_expected_wkc_size();
+            int wkc;
 
-            while (!stopProcessing)
+            lock (lockIOMap)
+                ExpectedWorkCounter = ECMBind.ecmbind_get_expected_wkc_size();
+
+            Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): start");
+
+            while (!stopProcessData)
             {
-                int wkc;
-
                 lock (lockIOMap)
                 {
                     wkc = ECMBind.ecmbind_processdata2(iomap, iomap.Length);
@@ -187,26 +259,160 @@ namespace ln.ethercat
                     {
                         int success = ECMBind.ecmbind_recover();
                         if (success > 0)
-                            ExpectedWorkCounter = success;
-                        else
                         {
-                            RequestSlaveState(0, ECSlaveState.SAFE_OP);
+                            Logging.Log(LogLevel.ERROR, "ECMaster: bus recovery successfull...");
+                            ExpectedWorkCounter = success;
+                            //SetupPDOMapping();
+                        } else
+                        {
+                            RequestSlaveState(0, ECSlaveState.INIT);
+                            Logging.Log(LogLevel.ERROR, "ECMaster: bus recovery failed, scheduling restart...");
+                            ScheduleRestart();
+                            stopProcessData = true;
                         }
                     }
                 }
                 Thread.Sleep(INTERVALL_PROCESSDATA);
+            }
 
-/*
-                if (wkc != ExpectedWorkCounter)
+            Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): finished");
+        }
+
+        public void ScanControllers()
+        {
+            driveControllers.Clear();
+
+            for (UInt16 slave_id = 1; slave_id <= CountSlaves; slave_id++)
+            {
+                if (ReadSDO(slave_id, 0x1000, 0, out byte[] bDeviceType))
                 {
-                    ExpectedWorkCounter = ECMBind.ecmbind_recover();
-                    if (ExpectedWorkCounter < 0)
-                        break;
-                }
-*/
+                    ushort profilecode = BitConverter.ToUInt16(bDeviceType);
+                    Logging.Log(LogLevel.INFO, "ECMaster: ScanControllers: Slave {0} has DeviceType {1} [{2}]", slave_id, profilecode, bDeviceType.ToHexString());
 
+                    switch (profilecode)
+                    {
+                        case 402:
+                            driveControllers.Add(new CIA402Controller(this, slave_id));
+                            break;
+                    }
+                }
             }
         }
+
+
+        bool WaitForState(ECSlaveState expectedState, int timeout)
+        {
+            int waitIncrement = 100;
+
+            ECSlaveState currentState = IOLocked(()=>ECMBind.ecmbind_read_state());
+            while ((timeout > 0) && (currentState != expectedState))
+            {
+                Thread.Sleep(waitIncrement);
+                timeout -= waitIncrement;
+                currentState = IOLocked(()=>ECMBind.ecmbind_read_state());
+            }
+            return currentState == expectedState;
+        }
+
+
+
+        object lockIOMap = new object();
+        byte[] iomap = new byte[8192];
+
+        void SetupPDOMapping()
+        {
+                IOMapSize = ECMBind.ecmbind_config_map();
+                Logging.Log(LogLevel.DEBUG, "ECMaster: IOMapSize={0}", IOMapSize);
+                //UpdatePDOMap();
+        }
+
+        HashSet<PDOMappingRequest> mappingRequests = new HashSet<PDOMappingRequest>();
+
+        public void ConfigureProcessDataMappings()
+        {
+            Logging.Log(LogLevel.INFO, "configure PDO mappings");
+
+            for (UInt16 slave = 1; slave <= CountSlaves; slave++)
+            {
+                List<int> rxPDOs = new List<int>();
+                List<int> txPDOs = new List<int>();
+
+                foreach (PDOMappingRequest mappingRequest in mappingRequests)
+                {
+                    if (mappingRequest.Slave == slave)
+                    {
+                        if (GetSDOValue(mappingRequest.Slave, mappingRequest.Index, mappingRequest.SubIndex, out SDOValue sdoValue))
+                        {
+                            int mappingEntry = (mappingRequest.Index << 16) | (mappingRequest.SubIndex << 8) | sdoValue.BitLength;
+                            Logging.Log(LogLevel.DEBUG, "PDO mapping: {0:X8}", mappingEntry);
+                            
+                            if (mappingRequest.RxPDO)
+                                rxPDOs.Add(mappingEntry);
+                            else
+                                txPDOs.Add(mappingEntry);
+
+                        } else {
+                            Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not retrieve SDOValue {0}:{1:X4}.{2}", mappingRequest.Slave, mappingRequest.Index, mappingRequest.SubIndex);
+                        }
+                    }
+                }
+
+                MemoryStream rxStream = new MemoryStream();
+                rxStream.WriteByte((byte)rxPDOs.Count);
+                rxStream.WriteByte(0);
+                foreach (int pdoMapping in rxPDOs)
+                    rxStream.WriteBytes(pdoMapping.GetBytes());
+
+                Logging.Log(LogLevel.DEBUG, "RxPDO mappings for slave {0} => {1}", slave, rxPDOs.Count);
+
+                if (!WriteSDOCA(slave, 0x1600, 0, rxStream.ToArray()))
+                {
+                    Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not write RxPDO mappings for slave {0}", slave);
+                }
+
+                MemoryStream txStream = new MemoryStream();
+                txStream.WriteByte((byte)txPDOs.Count);
+                txStream.WriteByte(0);
+                foreach (int pdoMapping in txPDOs)
+                    txStream.WriteBytes(pdoMapping.GetBytes());
+
+                Logging.Log(LogLevel.DEBUG, "TxPDO mappings for slave {0} => {1}", slave, txPDOs.Count);
+
+                if (!WriteSDOCA(slave, 0x1A00, 0, txStream.ToArray()))
+                {
+                    Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not write TxPDO mappings for slave {0}", slave);
+                }
+
+                if (!WriteSDOCA(slave, 0x1c12, 0, new byte[]{ 0x01, 0x00, 0x00, 0x16 }))
+                {
+                    Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not configure syncmanager 2 PDO assignment for slave {0}", slave);
+                }
+                if (!WriteSDOCA(slave, 0x1c13, 0, new byte[]{ 0x01, 0x00, 0x00, 0x1A }))
+                {
+                    Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not configure syncmanager 3 PDO assignment for slave {0}", slave);
+                }
+            }
+        }
+
+        public void RequestPDOMapping(UInt16 slave,UInt16 index,byte subIndex, bool RxPDO) => RequestPDOMapping(new PDOMappingRequest(slave,index,subIndex,RxPDO));
+        public void RequestPDOMapping(IEnumerable<PDOMappingRequest> mappingRequests) => RequestPDOMapping(mappingRequests.ToArray());
+        public void RequestPDOMapping(params PDOMappingRequest[] mappingRequests)
+        {
+            foreach (PDOMappingRequest mappingRequest in mappingRequests)
+                this.mappingRequests.Add(mappingRequest);
+        }
+
+
+        void ScheduleRestart()
+        {
+            ThreadPool.QueueUserWorkItem((o)=>{
+                Stop();
+                Thread.Sleep(500);
+                Start();
+            });
+        }
+
+
         
         public bool ReadSDO(UInt16 slave,UInt16 index, byte subIndex, out byte[] rawValue)
         {
@@ -220,7 +426,27 @@ namespace ln.ethercat
                     ECMBind.ecmbind_sdo_read(slave, index, subIndex, rawValue, rawValue.Length );
                 if (dataSize <= 0)
                 {
-                    Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read({0},{1},{2},..,{3}) == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString());
+                    Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read({0},{1},{2},..,{3}) [{5}] == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString(), dataSize);
+                    throw new IOException();
+                }
+            }
+            rawValue = rawValue.Slice(0, dataSize);
+            return true;
+        }
+
+        public bool ReadSDOCA(UInt16 slave,UInt16 index, byte subIndex, out byte[] rawValue)
+        {
+            rawValue =
+                new byte[128];
+            int dataSize;
+
+            lock (lockIOMap)
+            {
+                dataSize =
+                    ECMBind.ecmbind_sdo_read_ca(slave, index, subIndex, rawValue, rawValue.Length );
+                if (dataSize <= 0)
+                {
+                    Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read_ca({0},{1},{2},..,{3}) == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString());
                     throw new IOException();
                 }
             }
@@ -233,41 +459,20 @@ namespace ln.ethercat
             int result;
             lock (lockIOMap)
                 result = ECMBind.ecmbind_sdo_write(slave, index, subIndex, rawValue, rawValue.Length);
-            Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDO({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
+            //Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDO({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
+            return result > 0;
+        }
+        public bool WriteSDOCA(UInt16 slave,UInt16 index, byte subIndex, byte[] rawValue)
+        {
+            int result;
+            lock (lockIOMap)
+                result = ECMBind.ecmbind_sdo_write_ca(slave, index, subIndex, rawValue, rawValue.Length);
+            //Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDOCA({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
             return result > 0;
         }
 
-        public ECSlaveState ReadSlaveState(int slave) => ECMBind.ecmbind_get_slave_state(slave);
-
-        private ECSlaveState UpdateEthercatState()
-        {
-            EthercatState = ECMBind.ecmbind_read_state();
-            return ethercatState;
-        }
-
-        public bool CheckState(ECSlaveState minimumState, int timeout)
-        {
-            for (; timeout > 0; timeout -= 10)
-            {
-                ECSlaveState state = ECMBind.ecmbind_read_state();
-                if (state >= minimumState)
-                {
-                    EthercatState = state;
-                    return true;
-                }
-                Thread.Sleep(10);
-            }
-            return false;
-        }
-        public bool RequestState(ECSlaveState requestedState, out ECSlaveState reachedState, int timeout)
-        {
-            reachedState = ECMBind.ecmbind_request_state(0, requestedState, timeout);
-            Logging.Log(LogLevel.DEBUG, "ECMaster.RequestState({1}): lowest slave state: {0}", reachedState, requestedState);
-            EthercatState = reachedState;
-            return (reachedState >= requestedState);
-        }
-
-        public int RequestSlaveState(int slave, ECSlaveState slaveState) => ECMBind.ecmbind_write_slave_state(slave, slaveState);        
+        public ECSlaveState ReadSlaveState(int slave) => IOLocked(()=>ECMBind.ecmbind_get_slave_state(slave));
+        int RequestSlaveState(int slave, ECSlaveState slaveState) => IOLocked(()=>ECMBind.ecmbind_write_slave_state(slave, slaveState));
 
 
         public bool GetSlave(UInt16 slave_id, out ECSlave slave)
@@ -297,6 +502,7 @@ namespace ln.ethercat
             if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
             {
                 rawData = iomap.Slice(offset, length);
+                //Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1})", offset, length);
                 return true;
             }
             Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1}) failed", offset, length);
@@ -308,7 +514,7 @@ namespace ln.ethercat
             lock (lockIOMap)
             if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
             {
-                Logging.Log(LogLevel.DEBUG, "ECMaster: SetIOmapData({0},{1},{2})", offset, length, rawData.ToHexString());
+                //Logging.Log(LogLevel.DEBUG, "ECMaster: SetIOmapData({0},{1},{2})", offset, length, rawData.ToHexString());
                 Buffer.BlockCopy(rawData, 0, iomap, offset, length);
                 return true;
             }
@@ -334,18 +540,19 @@ namespace ln.ethercat
         public void UpdatePDOMap()
         {
             List<PDO> pdoList = new List<PDO>();
-            ECMBind.ecmbind_pdo_enumerate((UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength)=>{
-                if (addr_bit != 0)
-                    Logging.Log(LogLevel.WARNING, "currently only PDO mappings on byte boundaries are supported");
-                else
-                    pdoList.Add(new PDO(this,slave, index, subindex){
-                        AddressOffset = addr_offset,
-                        AddressBit = addr_bit,
-                        BitLength = bitlength
-                    });
+            lock (lockIOMap)
+                ECMBind.ecmbind_pdo_enumerate((UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength)=>{
+            if (addr_bit != 0)
+                Logging.Log(LogLevel.WARNING, "currently only PDO mappings on byte boundaries are supported");
+            else
+                pdoList.Add(new PDO(this,slave, index, subindex){
+                    AddressOffset = addr_offset,
+                    AddressBit = addr_bit,
+                    BitLength = bitlength
+                });
             });
 
-            lock (this)
+            lock (lockIOMap)
             {
                 IOMapPtr = ECMBind.ecmbind_get_iomap();
                 pdoMap = pdoList;

ln.ethercat/ECSlave.cs

@@ -58,7 +58,7 @@ namespace ln.ethercat
 
         public void Update()
         {
-                if (ECMBind.ecmbind_read_objectdescription(Slave.Id, Index, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, String name)=>{
+                if (ECMBind.ecmbind_read_objectdescription(Slave.Id, Index, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, ushort bitlength, String name)=>{
                     Name = name;
                     DataType = dataType;
                     ObjectCode = objectCode;
@@ -105,6 +105,7 @@ namespace ln.ethercat
     {
         public SDODescriptor Descriptor;
         public byte SubIndex { get; }
+        public ushort BitLength { get; private set; }
 
         public string Name { get; private set; }
         public ECDataTypes DataType { get; private set; }
@@ -114,13 +115,14 @@ namespace ln.ethercat
             Descriptor = descriptor;
             SubIndex = subIndex;
 
-            if (SubIndex == 0)
+            if ((Descriptor.MaxSubIndex > 0) && (SubIndex == 0))
             {
                 DataType = descriptor.DataType;
                 Name = descriptor.Name;
             } else {
-                if (ECMBind.ecmbind_read_objectdescription_entry(Descriptor.SlaveId, descriptor.Index, SubIndex, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int si, String name)=>{
-                    Name = name;
+                if (ECMBind.ecmbind_read_objectdescription_entry(Descriptor.SlaveId, descriptor.Index, SubIndex, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int si, ushort bitlength, String name)=>{
+                    Name = (SubIndex == 0) ? descriptor.Name : name;
+                    BitLength = bitlength;
                     DataType = dataType;
                 }) <= 0)
                 {

ln.ethercat/controller/Controller.cs

@@ -1,4 +1,5 @@
 
+using System;
 using System.Collections.Generic;
 using System.Threading;
 using ln.ethercat.controller.drives;
@@ -8,98 +9,225 @@ namespace ln.ethercat.controller
 {
 
     public delegate void ControllerLogicDelegate(Controller controller);
+    public delegate void ControllerStateChangeDelegate(Controller controller, ControllerStates newState);
+
+    public enum ControllerStates {
+        NONE,
+        NOTREADY,
+        FAULT,
+        READY,
+        DISABLING,
+        ENABLING,
+        OPERATIONAL
+    }
 
     public class Controller
     {
         public event ControllerLogicDelegate ControllerLogic;
+        public event ControllerStateChangeDelegate OnStateChanging;
+        public event ControllerStateChangeDelegate OnStateChanged;
 
+        public ECMaster ECMaster { get; }
 
-        List<DriveController> driveControllers = new List<DriveController>();
-        public DriveController[] DriveControllers => driveControllers.ToArray();
+        public bool IgnoreRemoteInterface { get; set; }
 
         List<ControlLoop> controlLoops = new List<ControlLoop>();
         public ControlLoop[] ControlLoops => controlLoops.ToArray();
 
-        public bool IsRunning => threadController?.IsAlive ?? false;
+        public ControllerStates ControllerState { get; private set; }
 
         public double ControllerLoopInterval { get; set; } = 0.1;
         public double ControllerLoopFrequency {
             get => 1.0 / ControllerLoopInterval;
             set => ControllerLoopInterval = 1.0 / value;
         }
+        public long CycleCounter { get; private set; }
 
-        bool stopRequested;
-        Thread threadController;
+        Thread threadWatchdog;
+        int wdogCounter;
+        public int WatchdogReset { get; set; } = 5;
 
-        public Controller()
+        public Controller(ECMaster ecMaster)
         {
+            ECMaster = ecMaster;
         }
 
-        public void Start()
-        {
-            if (threadController?.IsAlive ?? false)
-            {
-                Logging.Log(LogLevel.WARNING, "Controller; Start(): already started");
-            } else {
-                stopRequested = false;
-
-                foreach (DriveController driveController in driveControllers)
-                    driveController.Initialize();
-
-                threadController = new Thread(ControllerThread);
-                threadController.Start();
-            }
-        }
-
-        public void Stop()
-        {
-            if (threadController?.IsAlive ?? false)
-            {
-                stopRequested = true;
-                threadController.Join();
-                threadController = null;
-            }
-        }
-
-        public void Add(DriveController driveController) => driveControllers.Add(driveController);
-        public void Remove(DriveController driveController) => driveControllers.Remove(driveController);
-
         public void Add(ControlLoop controlLoop) => controlLoops.Add(controlLoop);
         public void Remove(ControlLoop controlLoop) => controlLoops.Remove(controlLoop);
 
-        public DriveStates DrivesState {
-            get {
-                DriveStates lowestState = DriveStates.OPERATIONAL;
-                foreach (DriveController driveController in driveControllers)
-                {
-                    DriveStates driveState = driveController.DriveState;
-                    if (driveState < lowestState)
-                        lowestState = driveState;
-                }
-                return lowestState;
-            }
-        }
-
-        void ControllerThread()
+        public void Initialize()
         {
-            while (!stopRequested)
+            CycleCounter = 0;
+
+            if (!(threadWatchdog?.IsAlive ?? false))
             {
-                foreach (DriveController driveController in driveControllers)
-                    driveController.UpdateStates();
+                threadWatchdog = new Thread(Watchdog);
+                threadWatchdog.Start();
+            }
 
-                foreach (ControlLoop controlLoop in controlLoops)
-                    controlLoop.Loop();
+            foreach (DriveController driveController in ECMaster.DriveControllers)
+                driveController.Initialize();
 
-                ControllerLogic?.Invoke(this);
+        }
 
-                foreach (DriveController driveController in driveControllers)
-                    driveController.UpdateDrive();
+        void UpdateControllerState()
+        {
+            ControllerStates nextState = ControllerStates.OPERATIONAL;
 
-                Thread.Sleep((int)(1000.0 * ControllerLoopInterval));
+            foreach (DriveController driveController in ECMaster.DriveControllers)
+            {
+                if (!driveController.IgnoredByController)
+                {
+                    ControllerStates driveState;
+                    switch (driveController.DriveState)
+                    {
+                        case DriveStates.OPERATIONAL:
+                            driveState = ControllerStates.OPERATIONAL;
+                            break;
+                        case DriveStates.ERROR:
+                            driveState = ControllerStates.FAULT;
+                            break;
+                        case DriveStates.INIT:
+                        case DriveStates.BOOT:
+                            driveState = ControllerStates.NOTREADY;
+                            break;
+                        default:
+                            driveState = ControllerStates.READY;
+                            break;
+                    }
+                    if (driveState < nextState)
+                        nextState = driveState;
+                }
+            }
+            ChangeState(nextState);
+        }
+
+        void ChangeState(ControllerStates newState)
+        {
+            if (newState == ControllerState)
+                return;
+
+            OnStateChanging?.Invoke(this, newState);
+
+            switch (newState)
+            {
+                case ControllerStates.FAULT:
+                    DisableDrives();
+                    break;
+                case ControllerStates.DISABLING:
+                    foreach (DriveController driveController in ECMaster.DriveControllers)
+                        driveController.DisableDrive();
+                    break;
+                case ControllerStates.ENABLING:
+                    foreach (DriveController driveController in ECMaster.DriveControllers)
+                        if (!driveController.IgnoredByController)
+                            driveController.EnableDrive();
+                    break;
+            }
+            
+            ControllerState = newState;
+            OnStateChanged?.Invoke(this, newState);
+        }
+
+        public void DisableDrives()
+        {
+            ChangeState(ControllerStates.DISABLING);
+        }
+
+        public void EnableDrives()
+        {
+            if (ControllerState == ControllerStates.READY)
+            {
+                ChangeState(ControllerStates.ENABLING);
+            } else {
+                Logging.Log(LogLevel.INFO, "Controller: EnableDrives(): Current ControllerState={0}. Refusing to enable drives", ControllerState.ToString());
+            }
+        }
+
+        public void ClearFaults()
+        {
+            foreach (DriveController driveController in ECMaster.DriveControllers)
+            {
+                if (driveController.DriveState == DriveStates.ERROR)
+                    driveController.ClearFault();
+            }
+        }
+
+        public void RemoteAction(CRActions action)
+        {
+            if (!IgnoreRemoteInterface)
+            {
+                switch (action)
+                {
+                    case CRActions.CLEARFAULT:
+                        ClearFaults();
+                        break;
+                    case CRActions.DISABLE:
+                        DisableDrives();
+                        break;
+                    case CRActions.ENABLE:
+                        EnableDrives();
+                        break;
+                }
+            }
+        }
+
+        public void RemoteUpdateTarget(int drive, double targetValue)
+        {
+            if (!IgnoreRemoteInterface)
+            {
+                targetValue = Math.Clamp(targetValue, 0, 1);
+
+                if (drive < 0)
+                    throw new ArgumentOutOfRangeException(nameof(targetValue));
+                if (drive > ECMaster.DriveControllers.Length)
+                {
+                    Logging.Log(LogLevel.WARNING, "Controller: RemoteUpdateTarget(): trying to update non-existent drive {0}", drive);
+                    return;
+                }
+
+                ECMaster.DriveControllers[drive].TargetValue = targetValue;
             }
         }
 
 
+        public void Cycle()
+        {
+            wdogCounter = WatchdogReset;
+            CycleCounter++;
+
+            foreach (DriveController driveController in ECMaster.DriveControllers)
+                driveController.UpdateStates();
+
+            UpdateControllerState();
+
+            foreach (ControlLoop controlLoop in controlLoops)
+                controlLoop.Loop();
+
+            ControllerLogic?.Invoke(this);
+
+            foreach (DriveController driveController in ECMaster.DriveControllers)
+                driveController.UpdateDrive();
+        }
+
+        void Watchdog()
+        {
+            while (true)
+            {
+                try
+                {
+                    Thread.Sleep(TimeSpan.FromSeconds(ControllerLoopInterval));
+                    if ((wdogCounter--) < 0)
+                    {
+                        ChangeState(ControllerStates.FAULT);
+                    }
+                } catch (Exception e)
+                {
+                    Logging.Log(e);
+                }
+            }
+
+        }
 
     }
 }

ln.ethercat/controller/ControllerRemote.cs

@@ -0,0 +1,105 @@
+
+
+using System;
+using System.Threading;
+using ln.ethercat.controller.drives;
+using ln.logging;
+
+namespace ln.ethercat.controller
+{
+    public enum CRActions {
+        NONE,           // No Action
+        ENABLE,         // Enable system (turn on)
+        DISABLE,        // Disable system (turn off)
+        CLEARFAULT,     // Clear fault state        
+    }
+
+    public abstract class ControllerRemote
+    {   
+        protected Controller Controller;
+
+        public double CycleIntervall { get; set; }
+        public double CycleFrequency {
+            get => 1.0 / CycleIntervall;
+            set => CycleIntervall = 1.0 / value;
+        }
+
+        public long CycleCounter { get; private set; }
+
+        public bool IsRunning => threadCycle?.IsAlive ?? false;
+
+        bool stopCycleThread;
+        Thread threadCycle;
+
+        public ControllerRemote(Controller controller)
+        {
+            Controller = controller;
+        }
+
+        public void Start()
+        {
+            lock (this)
+            {
+                if (threadCycle?.IsAlive ?? false)
+                    Stop();
+
+                threadCycle = new Thread(CycleThread);
+                threadCycle.Start();
+            }
+        }
+
+        public void Stop()
+        {
+            lock (this)
+            {
+                if (threadCycle?.IsAlive ?? false)
+                {
+                    stopCycleThread = true;
+                    threadCycle.Join(250);
+                    if (threadCycle.IsAlive)
+                        threadCycle.Abort();
+                    threadCycle = null;
+                }
+            }
+        }
+
+        public void SetTargetValue(int drive, double targetValue) => Controller.RemoteUpdateTarget( drive, targetValue );
+        public void Action(CRActions action) => Controller.RemoteAction(action);
+
+        protected abstract void Initialize();
+        protected abstract void Cycle();
+        protected abstract void Shutdown();
+
+        private void CycleThread()
+        {
+            CycleCounter = 0;
+            Initialize();
+
+            DateTime nextCycleRun = DateTime.Now;
+            while (!stopCycleThread)
+            {
+                DateTime currentTime = DateTime.Now;
+                nextCycleRun += TimeSpan.FromSeconds(CycleIntervall);
+                if (currentTime > nextCycleRun)
+                {
+                    Logging.Log(LogLevel.WARNING, "ControllerRemote: missed controller cycle");
+                    while (currentTime > nextCycleRun)
+                        nextCycleRun += TimeSpan.FromSeconds(CycleIntervall);
+                }
+                TimeSpan timeToNextCycle = nextCycleRun - currentTime;
+                try
+                {
+                    Thread.Sleep(timeToNextCycle);
+                    CycleCounter++;
+                    Cycle();
+                } catch (Exception e)
+                {
+                    Logging.Log(e);
+                }
+            }
+
+            Shutdown();
+        }
+
+    }
+}

ln.ethercat/controller/drives/CIA402Controller.cs

@@ -41,11 +41,30 @@ namespace ln.ethercat.controller.drives
         SDOValue svActualTorque;
         SDOValue svModesOfOperation;
         SDOValue svModesOfOperationDisplay;
+        SDOValue svActualCurrent;
 
+        SDOValue svMaxSpeed;
+
+        UInt32 MotorMaxSpeed = 1000;
         
         public CIA402Controller(ECMaster ecMaster,UInt16 slave)
         :base(ecMaster, slave)
         {
+            PDOMappingRequest[] mappingRequests = new PDOMappingRequest[]{
+                new PDOMappingRequest(slave, 0x603F, 0, false),
+                new PDOMappingRequest(slave, 0x6040, 0, true),
+                new PDOMappingRequest(slave, 0x6041, 0, false),
+                new PDOMappingRequest(slave, 0x607A, 0, true),
+                new PDOMappingRequest(slave, 0x60FF, 0, true),
+                new PDOMappingRequest(slave, 0x6071, 0, true),
+                new PDOMappingRequest(slave, 0x6064, 0, false),
+                new PDOMappingRequest(slave, 0x606C, 0, false),
+                new PDOMappingRequest(slave, 0x6077, 0, false),
+                new PDOMappingRequest(slave, 0x6060, 0, true),
+                new PDOMappingRequest(slave, 0x6061, 0, false),
+                new PDOMappingRequest(slave, 0x6078, 0, false)
+            };
+            ecMaster.RequestPDOMapping(mappingRequests);
         }
 
         public override void Initialize()
@@ -61,23 +80,82 @@ namespace ln.ethercat.controller.drives
                 ECMaster.GetSDOValue(Slave, 0x606C, 0, out svActualSpeed) &&
                 ECMaster.GetSDOValue(Slave, 0x6077, 0, out svActualTorque) &&
                 ECMaster.GetSDOValue(Slave, 0x6060, 0, out svModesOfOperation) &&
-                ECMaster.GetSDOValue(Slave, 0x6061, 0, out svModesOfOperationDisplay)
+                ECMaster.GetSDOValue(Slave, 0x6061, 0, out svModesOfOperationDisplay) &&
+                ECMaster.GetSDOValue(Slave, 0x6080, 0, out svMaxSpeed) &&
+                ECMaster.GetSDOValue(Slave, 0x6078, 0, out svActualCurrent)
             ))
                 throw new ArgumentOutOfRangeException("CIA402Controller could not retrieve SDOvalues for CiA402 profile");
         }
 
-        public override void UpdateStates() { }
-        public override void UpdateDrive() { }
+        public override void UpdateStates() {
+            CIA402State = GetCIA402State();
+        }
+        public override void UpdateDrive() {
+
+            if (CIA402State != CIA402TargetState)
+            {
+                switch (CIA402State)
+                {
+                    case CIA402States.NOT_READY_TO_SWITCH_ON:
+                        svControlWord.SetValue((ushort)0x0000);
+                        ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_TORQUE;
+                        break;
+                    case CIA402States.SWITCH_ON_DISABLED:
+                        svControlWord.SetValue((ushort)0x0006);
+                        break;
+                    case CIA402States.READY_TO_SWITCH_ON:
+                        switch (CIA402TargetState)
+                        {
+                            case CIA402States.SWITCH_ON_DISABLED:
+                                svControlWord.SetValue((ushort)0x0000);
+                                break;
+                            case CIA402States.SWITCHED_ON:
+                            case CIA402States.OPERATION_ENABLED:
+                                svControlWord.SetValue((ushort)0x0007);
+                                break;
+                        }
+                        break;
+                    case CIA402States.SWITCHED_ON:
+                        switch (CIA402TargetState)
+                        {
+                            case CIA402States.SWITCH_ON_DISABLED:
+                            case CIA402States.READY_TO_SWITCH_ON:
+                                svControlWord.SetValue((ushort)0x0006);
+                                break;
+                            case CIA402States.OPERATION_ENABLED:
+                                svControlWord.SetValue((ushort)0x000F);
+                                break;
+                        }
+                        break;
+                    case CIA402States.OPERATION_ENABLED:
+                        switch (CIA402TargetState)
+                        {
+                            case CIA402States.SWITCH_ON_DISABLED:
+                            case CIA402States.READY_TO_SWITCH_ON:
+                                svControlWord.SetValue((ushort)0x0006);
+                                break;
+                            case CIA402States.SWITCHED_ON:
+                                svControlWord.SetValue((ushort)0x0007);
+                                break;
+                        }
+                        break;
+                }
+            }
+
+            if (DriveMode == DriveMode.UNDEFINED)
+                DriveMode = DriveMode.TORQUE;
+        }
 
         public override DriveStates DriveState 
         {
             get {
-                switch (GetCIA402State())
+                switch (CIA402State)
                 {
                     case CIA402States.NOT_READY_TO_SWITCH_ON:
+                        return DriveStates.INIT;
                     case CIA402States.SWITCH_ON_DISABLED:
                     case CIA402States.READY_TO_SWITCH_ON:
-                        return DriveStates.INIT;
+                        return DriveStates.READY;
                     case CIA402States.SWITCHED_ON:
                     case CIA402States.QUICK_STOP_ACTIVE:
                         return DriveStates.POWERED;
@@ -93,7 +171,7 @@ namespace ln.ethercat.controller.drives
         }
 
         public override string OEMDriveState {
-            get => GetCIA402State().ToString();
+            get => CIA402State.ToString();
         }
         public override string OEMDriveMode => ModeOfOperation.ToString();
 
@@ -135,26 +213,27 @@ namespace ln.ethercat.controller.drives
         public void SetDriveModeTorque() => DriveMode = DriveMode.TORQUE;
 
         public CIA402ModesOfOperation ModeOfOperation {
-            get => (CIA402ModesOfOperation)(svModesOfOperationDisplay.GetValue<sbyte>());
+            get => (ECMaster.EthercatState==ECSlaveState.OPERATIONAL) ? (CIA402ModesOfOperation)(svModesOfOperationDisplay?.GetValue<sbyte>() ?? 0) : CIA402ModesOfOperation.NO_MODE_CHANGE;
             set => svModesOfOperation.SetValue((sbyte)value);
         }
 
-        public override decimal ActualPosition => svActualPosition.GetValue<int>();
-        public override decimal ActualSpeed => svActualSpeed.GetValue<int>();
-        public override decimal ActualTorque => svActualTorque.GetValue<short>();
+        public override double ActualPosition => (svActualPosition?.GetValue<int>() ?? 0) ;
+        public override double ActualSpeed => (double)(svActualSpeed?.GetValue<int>() ?? 0) / (1000.0 * MotorMaxSpeed);
+        public override double ActualTorque => (double)(svActualTorque?.GetValue<short>() ?? 0)  / 1000.0;
+        public override double ActualLoad => (double)(svActualCurrent?.GetValue<short>() ?? 0)  / 1000.0;
 
-        public override decimal TargetPosition {
+        public override double TargetPosition {
             get => svTargetPosition.GetValue<int>();
             set => svTargetPosition.SetValue((int)value);
         }
 
-        public override decimal TargetSpeed {
-            get => svTargetSpeed.GetValue<int>();
-            set => svTargetSpeed.SetValue((int)value);
+        public override double TargetSpeed {
+            get => (double)svTargetSpeed.GetValue<int>() / (1000.0 * MotorMaxSpeed);
+            set => svTargetSpeed.SetValue((int)(value * (1000.0 * MotorMaxSpeed)));
         }
-        public override decimal TargetTorque {
-            get => svTargetTorque.GetValue<short>();
-            set => svTargetTorque.SetValue((short)value);
+        public override double TargetTorque {
+            get => (double)svTargetTorque.GetValue<short>() / 1000.0;
+            set => svTargetTorque.SetValue((short)(1000 * value));
         }
 
         public override int ErrorCode => svErrorCode.GetValue<int>();
@@ -163,51 +242,12 @@ namespace ln.ethercat.controller.drives
 
         public override void Enable(bool enable)
         {
-            if (enable)
-            {
-                switch (CIA402State)
-                {
-                    case CIA402States.SWITCHED_ON:
-                        svControlWord.SetValue((UInt16)0x000F);
-                        break;
-                    default:
-                        Logging.Log(LogLevel.WARNING, "CIA402Controller: EnableDrive(): current state is {0}, can't enable drive", CIA402State.ToString());
-                        break;
-                }
-            } else {
-                switch (CIA402State)
-                {
-                    case CIA402States.OPERATION_ENABLED:
-                        svControlWord.SetValue((UInt16)0x0007);
-                        break;
-                    default:
-                        Logging.Log(LogLevel.WARNING, "CIA402Controller: EnableDrive(): current state is {0}, can't disable drive", CIA402State.ToString());
-                        break;
-                }
-            }
+            CIA402TargetState = enable ? CIA402States.OPERATION_ENABLED : CIA402States.READY_TO_SWITCH_ON;
         }
 
         public override void Power(bool poweron)
         {
-            if (poweron)
-            {
-                switch (CIA402State)
-                {
-                    case CIA402States.FAULT:
-                    case CIA402States.FAULT_REACTION_ACTIVE:
-                        Logging.Log(LogLevel.WARNING, "CIA402Controller: Power(): Drive in fault state, not ready to switch power on");
-                        break;
-                    case CIA402States.NOT_READY_TO_SWITCH_ON:
-                        Logging.Log(LogLevel.WARNING, "CIA402Controller: Power(): Drive not ready to switch power on");
-                        break;
-                    case CIA402States.SWITCH_ON_DISABLED:    
-                    case CIA402States.READY_TO_SWITCH_ON:
-                        svControlWord.SetValue((UInt16)0x0007); // Switch ON
-                        break;
-                }
-            } else {
-                svControlWord.SetValue((UInt16)0x0006);
-            }
+            CIA402TargetState = poweron ? CIA402States.SWITCHED_ON : CIA402States.READY_TO_SWITCH_ON;
         }
 
         public override void ClearFault()
@@ -220,10 +260,12 @@ namespace ln.ethercat.controller.drives
             }
         }
 
-        CIA402States CIA402State => GetCIA402State();
+        public CIA402States CIA402State { get; private set; }
+        public CIA402States CIA402TargetState { get; private set; }
+
         public CIA402States GetCIA402State()
         {
-            UInt16 statusword = svStatusWord.GetValue<ushort>();
+            UInt16 statusword = svStatusWord?.GetValue<ushort>() ?? 0;
 
             if ((statusword & 0x004F)==0)
                 return CIA402States.NOT_READY_TO_SWITCH_ON;

ln.ethercat/controller/drives/DriveController.cs

@@ -1,6 +1,7 @@
 
 
 using System;
+using System.Net.Http.Headers;
 using System.Runtime;
 
 namespace ln.ethercat.controller.drives
@@ -9,6 +10,7 @@ namespace ln.ethercat.controller.drives
         UNDEFINED,
         BOOT,
         INIT,
+        READY,
         ERROR,
         POWERED,
         OPERATIONAL,
@@ -26,6 +28,9 @@ namespace ln.ethercat.controller.drives
         protected ECMaster ECMaster { get; }
         public UInt16 Slave { get; }
 
+        public bool IgnoredByController { get; set; }
+
+
         public DriveController(ECMaster ecMaster, UInt16 slave)
         {
             ECMaster = ecMaster;
@@ -57,13 +62,46 @@ namespace ln.ethercat.controller.drives
         public void DisableDrive() => Enable(false);
         public abstract void Enable(bool enabled);
 
-        public abstract decimal ActualPosition { get; }
-        public abstract decimal ActualSpeed { get; }
-        public abstract decimal ActualTorque { get; }
-        public abstract decimal TargetPosition { get; set; }
-        public abstract decimal TargetSpeed { get; set; }
-        public abstract decimal TargetTorque { get; set; }
+        public abstract double ActualPosition { get; }
+        public abstract double ActualSpeed { get; }
+        public abstract double ActualTorque { get; }
+        public abstract double TargetPosition { get; set; }
+        public abstract double TargetSpeed { get; set; }
+        public abstract double TargetTorque { get; set; }
+
+        public abstract double ActualLoad { get; }
                 
+        public virtual double TargetValue {
+            get {
+                switch (DriveMode)
+                {
+                    case DriveMode.POSITION:
+                        return TargetPosition;
+                    case DriveMode.SPEED:
+                        return TargetSpeed;
+                    case DriveMode.TORQUE:
+                        return TargetTorque;
+                    default:
+                        return 0;
+                }
+            }
+            set {
+                switch (DriveMode)
+                {
+                    case DriveMode.POSITION:
+                        TargetPosition = value;
+                        break;
+                    case DriveMode.SPEED:
+                        TargetSpeed = value;
+                        break;
+                    case DriveMode.TORQUE:
+                        TargetTorque = value;
+                        break;
+                    default:
+                        break;
+                }
+            }
+        }
 
     }
 

ln.ethercat/controller/remote/StupidSerialRemote.cs

@@ -0,0 +1,171 @@
+
+
+using System;
+using System.IO.Ports;
+using System.Net.Http.Headers;
+using System.Text;
+using System.Threading;
+using ln.logging;
+using ln.type;
+
+namespace ln.ethercat.controller.remote
+{
+    /*
+#define LED_ERROR		0x00
+#define LED_RUN			0x01
+#define LED_LOAD25		0x02
+#define LED_LOAD50		0x03
+#define LED_LOAD75		0x04
+#define LED_LOAD100		0x05
+#define LED_SERVICE		0x06
+#define LED_AUX			0x07
+*/
+
+    [Flags]
+    public enum StupidLEDs : int
+    {
+        NONE = 0,
+        ERROR = (1<<15),
+        RUN = (1<<1),
+        LOAD25 = (1<<2),
+        LOAD50 = (1<<3),
+        LOAD75 = (1<<4),
+        LOAD100 = (1<<15),
+        ALL = -1
+    }
+    public class StupidSerialRemote : ControllerRemote
+    {
+        public string SerialPortName { get; }
+
+        SerialPort serialPort;
+
+        bool stopReceiverThread;
+        Thread threadReceiver;
+
+        public StupidSerialRemote(Controller controller)
+            :this(controller, SerialPort.GetPortNames()[0]){}
+        public StupidSerialRemote(Controller controller, string serialDevice)
+            :base(controller)
+        {
+            SerialPortName = serialDevice;
+            CycleFrequency = 10.0;
+        }
+
+        protected override void Cycle()
+        {
+            byte cycleDisplayStep = (byte)(CycleCounter & 0x0F);
+
+            switch (Controller.ControllerState)
+            {
+                case ControllerStates.NOTREADY:
+                    SetLEDs(((cycleDisplayStep & 0x07) < 0x04) ? StupidLEDs.ALL : StupidLEDs.NONE);
+                    break;
+                case ControllerStates.FAULT:
+                    SetLEDs(((cycleDisplayStep & 0x07) < 0x04) ? StupidLEDs.ERROR : StupidLEDs.NONE);
+                    break;
+                case ControllerStates.READY:
+                    SetLEDs((cycleDisplayStep < 0x08) ? StupidLEDs.RUN : StupidLEDs.NONE);
+                    break;
+                case ControllerStates.OPERATIONAL:
+                    StupidLEDs leds = StupidLEDs.RUN;
+                    if (Controller.ECMaster.DriveControllers[0].ActualLoad >= 0.25)
+                        leds |= StupidLEDs.LOAD25;
+                    if (Controller.ECMaster.DriveControllers[0].ActualLoad >= 0.5)
+                        leds |= StupidLEDs.LOAD50;
+                    if (Controller.ECMaster.DriveControllers[0].ActualLoad >= 0.75)
+                        leds |= StupidLEDs.LOAD75;
+                    if (Controller.ECMaster.DriveControllers[0].ActualLoad >= 1)
+                        leds |= StupidLEDs.LOAD100;
+                    SetLEDs(leds);
+                    break;
+                case ControllerStates.ENABLING:
+                    break;
+                case ControllerStates.DISABLING:
+                    break;
+            }
+        }
+
+        void Receiver()
+        {
+            while (!stopReceiverThread)
+            {
+                string rxLine = serialPort.ReadLine();
+               //Logging.Log(LogLevel.DEBUGDETAIL, rxLine);
+               if (rxLine.Length >= 6)
+               {
+                    ushort av = ushort.Parse(rxLine.Substring(2,4), System.Globalization.NumberStyles.HexNumber);
+                    switch (rxLine[0])
+                    {
+                        case 'A':
+                            int drive = rxLine[1] - '0';
+                            if (drive == 0) drive = 1;
+                            else if (drive == 1) drive = 2;
+                            else if (drive == 2) drive = 0;
+                            double rel = (double)av / 65535;
+                            if ((drive >= 0) && (drive < Controller.ECMaster.DriveControllers.Length))
+                                Controller.RemoteUpdateTarget(drive, rel);
+                            break;
+                        case 'B':
+                            switch (rxLine[1])
+                            {
+                                case 'P':
+                                    switch (av)
+                                    {
+                                        case 0:
+                                            if (Controller.ControllerState == ControllerStates.FAULT)
+                                                Controller.RemoteAction(CRActions.CLEARFAULT);
+                                            else if (Controller.ControllerState == ControllerStates.READY)
+                                                Controller.RemoteAction(CRActions.ENABLE);
+                                            break;
+                                        case 1:
+                                            Controller.RemoteAction(CRActions.DISABLE);
+                                            break;
+                                        case 3:
+                                            // ToDo: Feeder left
+                                            break;
+                                        case 4:
+                                            // ToDo: Feeder right
+                                            break;
+                                        case 14:
+                                            // ToDo: cycle drilling
+                                            break;
+                                    }
+                                    break;
+                            }
+                            break;
+                    }
+               }
+            }
+        }
+
+        void SetLEDs(StupidLEDs leds)
+        {
+            serialPort.Write(string.Format("LS{0:X4}\r\n", (ushort)leds));
+        }
+
+
+        protected override void Initialize()
+        {
+            stopReceiverThread = false;
+
+            serialPort = new SerialPort(SerialPortName);
+            serialPort.BaudRate = 57600;
+            serialPort.Parity = Parity.None;
+            serialPort.DataBits = 8;
+            serialPort.StopBits = StopBits.One;
+            serialPort.Open();
+
+            if (!(threadReceiver?.IsAlive ?? false))
+            {
+                threadReceiver = new Thread(Receiver);
+                threadReceiver.Start();
+            }
+        }
+
+        protected override void Shutdown()
+        {
+            stopReceiverThread = true;
+            serialPort.Close();
+        }
+    }
+}

ln.ethercat/ln.ethercat.csproj

@@ -16,6 +16,7 @@
   </ItemGroup>
 
   <ItemGroup>
+    <PackageReference Include="System.IO.Ports" Version="5.0.0" />
     <PackageReference Include="ln.type" Version="0.1.7-ci" />
     <PackageReference Include="ln.logging" Version="1.0.2" />
     <PackageReference Include="ln.json" Version="1.0.6" />