Merge pull request #21 from grf14003/dev

Version 1.1

bfs/implementations/breadcrumbs/CMakeLists.txt

@@ -2,3 +2,7 @@
 set (VirtualOutputIOProcessor_IMPL_LIBS
 	"ea_tdk"
 	PARENT_SCOPE)
+
+set (TactorOutputIOProcessor_IMPL_LIBS
+   "ea_tdk"
+   PARENT_SCOPE)

bfs/implementations/breadcrumbs/algos/AlgoBreadcrumbs.cpp

@@ -1,5 +1,6 @@
 
 #include "AlgoBreadcrumbs.hpp"
+#include "BreadcrumbsConstants.hpp"
 
 
 void AlgoBreadcrumbs::loop()
@@ -19,6 +20,9 @@ void AlgoBreadcrumbs::loop()
 
 		sendAttribute(msgCountAttrib);
 	}
+	int degree = 0;
+	Attribute degreeAttrib = { ATTRKEY_EATAC_DEGREE, sizeof(double), &degree };
+	sendAttribute(degreeAttrib);
 }
 
 bool AlgoBreadcrumbs::loopCondition()

bfs/implementations/breadcrumbs/ea_tactor/ResponseHelper.cpp

@@ -0,0 +1,138 @@
+#include "ResponseHelper.h"
+
+#include <cstring>
+#include <cstdio>
+
+void PrintPacketMsg(string msg, float timer, unsigned char*buffer, int size) {
+
+	printf("[TDK] %i,%f,%s,", 0, timer, msg.c_str());
+
+	for (int i = 0; i < size; ++i)
+		printf("%X:", buffer[i]);
+	printf("\n");
+}
+unsigned char ComputeCheckSum(unsigned char* buf, int size) {
+	unsigned char checksum = 0x00;
+	for (int i = 0; i < size; ++i) {
+		checksum = checksum ^ buf[i];
+	}
+	checksum ^= 0xEA;
+
+	return checksum;
+}
+ int __stdcall ParsePacket(int deviceID, unsigned char packet[READPACKETLIMIT], int size)
+ {
+	//[1] of the packet data is the type.
+	unsigned char type = packet[1];
+	//[2] if the packet data is the data len
+	unsigned char len = packet[2];
+
+	unsigned char * data = new unsigned char[len];
+	memset(data, '\0', len);
+	memcpy(data, &packet[3], len);
+	unsigned char chksum = packet[len + 3];
+	//comupting the checksum of the packet to ensure we got the entire packet correctly
+	unsigned char computed_chksum = ComputeCheckSum(packet, len + 3);
+
+	if (computed_chksum == chksum) 
+	{
+		switch (type) 
+		{
+			//ack packet
+		case 0xc8:
+			PrintPacketMsg("ACK WITH DATA", 0, packet, size);
+			break;
+			//ack packet
+		case 0xc9:
+			PrintPacketMsg("ACK", 0, packet, size);
+			break;
+			//nak packet
+		case 0xc4: {
+			PrintPacketMsg("NAK", 0, packet, size);
+			//[0] response to wich command
+			unsigned char responseTo = data[0];
+			//[1] reason code for nak
+			unsigned  char reasonCode = data[1];
+			switch (reasonCode) {
+			case 0x01: //The ETX value was not found in the expected place
+				PrintPacketMsg("ETX value was not found ERROR", 0, packet, size);
+				break;
+			case 0x02: //The Checksum value was invalid
+				PrintPacketMsg(" Checksum value was invalid ERROR", 0, packet, size);
+				break;
+			case 0x03: //Insufficient Data in packet
+				PrintPacketMsg("Insufficient Data ERROR", 0, packet, size);
+				break;
+			case 0x04: //An invalid command was used
+				PrintPacketMsg("invalid command  ERROR", 0, packet, size);
+				break;
+			case 0x05: //Invaid Data was given with a valid command
+				PrintPacketMsg("Invalid Data ERROR", 0, packet, size);
+				break;
+			case 0x06: //Data length exceeds max payload
+				PrintPacketMsg("Data length exceeds ERROR", 0, packet, size);
+				break;
+			case 0x07: //Invalid Sequence Data
+				PrintPacketMsg(" Invalid Sequence Data ERROR", 0, packet, size);
+				break;
+			case 0x08: //Bus Error Sending to Node failed after three attempts
+				PrintPacketMsg(" Bus Error Sending ERROR", 0, packet, size);
+				break;
+			case 0x09: //Bus Master Times out while waiting on all bytes of incoming command to finish
+				PrintPacketMsg("Bus Master Times out ERROR", 0, packet, size);
+				break;
+			case 0x0A: //Node has a Fault Cannot Complete Request
+				PrintPacketMsg("Fault ERROR", 0, packet, size);
+				break;
+			case 0x0B: // Sequence is busy � you tried to perform a new sequence operation while the previous one is still in progress � sequences not implemented on TDK boards
+				PrintPacketMsg("NAK_SeqBusy ERROR", 0, packet, size);
+				break;
+			case 0x0C:  // not used anymore
+				PrintPacketMsg("NAK_TimeOut ERROR", 0, packet, size);
+				break;
+			case 0x0D:   // Ramp List is Full
+				PrintPacketMsg("NAK_RampListFull ERROR", 0, packet, size);
+				break;
+			case 0x0E:   // Action List is Full
+				PrintPacketMsg("NAK_ActionListFull ERROR", 0, packet, size);
+				break;
+			case 0x0F:// The command you sent is probably valid, but it has not been implemented on this board � you shouldn�t see this outside of early beta firmware
+				PrintPacketMsg("NAK_NotImplemented ERROR", 0, packet, size);
+				break;
+			case 0x10:// Master Packet is Full - you shouldn�t see this one � it means the Bus on eTSAS or other new distrib system is very busy with action-lists & ramps and can�t fit a new command at the instant you sent it.
+				PrintPacketMsg("NAK_MasterPacketFullOrBusy ERROR", 0, packet, size);
+				break;
+			case 0x11: // error while processing a command which is already in the action list
+				PrintPacketMsg("NAK_ActionListTooManyBytes ERROR", 0, packet, size);
+				break;
+			case 0x12:  // another error while processing a command which is already in the action list
+				PrintPacketMsg("NAK_ActionListTooFewBytes", 0, packet, size);
+				break;
+			case 0x13:  //your packet is too big to fit in the TDK receive buffer
+				PrintPacketMsg("NAK_TooMuch", 0, packet, size);
+				break;
+			case 0x14: //Received a command while Self Test is running
+				PrintPacketMsg("NAK_Received a command while Self Test is running ERROR", 0, packet, size);
+				break;
+			default: //did not find the error within the lookup table. 
+				PrintPacketMsg("UNKNOWN ERROR", 0, packet, size);
+				break;
+
+			}
+			break;
+				   }
+		case 0xEE:
+			PrintPacketMsg("0xEE", 0, packet, size);
+			return -1;
+			break;
+		}
+		delete[] data;
+	} else {
+		PrintPacketMsg("checksum was bad", 0, packet,
+			size);
+		delete[] data;
+		return -1;
+	}
+
+	return 0;
+}

bfs/implementations/breadcrumbs/gen/AHRSInputIOProcessor.cpp

@@ -0,0 +1,23 @@
+
+#include <iostream>
+
+#include "DataSyncThread.hpp"
+#include "AHRSInputIOProcessor.hpp"
+
+
+int main()
+{
+	IOProcessor* client = new AHRSInputIOProcessor;
+
+	if (!client->init())
+	{
+		while (client->loopCondition())
+			client->loop();
+
+		int result = client->close();
+		delete client;
+		return result;
+	}
+
+	return 0;
+}

bfs/implementations/breadcrumbs/gen/TactorOutputIOProcessor.cpp

@@ -0,0 +1,23 @@
+
+#include <iostream>
+
+#include "DataSyncThread.hpp"
+#include "TactorOutputIOProcessor.hpp"
+
+
+int main()
+{
+	IOProcessor* client = new TactorOutputIOProcessor;
+
+	if (!client->init())
+	{
+		while (client->loopCondition())
+			client->loop();
+
+		int result = client->close();
+		delete client;
+		return result;
+	}
+
+	return 0;
+}

bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp

@@ -0,0 +1,38 @@
+/*
+*
+* This is the IO Processor for the Xsens MTi-630 AHRS:
+* https://www.xsens.com/products/mti-600-series
+* The AHRS is able to calculate a lot of different orientational data,
+* we are specfically using it to find the Euler Angles.
+* Currently you must manually specify which COM port the device is connected
+* to, this is right at the begining of the CreateFile() method call.
+* By defualt it is set to COM8, you may have to change it to the correct port
+* on your system. You can find the corrct port manually by opening Device Manager,
+* under "Ports(COM & LPT)" you can see the ports detected by your system.
+*/
+
+#ifndef AHRS_INPUT_IO_PROCESSOR_HPP
+#define AHRS_INPUT_IO_PROCESSOR_HPP
+
+#include "IOProcessor.hpp"
+#include "DataSyncThread.hpp"
+#include "Attribute.hpp"
+
+#include <windows.h>
+#include <iostream>
+
+class AHRSInputIOProcessor : public IOProcessor
+{
+public:
+    using IOProcessor::IOProcessor;
+
+    void loop();
+    bool loopCondition();
+    int configAHRS();   // Initialize the AHRS for the first time
+private:
+    int iterations = 10;
+    bool configured = false;
+    HANDLE hSerial;
+};
+
+#endif

bfs/implementations/breadcrumbs/include/BreadcrumbsConstants.hpp

@@ -0,0 +1,8 @@
+#ifndef BREADCRUMBS_CONSTANTS_HPP
+#define BREADCRUMBS_CONSTANTS_HPP
+
+
+#define ATTRKEY_EATAC_DEGREE "EATACDEG"
+
+
+#endif

bfs/implementations/breadcrumbs/include/ResponseHelper.h

@@ -0,0 +1,23 @@
+#ifndef _EAIRESPONSEH_
+#define _EAIRESPONSEH_
+
+// This file was copied from the "TDK" folder in the examples directory!
+
+#include <iostream>
+#include <string>
+using namespace std;
+
+#define READPACKETLIMIT 512 // maximum bytes of an entire read packet
+
+// The calling convention on windows is stdcall,
+// on Linux, this identifier doesn't exist.
+#ifndef _WIN32
+#ifndef __stdcall
+#define __stdcall
+#endif // __stdcall
+#endif // !_WIN32
+
+//Parse Packet is the callback used for the device packet data.
+int __stdcall ParsePacket(int deviceID, unsigned char packet[READPACKETLIMIT], int size);
+
+#endif

bfs/implementations/breadcrumbs/include/TactorOutputIOProcessor.hpp

@@ -0,0 +1,31 @@
+#ifndef TACTOR_OUTPUT_IO_PROCESSOR_HPP
+#define TACTOR_OUTPUT_IO_PROCESSOR_HPP
+
+#include <map>
+
+#include "IOProcessor.hpp"
+#include "DataSyncThread.hpp"
+#include "Attribute.hpp"
+#include "BreadcrumbsConstants.hpp"
+#include "TactorInterface.h"
+#include "ResponseHelper.h"
+
+
+class TactorOutputIOProcessor : public IOProcessor
+{
+public:
+	using IOProcessor::IOProcessor;
+
+	void loop();
+	bool loopCondition();
+	double getTactorDistance(double tactorA, double tactorB, int tactorNumber);
+	void pulseTactors(double degree, int tactorNumber, int pulseRadius);
+	void checkForError(int errorCode);
+	int initializeController();
+
+private:
+	bool isControllerInit = false;
+	double tactorDegree = 0;
+};
+
+#endif