kopia lustrzana https://github.com/ozarchie/EQMOD-ETX
Initial commit
rodzic
12deac0889
commit
9fe6dc105c
|
@ -0,0 +1,344 @@
|
|||
// Pin definitions for HBX interface
|
||||
// =================================
|
||||
#define HDA1 8 // Pin2, 4, 6 on HBX interface
|
||||
#define HDA2 10 // Not used
|
||||
#define HCL1 2 // Pin3 on HBX interface
|
||||
#define HCL2 3 // Pin5 on HBX interface
|
||||
|
||||
#define H2X_INPUTPU INPUT_PULLUP // Set pin data input mode
|
||||
#define H2X_INPUT INPUT // Set pin data input mode
|
||||
#define H2X_OUTPUT OUTPUT // Set pin data output
|
||||
|
||||
// Jumpers to run monitor or test
|
||||
// ==============================
|
||||
#define TESTHBX 9 // Mega2560 D2
|
||||
#define MONITORHBX 11 // Mega2560 D3
|
||||
|
||||
|
||||
#define CR 0x0d
|
||||
#define LF 0x0a
|
||||
#define H2XLEN 256
|
||||
#define H2XMASK H2XLEN - 1
|
||||
|
||||
// ETX ISR States
|
||||
#define START 0x01
|
||||
#define ACK 0x02
|
||||
#define COMMAND 0x03
|
||||
#define DATA 0x04
|
||||
|
||||
// ETX Known Commands
|
||||
#define RotateSlow 0x00 // Output "8.16" speed
|
||||
#define RotateFast 0x01 // Output "16.8" ? speed
|
||||
#define SetOffset 0x02 // Output "16" correction offset
|
||||
#define SetLEDI 0x03 // Output "8" LED current
|
||||
#define CalibrateLED 0x04 // None
|
||||
#define Stop 0x05 // None
|
||||
#define SlewReverse 0x06 // None
|
||||
#define SlewForward 0x07 // None
|
||||
#define GetStatus 0x08 // Input "16.8.1" ticks.pwm.error
|
||||
#define GetLEDI 0x09 // Input "8" LED current
|
||||
#define GetMotorType 0x0B // Input "8" Motor type
|
||||
#define ResetH2X 0xE4 // None
|
||||
|
||||
unsigned long timeout;
|
||||
|
||||
char BitCountArray[16] =
|
||||
{ 24,24,16,8,0,0,0,0,25,8,8,0,0,0,0,0 };
|
||||
|
||||
volatile unsigned char isr_state;
|
||||
volatile unsigned long isr_timeout;
|
||||
volatile unsigned char sFlag;
|
||||
volatile unsigned char BitCountIndex;
|
||||
volatile unsigned char HexData;
|
||||
volatile unsigned char HBXCmnd;
|
||||
volatile unsigned char HBXData;
|
||||
volatile unsigned char HBXByteCount;
|
||||
volatile unsigned char HBXBitCount;
|
||||
|
||||
volatile unsigned char H2XRxBuffer[H2XLEN]; // Hold data from H2X
|
||||
volatile unsigned char H2XRxiPtr = 0; // Pointer for input from H2X
|
||||
volatile unsigned char H2XRxoPtr = 0; // Pointer for output from H2X Rx buffer
|
||||
|
||||
|
||||
void setup() {
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial.println("H2X-Monitor");
|
||||
attachInterrupt(digitalPinToInterrupt(HCL1), hcl1_isr, FALLING);
|
||||
Serial.print ("Az on Pin ");
|
||||
Serial.println(HCL1);
|
||||
attachInterrupt(digitalPinToInterrupt(HCL2), hcl2_isr, FALLING);
|
||||
Serial.print ("Alt on Pin ");
|
||||
Serial.println(HCL2);
|
||||
|
||||
HBXMonitorMode();
|
||||
sFlag = 0;
|
||||
isr_state = START;
|
||||
isr_timeout = micros();
|
||||
interrupts();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
if (H2XRxoPtr != H2XRxiPtr) {
|
||||
Serial.write(H2XRxBuffer[H2XRxoPtr]);
|
||||
H2XRxoPtr++;
|
||||
H2XRxoPtr &= H2XMASK;
|
||||
}
|
||||
}
|
||||
|
||||
void HBXMonitorMode(void) {
|
||||
HDAListen(); // HDA as input
|
||||
HCL1Listen(); // HCLs as inputs
|
||||
HCL2Listen();
|
||||
}
|
||||
|
||||
void hcl1_isr() {
|
||||
if ((micros() - isr_timeout) > 5000)
|
||||
isr_state = START;
|
||||
isr_timeout = micros();
|
||||
if (isr_state == START) {
|
||||
HBXBitCount = 8;
|
||||
isr_state = COMMAND;
|
||||
H2XRxBuffer[H2XRxiPtr] = '1'; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
H2XRxBuffer[H2XRxiPtr] = ','; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
}
|
||||
else if (isr_state == COMMAND) {
|
||||
if (HBXBitCount > 0) { // Read the command
|
||||
HBXCmnd = (HBXCmnd << 1);
|
||||
HBXBitCount -= 1;
|
||||
if (digitalRead(HDA1) == HIGH)
|
||||
HBXCmnd |= 0x01;
|
||||
}
|
||||
else {
|
||||
HexData = HBXCmnd >> 4; // Get high nibble
|
||||
if (HexData <= 9) HexData += '0'; // convert to hex
|
||||
else HexData += '7';
|
||||
H2XRxBuffer[H2XRxiPtr] = HexData; // Put the (hex) data in the buffer
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
|
||||
HexData = HBXCmnd & 0x0F; // Get low nibble
|
||||
if (HexData <= 9) HexData += '0'; // convert to hex
|
||||
else HexData += '7';
|
||||
H2XRxBuffer[H2XRxiPtr] = HexData; // Put the (hex) data in the buffer
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
|
||||
H2XRxBuffer[H2XRxiPtr] = ','; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
|
||||
BitCountIndex = HBXCmnd & 0x0F; // Only 11 known commands
|
||||
HBXBitCount = BitCountArray[BitCountIndex]; // Get number of data bits
|
||||
if (HBXBitCount > 0) { // Check bitcount associated with command
|
||||
isr_state = DATA;
|
||||
HBXBitCount--; // This is the first data bit
|
||||
HBXByteCount = 1; // ditto
|
||||
if (digitalRead(HDA1) == HIGH) // Read the first data bit
|
||||
HBXData |= 0x01;
|
||||
}
|
||||
else { // Nothing to do - all finished - back to start
|
||||
isr_state = START;
|
||||
H2XRxBuffer[H2XRxiPtr] = CR; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
H2XRxBuffer[H2XRxiPtr] = LF; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (isr_state == DATA) {
|
||||
if (HBXBitCount > 0) { // Read the data
|
||||
HBXData = (HBXData << 1);
|
||||
HBXBitCount -= 1;
|
||||
HBXByteCount += 1;
|
||||
if (digitalRead(HDA1) == HIGH)
|
||||
HBXData |= 0x01;
|
||||
if ((HBXByteCount == 8) || (HBXBitCount == 0)) { // Full byte or last bit
|
||||
|
||||
HexData = HBXData >> 4; // Get high nibble
|
||||
if (HexData <= 9) HexData += '0'; // convert to hex
|
||||
else HexData += '7';
|
||||
H2XRxBuffer[H2XRxiPtr] = HexData; // Put the (hex) data in the buffer
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
|
||||
HexData = HBXData & 0x0F; // Get low nibble
|
||||
if (HexData <= 9) HexData += '0'; // convert to hex
|
||||
else HexData += '7';
|
||||
H2XRxBuffer[H2XRxiPtr] = HexData; // Put the (hex) data in the buffer
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
|
||||
if (HBXBitCount != 0) {
|
||||
H2XRxBuffer[H2XRxiPtr] = ','; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
}
|
||||
|
||||
HBXData = 0; // For the last data bit
|
||||
HBXByteCount = 0;
|
||||
}
|
||||
}
|
||||
if (HBXBitCount == 0) { // All data received - back to start
|
||||
isr_state = START;
|
||||
H2XRxBuffer[H2XRxiPtr] = CR; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
H2XRxBuffer[H2XRxiPtr] = LF; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void hcl2_isr() {
|
||||
if ((micros() - isr_timeout) > 5000)
|
||||
isr_state = START;
|
||||
isr_timeout = micros();
|
||||
if (isr_state == START) {
|
||||
HBXBitCount = 8;
|
||||
isr_state = COMMAND;
|
||||
H2XRxBuffer[H2XRxiPtr] = '2'; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
H2XRxBuffer[H2XRxiPtr] = ','; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
}
|
||||
else if (isr_state == COMMAND) {
|
||||
if (HBXBitCount > 0) { // Read the command
|
||||
HBXCmnd = (HBXCmnd << 1);
|
||||
HBXBitCount -= 1;
|
||||
if (digitalRead(HDA1) == HIGH)
|
||||
HBXCmnd |= 0x01;
|
||||
}
|
||||
else {
|
||||
HexData = HBXCmnd >> 4; // Get high nibble
|
||||
if (HexData <= 9) HexData += '0'; // convert to hex
|
||||
else HexData += '7';
|
||||
H2XRxBuffer[H2XRxiPtr] = HexData; // Put the (hex) data in the buffer
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
|
||||
HexData = HBXCmnd & 0x0F; // Get low nibble
|
||||
if (HexData <= 9) HexData += '0'; // convert to hex
|
||||
else HexData += '7';
|
||||
H2XRxBuffer[H2XRxiPtr] = HexData; // Put the (hex) data in the buffer
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
|
||||
BitCountIndex = HBXCmnd & 0x0F; // Only 11 known commands
|
||||
HBXBitCount = BitCountArray[BitCountIndex]; // Get number of data bits
|
||||
if (HBXBitCount > 0) { // Check bitcount associated with command
|
||||
isr_state = DATA;
|
||||
HBXBitCount--; // This is the first data bit
|
||||
HBXByteCount = 1; // ditto
|
||||
if (digitalRead(HDA1) == HIGH) // Read the first data bit
|
||||
HBXData |= 0x01;
|
||||
H2XRxBuffer[H2XRxiPtr] = ','; // Data to follow
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
}
|
||||
else { // Nothing to do - all finished - back to start
|
||||
isr_state = START;
|
||||
H2XRxBuffer[H2XRxiPtr] = CR; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
H2XRxBuffer[H2XRxiPtr] = LF; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (isr_state == DATA) {
|
||||
if (HBXBitCount > 0) { // Read the data
|
||||
HBXData = (HBXData << 1);
|
||||
HBXBitCount -= 1;
|
||||
HBXByteCount += 1;
|
||||
if (digitalRead(HDA1) == HIGH)
|
||||
HBXData |= 0x01;
|
||||
if ((HBXByteCount == 8) || (HBXBitCount == 0)) { // Full byte or last bit
|
||||
|
||||
HexData = HBXData >> 4; // Get high nibble
|
||||
if (HexData <= 9) HexData += '0'; // convert to hex
|
||||
else HexData += '7';
|
||||
H2XRxBuffer[H2XRxiPtr] = HexData; // Put the (hex) data in the buffer
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
|
||||
HexData = HBXData & 0x0F; // Get low nibble
|
||||
if (HexData <= 9) HexData += '0'; // convert to hex
|
||||
else HexData += '7';
|
||||
H2XRxBuffer[H2XRxiPtr] = HexData; // Put the (hex) data in the buffer
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
|
||||
if (HBXBitCount != 0) {
|
||||
H2XRxBuffer[H2XRxiPtr] = ','; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
}
|
||||
|
||||
HBXData = 0; // For the last data bit
|
||||
HBXByteCount = 0;
|
||||
}
|
||||
}
|
||||
if (HBXBitCount == 0) { // All data received - back to start
|
||||
isr_state = START;
|
||||
H2XRxBuffer[H2XRxiPtr] = CR; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
H2XRxBuffer[H2XRxiPtr] = LF; // Finish off with a CRLF
|
||||
H2XRxiPtr += 1;
|
||||
H2XRxiPtr &= H2XMASK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// H2X Low level Functions
|
||||
// -----------------------
|
||||
void HDAListen() {
|
||||
pinMode(HDA1, H2X_INPUT);
|
||||
// digitalWrite(HDA1, HIGH);
|
||||
}
|
||||
void HDAFloat() {
|
||||
pinMode(HDA1, H2X_INPUT);
|
||||
}
|
||||
void HDATalk() {
|
||||
digitalWrite(HDA1, HIGH);
|
||||
pinMode(HDA1, H2X_OUTPUT);
|
||||
}
|
||||
void HCL1Listen() {
|
||||
pinMode(HCL1, H2X_INPUT);
|
||||
}
|
||||
void HCL1Talk() {
|
||||
digitalWrite(HCL1, HIGH);
|
||||
pinMode(HCL1, H2X_OUTPUT);
|
||||
}
|
||||
void HCL2Listen() {
|
||||
pinMode(HCL2, H2X_INPUT);
|
||||
}
|
||||
void HCL2Talk() {
|
||||
digitalWrite(HCL2, HIGH);
|
||||
pinMode(HCL2, H2X_OUTPUT);
|
||||
}
|
||||
|
||||
void H2XReset() {
|
||||
HCL1Talk();
|
||||
HCL2Talk();
|
||||
HDATalk();
|
||||
/* digitalWrite(HDA1, LOW);
|
||||
TimerDelayuS(H2XRESETTIME);
|
||||
digitalWrite(HDA1, HIGH);
|
||||
TimerDelayuS(H2XRESETTIME);
|
||||
HDAListen();
|
||||
*/
|
||||
}
|
||||
|
Ładowanie…
Reference in New Issue