132 lines
2.8 KiB
C++
132 lines
2.8 KiB
C++
|
#include <Arduino.h>
|
||
|
|
||
|
#include "utils.h"
|
||
|
#include "main.h"
|
||
|
#include "bafang.h"
|
||
|
|
||
|
namespace Bafang {
|
||
|
namespace DisplaySlave {
|
||
|
|
||
|
enum eDisplayState {
|
||
|
STATE_IDLE = 0,
|
||
|
STATE_REQUEST = 1,
|
||
|
STATE_RESPONSE = 2
|
||
|
};
|
||
|
|
||
|
eDisplayState g_State;
|
||
|
uint8_t g_RxBuffer[255];
|
||
|
uint8_t g_RxBufferLen = 0;
|
||
|
unsigned long g_LastRx = 0;
|
||
|
|
||
|
void OnData(uint8_t c)
|
||
|
{
|
||
|
static const sPacketDesc *packet = NULL;
|
||
|
|
||
|
if(g_State == STATE_REQUEST && millis() - g_LastRx > 2 * BYTE_TIME)
|
||
|
g_State = STATE_IDLE;
|
||
|
|
||
|
if(g_State == STATE_RESPONSE)
|
||
|
g_State = STATE_IDLE;
|
||
|
|
||
|
if(g_State == STATE_IDLE)
|
||
|
{
|
||
|
packet = NULL;
|
||
|
g_RxBufferLen = 0;
|
||
|
}
|
||
|
|
||
|
if(g_RxBufferLen >= sizeof(g_RxBuffer))
|
||
|
return;
|
||
|
|
||
|
g_RxBuffer[g_RxBufferLen++] = c;
|
||
|
|
||
|
switch(g_State)
|
||
|
{
|
||
|
case STATE_IDLE:
|
||
|
{
|
||
|
if(c == MODE_READ || c == MODE_WRITE)
|
||
|
g_State = STATE_REQUEST;
|
||
|
} break;
|
||
|
|
||
|
case STATE_REQUEST:
|
||
|
{
|
||
|
if(g_RxBufferLen == 2)
|
||
|
{
|
||
|
packet = NULL;
|
||
|
for(int i = 0; i < sizeof(PacketDesc)/sizeof(*PacketDesc); i++)
|
||
|
{
|
||
|
if(PacketDesc[i].cmd == c)
|
||
|
{
|
||
|
packet = &PacketDesc[i];
|
||
|
break;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if(!packet)
|
||
|
{
|
||
|
Serial.printf("Display: !!! UNKNOWN CMD %X\n", c);
|
||
|
g_State = STATE_IDLE;
|
||
|
break;
|
||
|
}
|
||
|
const sPacketDesc::sInfo &info = packet->req;
|
||
|
|
||
|
if(info.len < 0)
|
||
|
{
|
||
|
g_State = STATE_IDLE;
|
||
|
break;
|
||
|
}
|
||
|
|
||
|
int readLen = 2 + info.len;
|
||
|
if(info.crc)
|
||
|
readLen++;
|
||
|
|
||
|
if(g_RxBufferLen < readLen)
|
||
|
break;
|
||
|
|
||
|
if(info.crc)
|
||
|
{
|
||
|
uint8_t crc = 0;
|
||
|
for(int i = 0; i < g_RxBufferLen - 1; i++)
|
||
|
crc += g_RxBuffer[i];
|
||
|
|
||
|
if(c != crc)
|
||
|
{
|
||
|
char bla[16];
|
||
|
bytes2hex(g_RxBuffer, g_RxBufferLen, bla, sizeof(bla));
|
||
|
Serial.printf("!!! CRC ERROR %X != %X -> %s\n", c, crc, bla);
|
||
|
g_State = STATE_IDLE;
|
||
|
break;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if(packet->res.len > 0)
|
||
|
g_State = STATE_RESPONSE;
|
||
|
else
|
||
|
g_State = STATE_IDLE;
|
||
|
|
||
|
OnDisplayRequest(packet, g_RxBuffer, g_RxBufferLen);
|
||
|
} break;
|
||
|
|
||
|
default: break;
|
||
|
}
|
||
|
|
||
|
g_LastRx = millis();
|
||
|
}
|
||
|
|
||
|
bool ReplyRawPacket(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen)
|
||
|
{
|
||
|
if(g_State != STATE_RESPONSE)
|
||
|
return false;
|
||
|
|
||
|
DisplaySerial.write(data, dataLen);
|
||
|
g_State = STATE_IDLE;
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
void tick()
|
||
|
{
|
||
|
|
||
|
}
|
||
|
|
||
|
}
|
||
|
}
|