lbsadmin
/
HCPBridgeESP32
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
HCPBridgeESP32/HCPBridge/src/hciemulator.cpp

425 lines
14 KiB
C++
Raw Blame History

#include "hciemulator.h"
#define CHECKCHANGEDSET(Target,Value,Flag) if((Target)!=(Value)){Target=Value;Flag=true;}
int hciloglevel = DEFAULTLOGLEVEL;
#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(Stream * port) {
m_state.valid = false;
m_statemachine=WAITING;
m_rxlen = m_txlen = 0;
m_recvTime=m_lastStateTime=0;
m_skipFrame=false;
m_port = port;
m_statusCallback = NULL;
setLogLevel(DEFAULTLOGLEVEL);
};
void HCIEmulator::poll(){
if(m_port==NULL) return;
// receive Data
if(m_port->available() >0)
{
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();
}
// check frame, process frame
if(m_rxlen>0 && (micros()-m_recvTime > T3_5))
{
// 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());
m_port->write(m_txbuffer, m_txlen);
Log3(LL_DEBUG,"Response: ", m_txbuffer, m_txlen);
m_txlen = 0;
}
}
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_P(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_P(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_P(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;
}
Reference in New Issue View Git Blame Copy Permalink