mil1553_cond_branch_1.cs

/*****************************************************************************
//
##    File:
##       mil1553_cond_branch_1.cs
//
//       Copyright (c) MAX Technologies Inc. 1988-2019, All Rights Reserved.
//             CONFIDENTIAL AND PROPRIETARY INFORMATION WHICH IS THE 
//                         PROPERTY OF MAX TECHNOLOGIES INC. 
//
//      This example demonstrates the usage of message conditional branch option
//       with MIL-STD-1553 BC channel.
//
//       Hardware requirements:
//          - MAXT Flex1553-PCIe or FlexMulti.
//
*****************************************************************************/

//#define LOCAL
#define LOOPBACK

using System;
using System.Text;
using System.Runtime.InteropServices;

using static MAXT.MXFoundation.mxf;



namespace mil1553_example
{
   public class mil1553_cond_branch_1
   {
      const UInt64 BUF_SA3_RX = 0;
      const UInt64 BUF_SA3_TX = 1;
      const UInt64 BUF_SA5_RX = 2;
      const UInt64 BUF_SA5_TX = 3;

      const UInt16 RT_ADRS = 5;

      static void Main(string[] args)
      {
         UInt32 rc;
         UInt64 deviceCount = 0;
         UInt64 moduleCount = 0;
         UInt64 channelCount = 0;
         UInt64 server = 0;
         var device = new UInt64[1];
         var module = new UInt64[1];
         var bc = new UInt64[1];
         var rt = new UInt64[1];
         var bcBufferTx = new UInt64[4];
         var bcBufferRx = new UInt64();
         var rtBuffer = new UInt64[4];

         var bcMsgID = new MXF_MSGID_MIL1553[]
        {
                new MXF_MSGID_MIL1553{ type = MXF_MIL1553_MSGTYPE_RX, address = RT_ADRS, subAddress = 3, reserved = 0 },
                new MXF_MSGID_MIL1553{ type = MXF_MIL1553_MSGTYPE_TX, address = RT_ADRS, subAddress = 3, reserved = 0 },
                new MXF_MSGID_MIL1553{ type = MXF_MIL1553_MSGTYPE_RX, address = RT_ADRS, subAddress = 5, reserved = 0 },
                new MXF_MSGID_MIL1553{ type = MXF_MIL1553_MSGTYPE_TX, address = RT_ADRS, subAddress = 5, reserved = 0 }
        };

         var rtMsgID = new MXF_MSGID_MIL1553[]
        {
                new MXF_MSGID_MIL1553{ type = MXF_MIL1553_MSGTYPE_RX, address = 0, subAddress = 3, reserved = 0 },
                new MXF_MSGID_MIL1553{ type = MXF_MIL1553_MSGTYPE_TX, address = 0, subAddress = 3, reserved = 0 },
                new MXF_MSGID_MIL1553{ type = MXF_MIL1553_MSGTYPE_RX, address = 0, subAddress = 5, reserved = 0 },
                new MXF_MSGID_MIL1553{ type = MXF_MIL1553_MSGTYPE_TX, address = 0, subAddress = 5, reserved = 0 }
        };

         IntPtr recPtr = IntPtr.Zero;

         UInt32 txBufferSize = 1024;
         IntPtr txBuffer = IntPtr.Zero;
         var txRec1553 = new MXF_MIL1553_DATAREC[3];
         for (int i = 0; i < 3; i++)
            txRec1553[i].data = new UInt16[36];

         UInt32 rxBufferSize = 10 * 1024;
         IntPtr rxBuffer = IntPtr.Zero;
         var rxRec1553 = new MXF_MIL1553_DATAREC[1];

         var minorFrame = new MXF_MIL1553_TXPERIODIC_MJRFRAME_MSG[2];
         var mfProp = new MXF_MIL1553_TXPERIODIC_MJRFRAME_MINOR_PROPERTIES();

         UInt64 bus = MXF_MIL1553_BUS_A | MXF_MIL1553_BUS_B;
         UInt64 rxAcqStatus = 0;
         UInt64 msgCount = 0;
         UInt64 byteCount = 0;
         UInt64 rxRec;
         UInt32 loop = 0;
         UInt64 address, subAddress, dir, wordCount, indexBuffer;
         UInt32 data;
         UInt32 msg;
         var msgInfo = new MXF_MIL1553_MSGINFO[1];
         var err = new MXF_MIL1553_ERROR_INJ();

         var errorString = new StringBuilder(200);

#if (LOCAL)
            rc = mxfServerConnect("0.0.0.0", "", "", Convert.ToUInt64(false), out server);
#else
         rc = mxfServerConnect("192.168.0.1", "admin", "admin", Convert.ToUInt64(false), out server);
#endif
         // Initialize MX Foundation library
         if (rc == MAXT_SUCCESS)
         {
            Console.Write("Starting ...\n\r");
            rc = mxfSystemInit(server);
         }

         // Get handle of first Flex MIL-STD-1553 Bus controller channel
         if (rc == MAXT_SUCCESS)
            rc = mxfSystemDeviceAllGet(server, MXF_DEVICE_ALL, 1, out deviceCount, device);

         if (rc == MAXT_SUCCESS && deviceCount > 0)
            rc = mxfDeviceModuleAllGet(device[0], MXF_MODULE_MIL1553MRT_EH, 1, out moduleCount, module);

         if (rc == MAXT_SUCCESS && moduleCount > 0)
            rc = mxfModuleChannelAllGet(module[0], MXF_CLASS_MIL1553, MXF_SCLASS_BC_CHANNEL, 1, out channelCount, bc);


         // If not found, return an error
         if (rc == MAXT_SUCCESS && channelCount == 0)
            rc = MAXT_ERROR_NOT_FOUND;

         // Get handle of the 6th Flex MIL-STD-1553 remote terminal channel
         if (rc == MAXT_SUCCESS)
            rc = mxfModuleChannelGet(module[0], 2 + RT_ADRS, out rt[0]);

#if (LOOPBACK)
         // Enable internal loopback
         if (rc == MAXT_SUCCESS)
            rc = mxfAttributeUint64Set(bc[0], KMXF_MIL1553_TX_RX_TEST_LB, VMXF_ENABLE);
#endif

         // Allocate 1KB buffer for tx data
         if (rc == MAXT_SUCCESS)
         {
            txBufferSize = 1024;

            // Flex allocation
            for (msg = 0; msg < 4 && rc == MAXT_SUCCESS; msg++)
            {
               rc = mxfTxPeriodicUpdateMsgBufferAlloc(bc[0], msg, txBufferSize, out bcBufferTx[msg], IntPtr.Zero);
               if (rc == MAXT_SUCCESS)
                  rc = mxfTxPeriodicUpdateMsgBufferAlloc(rt[0], msg, txBufferSize, out rtBuffer[msg], IntPtr.Zero);
               if (rc == MAXT_SUCCESS)
                  rc = mxfMIL1553RtSubsystemEnableSet(rt[0], rtMsgID[msg].type, rtMsgID[msg].subAddress, bus, rtBuffer[msg]);
            }

            // Host allocation
            if (rc == MAXT_SUCCESS)
            {
               try
               {
                  txBuffer = Marshal.AllocHGlobal((int)txBufferSize);
               }
               catch (OutOfMemoryException)
               {
                  rc = MAXT_ERROR_MEM;
               }
            }
         }

         // Allocate 10KB buffer for rx data
         if (rc == MAXT_SUCCESS)
         {
            rxBufferSize = 10 * 1024;

            // Flex allocation
            rc = mxfRxAcqBufferAlloc(bc[0], rxBufferSize, out bcBufferRx, IntPtr.Zero);

            // Host allocation
            if (rc == MAXT_SUCCESS)
            {
               try
               {
                  rxBuffer = Marshal.AllocHGlobal((int)rxBufferSize);
               }
               catch (OutOfMemoryException)
               {
                  rc = MAXT_ERROR_MEM;
               }
            }
         }

         // Set timebase to RTC nsec
         if (rc == MAXT_SUCCESS)
            rc = mxfSystemTimeBaseSet(server, MXF_TIMEBASE_DEVICE_NSEC);

         // Configure injection #0, override status
         if (rc == MAXT_SUCCESS)
         {
            err.errors = new UInt16[34];

            err.errors[33] = (RT_ADRS << 11) + 1;
            rc = mxfMIL1553ErrorInjectionCompose(MXF_MIL1553_ERRORID_STATUS_OV, 0, out err.errors[0]);
            if (rc == MAXT_SUCCESS)
               rc = mxfMIL1553ErrorInjectionSet(bc[0], 0, ref err);
         }

         // Set the minor frame #0 using 2 Commands
         if (rc == MAXT_SUCCESS)
         {
            mfProp.modulo = 1;
            mfProp.repeatCount = 1;
            mfProp.options = MXF_MIL1553_TXPERIODIC_MJRFRAME_MINOR_PROPERTIES_OPT_BRANCH_GOTO;
            mfProp.branchIndex = 0;

            // Command #0 : Address 5, Subaddress 3, RX, 2 words, Modulo 2
            rc = mxfMIL1553CommandCompose(bcMsgID[BUF_SA3_RX].address, bcMsgID[BUF_SA3_RX].subAddress, bcMsgID[BUF_SA3_RX].type, 2, out minorFrame[0].command);
            minorFrame[0].modulo = 2;
            minorFrame[0].buffer = bcBufferTx[BUF_SA3_RX];

            // Command #1 : Address 5, Subaddress 3, TX, 2 words, delay of 1 msec from preceding command or start of minor frame, conditional branch to minor frame 1 if status is not equal to 0x2800
            if (rc == MAXT_SUCCESS)
            {
               rc = mxfMIL1553CommandCompose(bcMsgID[BUF_SA3_TX].address, bcMsgID[BUF_SA3_TX].subAddress, bcMsgID[BUF_SA3_TX].type, 2, out minorFrame[1].command);
               if (rc == MAXT_SUCCESS)
               {
                  minorFrame[1].options = MXF_MIL1553_TXPERIODIC_MJRFRAME_MSG_OPT_DELAY;
                  minorFrame[1].modulo = 1;
                  minorFrame[1].buffer = bcBufferTx[BUF_SA3_TX];
                  minorFrame[1].delay = 8000; // 1 msec in 1/8 of bit time
                  minorFrame[1].branchMinorIndex = 1;
                  minorFrame[1].branchMsgIndex = 0xFFFF;
                  minorFrame[1].condMask = 0xFFFF;
                  minorFrame[1].condData = (UInt16)(bcMsgID[BUF_SA3_TX].address << 11);
                  rc = mxfMIL1553ConditionalBranchCompose(MXF_MIL1553_TXPERIODIC_MJRFRAME_MSG_CONDBRANCH_OPT_NOTEQUAL, 0, out minorFrame[1].condBranch);
               }
               if (rc == MAXT_SUCCESS)
                  rc = mxfMIL1553TxPeriodicMajorFrameSet(bc[0], 0, 0, 2, minorFrame, ref mfProp);
            }
         }

         // Set the minor frame #1 using 2 Commands
         if (rc == MAXT_SUCCESS)
         {
            // Command #0 : Address 5, Subaddress 5, RX, 3 words
            rc = mxfMIL1553CommandCompose(bcMsgID[BUF_SA5_RX].address, bcMsgID[BUF_SA5_RX].subAddress, bcMsgID[BUF_SA5_RX].type, 3, out minorFrame[0].command);
            minorFrame[0].modulo = 1;
            minorFrame[0].buffer = bcBufferTx[BUF_SA5_RX];

            // Command #1 : Address 5, Subaddress 5, TX, 3 words, delay of 1 msec after end of preceding command, conditional branch to message 1 of minor frame 0 if status is equal to 0x2800
            if (rc == MAXT_SUCCESS)
            {
               rc = mxfMIL1553CommandCompose(bcMsgID[BUF_SA5_TX].address, bcMsgID[BUF_SA5_TX].subAddress, bcMsgID[BUF_SA5_TX].type, 3, out minorFrame[1].command);
               if (rc == MAXT_SUCCESS)
               {
                  minorFrame[1].options = MXF_MIL1553_TXPERIODIC_MJRFRAME_MSG_OPT_GAP;
                  minorFrame[1].modulo = 1;
                  minorFrame[1].buffer = bcBufferTx[BUF_SA5_TX];
                  minorFrame[1].delay = 8000; // 1 msec in 1/8 of bit time
                  minorFrame[1].branchMinorIndex = 0;
                  minorFrame[1].branchMsgIndex = 1;
                  minorFrame[1].condMask = 0xFFFF;
                  minorFrame[1].condData = (UInt16)(bcMsgID[BUF_SA3_TX].address << 11);
                  rc = mxfMIL1553ConditionalBranchCompose(MXF_MIL1553_TXPERIODIC_MJRFRAME_MSG_CONDBRANCH_OPT_EQUAL, 0, out minorFrame[1].condBranch);
               }
            }

            if (rc == MAXT_SUCCESS)
               rc = mxfMIL1553TxPeriodicMajorFrameSet(bc[0], 0, 1, 2, minorFrame, IntPtr.Zero);
         }

         // Set BC data for address 5, subaddress 3, RX
         if (rc == MAXT_SUCCESS)
         {
            recPtr = txBuffer;
            txRec1553[0].data = new UInt16[36];

            txRec1553[0].repeatCount = 2; // repeat count of 2
            txRec1553[0].dataSize = 6; //6 bytes (command + 2 words)
            txRec1553[0].data[0] = 0x0000; //Not used
            txRec1553[0].data[1] = 0x0000;
            txRec1553[0].data[2] = 0x1111;

            Marshal.StructureToPtr(txRec1553[0], txBuffer, true);

            rc = mxfMIL1553TxPeriodicUpdateMsgWrite(bcBufferTx[BUF_SA3_RX], 1, txBuffer);
         }

         txRec1553[0].data = new UInt16[36];
         // Set BC data for address 5, subaddress 5, RX
         if (rc == MAXT_SUCCESS)
         {
            recPtr = txBuffer;

            txRec1553[0].repeatCount = 1;
            txRec1553[0].dataSize = 8; //8 bytes (command + 3 words)
            txRec1553[0].data[0] = 0x0000; //Not used
            txRec1553[0].data[1] = 0xFFFF;
            txRec1553[0].data[2] = 0xEEEE;
            txRec1553[0].data[3] = 0xDDDD;

            Marshal.StructureToPtr(txRec1553[0], recPtr, true);

            rc = mxfMIL1553TxPeriodicUpdateMsgWrite(bcBufferTx[BUF_SA5_RX], 1, txBuffer);
         }
         // Set RT data for address 5, subaddress 3, TX, Send 5 times first one, then 5 times second one that has an override status error
         // then send last one with no error
         if (rc == MAXT_SUCCESS)
         {
            recPtr = txBuffer;

            txRec1553[0].repeatCount = 5;
            txRec1553[0].dataSize = 6; //6 bytes (command + 2 words)
            txRec1553[0].data[0] = 0x0000; //Not used
            txRec1553[0].data[1] = 0x1234;
            txRec1553[0].data[2] = 0x5678;

            Marshal.StructureToPtr(txRec1553[0], recPtr, true);

            rc = mxfMIL1553NextDataRecordPtrGet(recPtr, out recPtr);

            if (rc == MAXT_SUCCESS)
            {
               txRec1553[1].dataSize = 6; //6 bytes (command + 2 words)
               txRec1553[1].repeatCount = 5;
               txRec1553[1].control = MXF_MIL1553_TXPERIODIC_REC_CTRL_ERROR_INJ;
               txRec1553[1].service.txPeriodicUpdateMsg.errorIndex = 0; // override status
               txRec1553[1].data[0] = 0x0000; //Not used
               txRec1553[1].data[1] = 0x9ABC;
               txRec1553[1].data[2] = 0xDEF0;
            }
            Marshal.StructureToPtr(txRec1553[1], recPtr, true);

            if (rc == MAXT_SUCCESS)
               rc = mxfMIL1553NextDataRecordPtrGet(recPtr, out recPtr);

            if (rc == MAXT_SUCCESS)
            {
               txRec1553[2].dataSize = 6; //6 bytes (command + 2 words)
               txRec1553[2].repeatCount = 1;
               txRec1553[2].control = 0;
               txRec1553[2].data[0] = 0x0000; //Not used
               txRec1553[2].data[1] = 0x1234;
               txRec1553[2].data[2] = 0x5678;
            }
            Marshal.StructureToPtr(txRec1553[2], recPtr, true);

            if (rc == MAXT_SUCCESS)
               rc = mxfMIL1553TxPeriodicUpdateMsgWrite(rtBuffer[BUF_SA3_TX], 3, txBuffer);
         }


         // Set RT data for address 5, subaddress 5, TX
         if (rc == MAXT_SUCCESS)
         {
            recPtr = txBuffer;
            txRec1553[0].data = new UInt16[36];

            txRec1553[0].repeatCount = 1;
            txRec1553[0].dataSize = 8; //8 bytes (command + 3 words)
            txRec1553[0].data[0] = 0x0000; //Not used
            txRec1553[0].data[1] = 0x4444;
            txRec1553[0].data[2] = 0x5555;
            txRec1553[0].data[3] = 0x6666;

            Marshal.StructureToPtr(txRec1553[0], recPtr, true);

            rc = mxfMIL1553TxPeriodicUpdateMsgWrite(rtBuffer[BUF_SA5_TX], 1, txBuffer);
         }

         // Start messages queues
         if (rc == MAXT_SUCCESS)
         {
            Console.Write("Starting BC and RT\n\r");
            rc = mxfMIL1553RtEnableSet(rt[0], VMXF_ENABLE);
         }

         // Start BC acquisition
         if (rc == MAXT_SUCCESS)
            rc = mxfRxAcqStart(bcBufferRx, MXF_RXACQ_FLAG_DEFAULT, 0, 0);

         // Start the major frame with 250 msec rate
         if (rc == MAXT_SUCCESS)
            rc = mxfMIL1553TxPeriodicMajorFrameStart(bc[0], 0, 250000000, IntPtr.Zero);


         // Read and display received messages
         if (rc == MAXT_SUCCESS)
         {
            do
            {
               rc = mxfMIL1553RxAcqRead(bcBufferRx, 0, rxBufferSize, out rxAcqStatus, out msgCount, out byteCount, rxBuffer);

               recPtr = rxBuffer;
               for (rxRec = 0; rxRec < msgCount && rc == MAXT_SUCCESS; rxRec++)
               {
                  rxRec1553[0] = (MXF_MIL1553_DATAREC)Marshal.PtrToStructure(recPtr, typeof(MXF_MIL1553_DATAREC));

                  rc = mxfMIL1553DataRecordDecompose(bc[0], 1, rxRec1553, msgInfo);
                  if (rc == MAXT_SUCCESS)
                  {
                     rc = mxfMIL1553CommandDecompose(rxRec1553[0].data[0], out address, out subAddress, out dir, out wordCount);
                     if (rc == MAXT_SUCCESS)
                     {
                        Console.Write("\n\r{0}:\t", rxRec1553[0].timeTag);
                        switch (msgInfo[0].msgType)
                        {
                           case MXF_MIL1553_MSGINFO_TYPE_BCRT:
                              Console.Write("BC to RT{0} SA{1} WC{2} (0x{3:x4})\n\r", address, subAddress, wordCount, rxRec1553[0].data[0]);
                              Console.Write("\t\tRT status: 0x{0:x4}\n\r", rxRec1553[0].data[msgInfo[0].statusIndex[0]]);
                              break;

                           case MXF_MIL1553_MSGINFO_TYPE_RTBC:
                              Console.Write("RT{0} SA{1} WC{2} to BC (0x{3:x4})\n\r", address, subAddress, wordCount, rxRec1553[0].data[0]);
                              Console.Write("\t\tRT status: 0x{0:x4}\n\r", rxRec1553[0].data[msgInfo[0].statusIndex[0]]);
                              Console.Write("\t\tRT data:");
                              for (data = 0; data < msgInfo[0].dataWordCount; data++)
                              {
                                 if (data > 0 && (data % 4) == 0)
                                    Console.Write("\n\r\t\t\t");

                                 Console.Write(" 0x{0:x4}", rxRec1553[0].data[msgInfo[0].dataIndex + data]);
                              }
                              Console.Write("\n\r");
                              break;

                           case MXF_MIL1553_MSGINFO_TYPE_MODECODE_TXDATA:
                              Console.Write("BC Mode Command {0} to RT{1} SA{2} (0x{3:x4})\n\r", wordCount, address, subAddress, rxRec1553[0].data[0]);
                              if (msgInfo[0].statusIndex[0] != 0xffff)
                              {
                                 Console.Write("\t\tRT status: 0x{0:x4}\n\r", rxRec1553[0].data[msgInfo[0].statusIndex[0]]);
                                 Console.Write("\t\tRT data: 0x{0:x4}\n\r", rxRec1553[0].data[msgInfo[0].dataIndex]);
                              }
                              break;
                        }
                     }
                  }
                  // Get next msg
                  rc = mxfMIL1553NextDataRecordPtrGet(recPtr, out recPtr);
               }
               mxfSleep(500);
               loop++;
            } while (loop < 10);
         }

         // Stop acquisition
         if (rc == MAXT_SUCCESS)
         {
            rc = mxfRxAcqStop(bcBufferRx);
            if (rc == MAXT_SUCCESS)
               rc = mxfRxAcqClear(bcBufferRx);
         }

         // Stop the major frame
         if (rc == MAXT_SUCCESS)
            rc = mxfTxPeriodicMajorFrameStop(bc[0], 0, 0);

         // Stop and Clear message queues
         if (rc == MAXT_SUCCESS)
         {
            Console.Write("Stopping BC and RT\n\r");
            rc = mxfMIL1553RtEnableSet(rt[0], VMXF_DISABLE);

            for (msg = 0; msg < 4 && rc == MAXT_SUCCESS; msg++)
            {
               rc = mxfTxPeriodicUpdateMsgClear(bcBufferTx[msg], MXF_TXPERIODIC_UPDATEMSG_CLEAR_OPT_DEFAULT);
               if (rc == MAXT_SUCCESS)
                  rc = mxfTxPeriodicUpdateMsgClear(rtBuffer[msg], MXF_TXPERIODIC_UPDATEMSG_CLEAR_OPT_DEFAULT);
            }
         }

         Console.Write("Terminating\n");

         // free buffers
         if (txBuffer != IntPtr.Zero)
            Marshal.FreeHGlobal(txBuffer);

         if (rxBuffer != IntPtr.Zero)
            Marshal.FreeHGlobal(rxBuffer);

         if (rc != MAXT_SUCCESS)
         {
            if (mxfSystemErrorStringGet(server, rc, (UInt32)errorString.Capacity, errorString) != MAXT_SUCCESS)
            {
               errorString.Clear();
               errorString.Append(string.Format("ERROR # 0x{0:x8}", rc));
            }
            Console.Write(errorString + "\n\r");
         }

         // When all services are terminated, there is no pending Tx/Rx data, free.

         for (indexBuffer = 0; indexBuffer < 4 && bcBufferTx[indexBuffer] > 0; indexBuffer++)
         {
            if (bcBufferTx[indexBuffer] != 0)
            {
                rc = mxfTxPeriodicUpdateMsgBufferFree(bcBufferTx[indexBuffer]);
                if (rc != MAXT_SUCCESS)
                    Console.Write("Free buffer failed !\n\r");
            }
         }


         for (indexBuffer = 0; indexBuffer < 4 && rtBuffer[indexBuffer] > 0; indexBuffer++)
         {
            if (rtBuffer[indexBuffer] != 0)
            {
                rc = mxfTxPeriodicUpdateMsgBufferFree(rtBuffer[indexBuffer]);
                if (rc != MAXT_SUCCESS)
                    Console.Write("Free buffer failed !\n\r");
            }
         }
         if (bcBufferRx > 0)
         {
            rc = mxfRxAcqBufferFree(bcBufferRx);
            if (rc != MAXT_SUCCESS)
               Console.Write("Free buffer failed !\n\r");
         }

         // Unload MX Foundation library
         mxfSystemTerminate(server);
         mxfServerDisconnect(server);

         Console.Write("\n\rPress enter to terminate\n\r");
         Console.ReadKey();

         return;
      }
   }
}