493 lines
14 KiB
C++
493 lines
14 KiB
C++
#include "hciemulator.h"
|
|
#include "Arduino.h"
|
|
#define CHECKCHANGEDSET(Target, Value, Flag) \
|
|
if ((Target) != (Value)) \
|
|
{ \
|
|
Target = Value; \
|
|
Flag = true; \
|
|
}
|
|
int hciloglevel = LL_DEBUG;
|
|
|
|
#define SOFTSERIAL 1
|
|
|
|
#ifdef SOFTSERIAL
|
|
#define Log(Level, Message) LogCore(Level, Message)
|
|
#define Log3(Level, Message, Buffer, Len) LogCore(Level, Message, Buffer, Len)
|
|
// LOGLEVEL
|
|
void LogCore(int Level, const char *msg, const unsigned char *data = NULL, size_t datalen = 0)
|
|
{
|
|
if (Level > hciloglevel)
|
|
{
|
|
return;
|
|
}
|
|
if (data != NULL && datalen > 0)
|
|
{
|
|
String newmsg(msg);
|
|
char str[4];
|
|
for (size_t i = 0; i < datalen; i++)
|
|
{
|
|
snprintf(str, sizeof(str), "%02x ", data[i]);
|
|
newmsg += str;
|
|
}
|
|
Serial.println(newmsg);
|
|
}
|
|
else
|
|
{
|
|
Serial.println(msg);
|
|
}
|
|
}
|
|
#else
|
|
#define Log(Level, Message)
|
|
#define Log3(Level, Message, Buffer, Len)
|
|
#endif
|
|
|
|
int HCIEmulator::getLogLevel()
|
|
{
|
|
return hciloglevel;
|
|
}
|
|
void HCIEmulator::setLogLevel(int level)
|
|
{
|
|
hciloglevel = level;
|
|
}
|
|
|
|
// modbus crc calculation borrowed from:
|
|
// https://github.com/yaacov/ArduinoModbusSlave
|
|
#define MODBUS_CRC_LENGTH 2
|
|
#define readCRC(arr, length) word(arr[(length - MODBUS_CRC_LENGTH) + 1], arr[length - MODBUS_CRC_LENGTH])
|
|
#define readUInt16(arr, index) word(arr[index], arr[index + 1])
|
|
/**
|
|
* Calculate the CRC of the passed byte array from zero up to the passed length.
|
|
*
|
|
* @param buffer The byte array containing the data.
|
|
* @param length The length of the byte array.
|
|
*
|
|
* @return The calculated CRC as an unsigned 16 bit integer.
|
|
*
|
|
* Calculate and add the CRC.
|
|
* uint16_t crc = Modbus::calculateCRC(_responseBuffer, _responseBufferLength - MODBUS_CRC_LENGTH);
|
|
* _responseBuffer[_responseBufferLength - MODBUS_CRC_LENGTH] = crc & 0xFF;
|
|
* _responseBuffer[(_responseBufferLength - MODBUS_CRC_LENGTH) + 1] = crc >> 8;
|
|
*
|
|
*
|
|
* #define MODBUS_FRAME_SIZE 4
|
|
* #define MODBUS_CRC_LENGTH 2
|
|
* uint16_t crc = readCRC(_requestBuffer, _requestBufferLength);
|
|
* #define readUInt16(arr, index) word(arr[index], arr[index + 1])
|
|
* #define readCRC(arr, length) word(arr[(length - MODBUS_CRC_LENGTH) + 1], arr[length - MODBUS_CRC_LENGTH])
|
|
*/
|
|
uint16_t calculateCRC(uint8_t *buffer, int length)
|
|
{
|
|
int i, j;
|
|
uint16_t crc = 0xFFFF;
|
|
uint16_t tmp;
|
|
|
|
// Calculate the CRC.
|
|
for (i = 0; i < length; i++)
|
|
{
|
|
crc = crc ^ buffer[i];
|
|
for (j = 0; j < 8; j++)
|
|
{
|
|
tmp = crc & 0x0001;
|
|
crc = crc >> 1;
|
|
if (tmp)
|
|
{
|
|
crc = crc ^ 0xA001;
|
|
}
|
|
}
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
HCIEmulator::HCIEmulator()
|
|
{
|
|
m_state.valid = false;
|
|
m_statemachine = WAITING;
|
|
m_rxlen = m_txlen = 0;
|
|
m_recvTime = m_lastStateTime = 0;
|
|
m_skipFrame = false;
|
|
m_port = &Serial2;
|
|
m_statusCallback = NULL;
|
|
setLogLevel(DEFAULTLOGLEVEL);
|
|
};
|
|
|
|
#define TX_ON 25
|
|
|
|
void HCIEmulator::poll()
|
|
{
|
|
|
|
if (m_port == NULL)
|
|
return;
|
|
|
|
// receive Data
|
|
if (m_port->available() > 0)
|
|
{
|
|
// Serial.println("got data");
|
|
m_rxlen += m_port->readBytes((char *)(m_rxbuffer + m_rxlen), _min((int)(255 - m_rxlen), m_port->available()));
|
|
if (m_rxlen > 254)
|
|
{
|
|
Log(LL_ERROR, "RX Bufferoverflow, skip next Frame");
|
|
Log3(LL_DEBUG, "Buffer Data: ", m_rxbuffer, m_rxlen);
|
|
m_rxlen = 0;
|
|
m_skipFrame = true;
|
|
}
|
|
m_recvTime = micros();
|
|
}
|
|
|
|
// Serial.printf("Data % x\n", m_txbuffer);
|
|
// check frame, process frame
|
|
if (m_rxlen > 0 && (micros() - m_recvTime > T3_5))
|
|
{
|
|
// Serial.printf("Act on it % x\n", m_txbuffer);
|
|
// check last action timeout -> reset > then 2sec
|
|
if (m_statemachine != WAITING && m_lastStateTime + 2000 < millis())
|
|
{
|
|
m_statemachine = WAITING;
|
|
}
|
|
|
|
if (!m_skipFrame)
|
|
{
|
|
|
|
processFrame();
|
|
|
|
// send response
|
|
if (m_txlen > 0)
|
|
{
|
|
// fix crc
|
|
uint16_t crc = calculateCRC(m_txbuffer, m_txlen - MODBUS_CRC_LENGTH);
|
|
m_txbuffer[m_txlen - MODBUS_CRC_LENGTH] = crc & 0xFF;
|
|
m_txbuffer[(m_txlen - MODBUS_CRC_LENGTH) + 1] = crc >> 8;
|
|
|
|
// send data
|
|
m_lastSendTime = micros() - m_recvTime;
|
|
|
|
// Log(LL_DEBUG, ("ST:"+String(m_lastSendTime)).c_str());
|
|
|
|
digitalWrite(TX_ON, HIGH);
|
|
// Log3(LL_DEBUG, "write data: ");
|
|
m_port->write(m_txbuffer, m_txlen);
|
|
Log3(LL_DEBUG, "Response: ", m_txbuffer, m_txlen);
|
|
delayMicroseconds(m_txlen * 9 * 22); // 8 bits + par * Bittime 18 micros on 57600 bauds
|
|
digitalWrite(TX_ON, LOW);
|
|
m_txlen = 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Serial.println("skipped frame");
|
|
}
|
|
|
|
m_skipFrame = false;
|
|
m_rxlen = 0;
|
|
}
|
|
}
|
|
|
|
void HCIEmulator::processFrame()
|
|
{
|
|
m_txlen = 0; // clear send buffer
|
|
|
|
if (m_rxlen < 5)
|
|
{
|
|
Log(LL_ERROR, "Frame skipped, invalid frame len");
|
|
Log3(LL_ERROR, "Data:", m_rxbuffer, m_rxlen);
|
|
return;
|
|
}
|
|
|
|
// check device id, pass only device id 2 and 0 (broadcast)
|
|
if (m_rxbuffer[0] != BROADCASTID && m_rxbuffer[0] != DEVICEID)
|
|
{
|
|
Log(LL_DEBUG, "Frame skipped, unsupported device id");
|
|
Log3(LL_DEBUG, "Data:", m_rxbuffer, m_rxlen);
|
|
return;
|
|
}
|
|
|
|
// check crc
|
|
uint16_t crc = readCRC(m_rxbuffer, m_rxlen);
|
|
if (crc != calculateCRC(m_rxbuffer, m_rxlen - MODBUS_CRC_LENGTH))
|
|
{
|
|
Log3(LL_ERROR, "Frame skipped, wrong crc", m_rxbuffer, m_rxlen);
|
|
return;
|
|
}
|
|
|
|
Log3(LL_DEBUG, "Incomming Data: ", m_rxbuffer, m_rxlen);
|
|
|
|
// dispatch modbus function
|
|
switch (m_rxbuffer[1])
|
|
{
|
|
case 0x10:
|
|
{ // Write Multiple registers
|
|
if (m_rxlen == 0x1b && m_rxbuffer[0] == BROADCASTID)
|
|
{
|
|
processBroadcastStatusFrame();
|
|
return;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case 0x17:
|
|
{ // Read/Write Multiple registers
|
|
if (m_rxbuffer[0] == DEVICEID)
|
|
{
|
|
switch (m_rxlen)
|
|
{
|
|
case 0x11:
|
|
{
|
|
processDeviceStatusFrame();
|
|
return;
|
|
}
|
|
|
|
case 0x13:
|
|
processDeviceBusScanFrame();
|
|
return;
|
|
;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
Log3(LL_ERROR, "Frame skipped, unexpected data: ", m_rxbuffer, m_rxlen);
|
|
}
|
|
|
|
const unsigned char ResponseTemplate_Fcn17_Cmd03_L08[] = {0x02, 0x17, 0x10, 0x3E, 0x00, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x74, 0x1B};
|
|
const unsigned char ResponseTemplate_Fcn17_Cmd04_L02[] = {0x02, 0x17, 0x04, 0x0F, 0x00, 0x04, 0xFD, 0x0A, 0x72};
|
|
void HCIEmulator::processDeviceStatusFrame()
|
|
{
|
|
if (m_rxlen == 0x11)
|
|
{
|
|
unsigned char counter = m_rxbuffer[11];
|
|
unsigned char cmd = m_rxbuffer[12];
|
|
if (m_rxbuffer[5] == 0x08)
|
|
{
|
|
// expose internal state
|
|
// 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16
|
|
// 0011: 02 17 9C B9 00 08 9C 41 00 02 04 3E 03 00 00 EB CC
|
|
// res=> 02 17 10 3E 00 03 01 00 00 00 00 00 00 00 00 00 00 00 00 74 1B
|
|
memcpy(m_txbuffer, ResponseTemplate_Fcn17_Cmd03_L08, sizeof(ResponseTemplate_Fcn17_Cmd03_L08));
|
|
m_txbuffer[0] = m_rxbuffer[0];
|
|
m_txbuffer[3] = counter;
|
|
m_txbuffer[5] = cmd;
|
|
m_txlen = sizeof(ResponseTemplate_Fcn17_Cmd03_L08);
|
|
|
|
switch (m_statemachine)
|
|
{
|
|
// open Door
|
|
case STARTOPENDOOR:
|
|
m_txbuffer[7] = 0x02;
|
|
m_txbuffer[8] = 0x10;
|
|
m_statemachine = STARTOPENDOOR_RELEASE;
|
|
m_lastStateTime = millis();
|
|
break;
|
|
case STARTOPENDOOR_RELEASE:
|
|
if (m_lastStateTime + SIMULATEKEYPRESSDELAYMS < millis())
|
|
{
|
|
m_txbuffer[7] = 0x01;
|
|
m_txbuffer[8] = 0x10;
|
|
m_statemachine = WAITING;
|
|
}
|
|
break;
|
|
|
|
// close Door
|
|
case STARTCLOSEDOOR:
|
|
m_txbuffer[7] = 0x02;
|
|
m_txbuffer[8] = 0x20;
|
|
m_statemachine = STARTCLOSEDOOR_RELEASE;
|
|
m_lastStateTime = millis();
|
|
break;
|
|
case STARTCLOSEDOOR_RELEASE:
|
|
if (m_lastStateTime + SIMULATEKEYPRESSDELAYMS < millis())
|
|
{
|
|
m_txbuffer[7] = 0x01;
|
|
m_txbuffer[8] = 0x20;
|
|
m_statemachine = WAITING;
|
|
}
|
|
break;
|
|
|
|
// stop Door
|
|
case STARTSTOPDOOR:
|
|
m_txbuffer[7] = 0x02;
|
|
m_txbuffer[8] = 0x40;
|
|
m_statemachine = STARTSTOPDOOR_RELEASE;
|
|
m_lastStateTime = millis();
|
|
break;
|
|
case STARTSTOPDOOR_RELEASE:
|
|
if (m_lastStateTime + SIMULATEKEYPRESSDELAYMS < millis())
|
|
{
|
|
m_txbuffer[7] = 0x01;
|
|
m_txbuffer[8] = 0x40;
|
|
m_statemachine = WAITING;
|
|
}
|
|
break;
|
|
|
|
// Ventilation
|
|
case STARTVENTPOSITION:
|
|
m_txbuffer[7] = 0x02;
|
|
m_txbuffer[9] = 0x40;
|
|
m_statemachine = STARTVENTPOSITION_RELEASE;
|
|
m_lastStateTime = millis();
|
|
break;
|
|
case STARTVENTPOSITION_RELEASE:
|
|
if (m_lastStateTime + SIMULATEKEYPRESSDELAYMS < millis())
|
|
{
|
|
m_txbuffer[7] = 0x01;
|
|
m_txbuffer[9] = 0x40;
|
|
m_statemachine = WAITING;
|
|
}
|
|
break;
|
|
|
|
// Half Position
|
|
case STARTOPENDOORHALF:
|
|
m_txbuffer[7] = 0x02;
|
|
m_txbuffer[9] = 0x04;
|
|
m_statemachine = STARTOPENDOORHALF_RELEASE;
|
|
m_lastStateTime = millis();
|
|
break;
|
|
|
|
case STARTOPENDOORHALF_RELEASE:
|
|
if (m_lastStateTime + SIMULATEKEYPRESSDELAYMS < millis())
|
|
{
|
|
m_txbuffer[7] = 0x01;
|
|
m_txbuffer[9] = 0x04;
|
|
m_statemachine = WAITING;
|
|
}
|
|
break;
|
|
|
|
// Toggle Lamp
|
|
case STARTTOGGLELAMP:
|
|
m_txbuffer[7] = 0x10;
|
|
m_txbuffer[9] = 0x02;
|
|
m_statemachine = STARTTOGGLELAMP_RELEASE;
|
|
m_lastStateTime = millis();
|
|
break;
|
|
case STARTTOGGLELAMP_RELEASE:
|
|
if (m_lastStateTime + SIMULATEKEYPRESSDELAYMS < millis())
|
|
{
|
|
m_txbuffer[7] = 0x08;
|
|
m_txbuffer[9] = 0x02;
|
|
m_statemachine = WAITING;
|
|
}
|
|
break;
|
|
|
|
case WAITING:
|
|
break;
|
|
}
|
|
return;
|
|
}
|
|
else if (m_rxbuffer[5] == 0x02)
|
|
{
|
|
// 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16
|
|
// 0011: 02 17 9C B9 00 02 9C 41 00 02 04 0F 04 17 00 7B 21
|
|
// res=> 02 17 04 0F 00 04 FD 0A 72
|
|
memcpy(m_txbuffer, ResponseTemplate_Fcn17_Cmd04_L02, sizeof(ResponseTemplate_Fcn17_Cmd04_L02));
|
|
m_txbuffer[0] = m_rxbuffer[0];
|
|
m_txbuffer[3] = counter;
|
|
m_txbuffer[5] = cmd;
|
|
m_txlen = sizeof(ResponseTemplate_Fcn17_Cmd04_L02);
|
|
return;
|
|
}
|
|
}
|
|
|
|
Log3(LL_ERROR, "Frame skipped, unexpected data: ", m_rxbuffer, m_rxlen);
|
|
}
|
|
|
|
const unsigned char ResponseTemplate_Fcn17_Cmd02_L05[] = {0x02, 0x17, 0x0a, 0x00, 0x00, 0x02, 0x05, 0x04, 0x30, 0x10, 0xff, 0xa8, 0x45, 0x0e, 0xdf};
|
|
void HCIEmulator::processDeviceBusScanFrame()
|
|
{
|
|
// 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16
|
|
// 0013: 02 17 9C B9 00 05 9C 41 00 03 06 00 02 00 00 01 02 f8 35
|
|
// res=> 02 17 0a 00 00 02 05 04 30 10 ff a8 45 0e df
|
|
unsigned char counter = m_rxbuffer[11];
|
|
unsigned char cmd = m_rxbuffer[12];
|
|
memcpy(m_txbuffer, ResponseTemplate_Fcn17_Cmd02_L05, sizeof(ResponseTemplate_Fcn17_Cmd02_L05));
|
|
m_txbuffer[0] = m_rxbuffer[0];
|
|
m_txbuffer[3] = counter;
|
|
m_txbuffer[5] = cmd;
|
|
m_txlen = sizeof(ResponseTemplate_Fcn17_Cmd02_L05);
|
|
|
|
Log(LL_INFO, "Busscan received");
|
|
}
|
|
|
|
void HCIEmulator::processBroadcastStatusFrame()
|
|
{
|
|
// 001B: 00 10 9D 31 00 09 12 64 00 00 00 40 60 00 00 00 00 00 00 00 00 00 01 00 00 CA 22
|
|
bool hasChanged = false;
|
|
CHECKCHANGEDSET(m_state.lampOn, m_rxbuffer[20] == 0x14, hasChanged);
|
|
CHECKCHANGEDSET(m_state.doorCurrentPosition, m_rxbuffer[10], hasChanged);
|
|
CHECKCHANGEDSET(m_state.doorTargetPosition, m_rxbuffer[9], hasChanged);
|
|
CHECKCHANGEDSET(m_state.doorState, m_rxbuffer[11], hasChanged);
|
|
CHECKCHANGEDSET(m_state.reserved, m_rxbuffer[17], hasChanged);
|
|
CHECKCHANGEDSET(m_state.valid, true, hasChanged);
|
|
|
|
if (hasChanged)
|
|
{
|
|
Log3(LL_INFO, "New State: ", m_rxbuffer, m_rxlen);
|
|
if (m_statusCallback != NULL)
|
|
{
|
|
m_statusCallback(m_state);
|
|
}
|
|
}
|
|
}
|
|
|
|
void HCIEmulator::openDoor()
|
|
{
|
|
if (m_statemachine != WAITING)
|
|
{
|
|
return;
|
|
}
|
|
m_lastStateTime = millis();
|
|
m_statemachine = STARTOPENDOOR;
|
|
}
|
|
|
|
void HCIEmulator::openDoorHalf()
|
|
{
|
|
if (m_statemachine != WAITING)
|
|
{
|
|
return;
|
|
}
|
|
m_lastStateTime = millis();
|
|
m_statemachine = STARTOPENDOORHALF;
|
|
}
|
|
|
|
void HCIEmulator::closeDoor()
|
|
{
|
|
if (m_statemachine != WAITING)
|
|
{
|
|
return;
|
|
}
|
|
m_lastStateTime = millis();
|
|
m_statemachine = STARTCLOSEDOOR;
|
|
}
|
|
|
|
void HCIEmulator::stopDoor()
|
|
{
|
|
if (m_statemachine != WAITING)
|
|
{
|
|
return;
|
|
}
|
|
m_lastStateTime = millis();
|
|
m_statemachine = STARTSTOPDOOR;
|
|
}
|
|
|
|
void HCIEmulator::toggleLamp()
|
|
{
|
|
if (m_statemachine != WAITING)
|
|
{
|
|
return;
|
|
}
|
|
m_lastStateTime = millis();
|
|
m_statemachine = STARTTOGGLELAMP;
|
|
}
|
|
|
|
void HCIEmulator::ventilationPosition()
|
|
{
|
|
if (m_statemachine != WAITING)
|
|
{
|
|
return;
|
|
}
|
|
m_lastStateTime = millis();
|
|
m_statemachine = STARTVENTPOSITION;
|
|
}
|
|
|
|
void HCIEmulator::onStatusChanged(callback_function_t handler)
|
|
{
|
|
m_statusCallback = handler;
|
|
}
|