Merge branch 'master' of http://unque781.synology.me:3000/GSI/OHV

Dev/OHV/VehicleControlSystem/ControlLayer/MQ/ZmqManager.cs

@@ -6,8 +6,10 @@ using NetMQ.Monitoring;
 using NetMQ.Sockets;
 using OHV.Common.Model;
 using OHV.Common.Shareds;
+using StackExchange.Redis.Extensions.Core.Extensions;
 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
 using System.Linq;
 using System.Text;
 using System.Threading.Tasks;
@@ -41,7 +43,7 @@ namespace VehicleControlSystem.ControlLayer.MQ
         public eSteeringState RequestSteering
         {
             get { return requestSteering; }
-            set { SetField(ref this.requestSteering, value); }
+            set { SetField( ref this.requestSteering, value ); }
         }
 
         private bool isDriveMoving;
@@ -49,7 +51,7 @@ namespace VehicleControlSystem.ControlLayer.MQ
         public bool IsDriveMoving
         {
             get { return isDriveMoving; }
-            set { SetField(ref isDriveMoving, value); }
+            set { SetField( ref isDriveMoving, value ); }
         }
 
         private eRoadForm currentLoadForm;
@@ -57,9 +59,18 @@ namespace VehicleControlSystem.ControlLayer.MQ
         public eRoadForm CurrentLoadForm
         {
             get { return currentLoadForm; }
-            set { SetField(ref currentLoadForm, value); }
+            set { SetField( ref currentLoadForm, value ); }
         }
 
+        private eOperatationMode operatationMode;
+
+        public eOperatationMode OperationMode
+        {
+            get { return operatationMode; }
+            set { operatationMode = value; }
+        }
+
+
 
 
         #endregion
@@ -111,25 +122,46 @@ namespace VehicleControlSystem.ControlLayer.MQ
             var ret = e.Socket.ReceiveMultipartStrings();
             logger.D( $"[rep] - {ret}" );
 
-            if ( ret[0].Equals( "3050" ) )
+            if ( ret[0].Equals( "setm" ) )
             {
-                switch ( ret[1] )
+                var v = ret[1].Split( '/' );
+                switch ( v[0] )
                 {
-                    case "1":
-                        this.RequestSteering = eSteeringState.Left;
+                    case "3050":
+                        {
+                            switch ( v[1] )
+                            {
+                                case "1":
+                                    this.RequestSteering = eSteeringState.Left;
+                                    break;
+                                case "-1":
+                                    this.RequestSteering = eSteeringState.Right;
+                                    break;
+                                default:
+                                    this.RequestSteering = eSteeringState.None;
+                                    break;
+                            }
+                        }
+                        break;
+
+                    case "111":
+                        {
+                            if ( v[1].Equals( "1" ) )
+                                OperationMode = eOperatationMode.AutoMode;
+                            else
+                                OperationMode = eOperatationMode.ManualMode;
+                        }
                         break;
-                    case "-1":
-                        this.RequestSteering = eSteeringState.Right;
+
+                    case "113":
+                        {
+                        }
                         break;
                     default:
-                        this.RequestSteering = eSteeringState.None;
                         break;
                 }
-                rep.SendMoreFrame( "3050" ).SendFrame( "OK" );
-                return;
+                rep.SendMoreFrame( "setm" ).SendFrame( $"ok/{v[0]}" );
             }
-
-            rep.SendFrameEmpty();
         }
 
         private void Sub_ReceiveReady( object sender, NetMQ.NetMQSocketEventArgs e )
@@ -215,6 +247,125 @@ namespace VehicleControlSystem.ControlLayer.MQ
                 return false;
         }
 
+        bool SetRequest(string index, string value )
+        {
+            List<string> repll = new List<string>();
+
+            if ( !this.IsReqConnected )
+                return false;
+
+            if ( !this.req.HasIn )
+                this.req.SendMoreFrame( "setm" ).SendFrame( index + "/" + value );
+
+            if ( !this.req.TryReceiveMultipartStrings( TimeSpan.FromSeconds( 5 ), ref repll ) )
+                return false;
+
+            return true;
+        }
+
+        bool GetRequst( string index, out List<string> vll) 
+        {
+            vll = new List<string>();
+
+            if ( !this.IsReqConnected )
+                return false;
+
+            if ( !this.req.HasIn )
+                this.req.SendMoreFrame( "getm" ).SendFrame( index );
+
+            List<string> repll = new List<string>();
+
+            if ( this.req.TryReceiveMultipartStrings( TimeSpan.FromSeconds( 5 ), ref repll ) )
+            {
+                repll.RemoveAt( 0 );
+
+                var ret = repll[0].Split( '/' );
+                repll.Clear();
+
+                ret.ForEach( s => 
+                {
+                    if ( s.Equals( "ok" ) || s.Equals( "nok" ) || s.Equals( index ) )
+                        repll.Add( s );
+                } );
+
+                vll = repll;
+
+                return true;
+            }
+            else
+                return false;
+        }
+
+        #region Get Request Method
+        public eOperatationMode GetOperationMode()
+        {
+            List<string> rll;
+            this.GetRequst( "111", out rll );
+
+            if ( rll[0].Equals( "1" ) )
+                return eOperatationMode.AutoMode;
+            else
+                return eOperatationMode.ManualMode;
+        }
+
+        public int CurrentPointNo()
+        {
+            List<string> rll;
+            this.GetRequst( "111", out rll );
+
+            int ret = 0;
+            int.TryParse( rll[1], out ret );
+
+            return ret;
+        }
+        #endregion
+
+        #region Set Request Method
+
+        internal void SetObstruction( eObstacleState state )
+        {
+            string outStr = string.Empty;
+
+            switch ( state )
+            {
+                case eObstacleState.Normal:
+                    this.Request( "setm", "5001/0", out outStr );
+                    break;
+                case eObstacleState.Abnormal:
+                    this.Request( "setm", "5001/3", out outStr );
+                    break;
+                case eObstacleState.Blocked:
+                    this.Request( "setm", "5001/2", out outStr );
+                    break;
+                case eObstacleState.Decelerate:
+                    this.Request( "setm", "5001/1", out outStr );
+                    break;
+                default:
+                    break;
+            }
+        }
+
+        internal void SetOperationSate(eOperatationMode mode)
+        {
+            string outStr = string.Empty;
+
+            if ( mode == eOperatationMode.AutoMode )
+                this.Request( "set", "112/1", out outStr );
+            else if ( mode == eOperatationMode.ManualMode )
+                this.Request( "set", "112/0", out outStr );
+            else
+            { }
+        }
+
+        internal void SetMCState(bool isOn )
+        {
+            string outStr = string.Empty;
+            if ( isOn )
+                this.Request( "setm", "5000/1", out outStr );
+            else
+                this.Request( "setm", "5000/0", out outStr );
+        }
+
         internal void SetCurrentSteeringState( eSteeringState state )
         {
             string outStr = string.Empty;
@@ -234,5 +385,6 @@ namespace VehicleControlSystem.ControlLayer.MQ
                     break;
             }
         }
+        #endregion
     }
 }

Dev/OHV/VehicleControlSystem/ControlLayer/Motion/GSIDrive.cs

@@ -191,19 +191,6 @@ namespace VehicleControlSystem.ControlLayer.Drive
             this.RearDriveState = rState;
         }
 
-        void GetReqSteeringState()
-        {
-            var ret = Redis.Instance.GetSteering();
-            if ( ret == 1 )// Left
-                this.ReqSteeringState = eSteeringState.Left;
-            else if ( ret == -1 ) //Right
-                this.ReqSteeringState = eSteeringState.Right;
-            else if ( ret == 0 ) // Do Nothing
-                this.ReqSteeringState = eSteeringState.None;
-            else 
-            { }
-        }
-
         public void SetCurrentSteeringState(eSteeringState state )
         {
             this.zmq.SetCurrentSteeringState( state );
@@ -293,28 +280,7 @@ namespace VehicleControlSystem.ControlLayer.Drive
         //public void ServoOff() => OHVdriveSetServoCommand( 2 );
         //public void AmpFaultReset() => OHVdriveSetServoCommand( -1 );
 
-        public void SetObstacleState( eObstacleState state )
-        {
-            int field = 0;
-
-            switch ( state )
-            {
-                case eObstacleState.Normal:
-                    field = 0;
-                    break;
-                case eObstacleState.Abnormal:
-                    field = 1;
-                    break;
-                case eObstacleState.Blocked:
-                    field = 2;
-                    break;
-                case eObstacleState.Decelerate:
-                    field = 3;
-                    break;
-                default:
-                    break;
-            }
-        }
+        public void SetObstacleState( eObstacleState state ) => zmq.SetObstruction( state );
 
         public bool VelocityChainge( double velocity )
         {

Dev/OHV/VehicleControlSystem/ControlLayer/Vehicle.cs

@@ -711,7 +711,7 @@ namespace VehicleControlSystem.ControlLayer
                     if ( this.autoManager.OperationModeProperty == eOperatationMode.AutoMode )
                         this.CheckObstacle();
 
-                    this.CheckIOState();
+                    //this.CheckIOState();
                 }
                 catch ( ThreadInterruptedException threadInterruptedException )
                 {
@@ -1968,6 +1968,8 @@ namespace VehicleControlSystem.ControlLayer
                     }
                     else
                         this.OccurVehicleAlarm( 29 );
+
+
                     break;
 
                 case "IN_EMS_SW":
@@ -1985,6 +1987,13 @@ namespace VehicleControlSystem.ControlLayer
                         this.OccurVehicleAlarm( 30 );
                     break;
 
+                case "IN_OBSTRUCTION_DETECT_ERROR":
+                case "IN_OBSTRUCTION_DETECT_SAFETY":
+                case "IN_OBSTRUCTION_DETECT_STOP":
+                case "IN_OBSTRUCTION_DETECT_SLOW":
+                    CheckObstacle();
+                    break;
+
                 default:
                     break;
             }