116 lines
2.4 KiB
C++
116 lines
2.4 KiB
C++
#include <Arduino.h>
|
|
|
|
#include "utils.h"
|
|
#include "main.h"
|
|
#include "bafang.h"
|
|
|
|
namespace Bafang {
|
|
namespace MotorMaster {
|
|
|
|
typedef void (*CommandCallback)(void *pUser, uint8_t *pData, int dataLen, bool crcError);
|
|
|
|
struct MotorCommand {
|
|
uint8_t *pSend;
|
|
uint8_t len;
|
|
bool crc;
|
|
CommandCallback pfnCallback;
|
|
void *pUser;
|
|
};
|
|
|
|
enum eMotorState {
|
|
STATE_IDLE = 0,
|
|
STATE_AWAITING = 1,
|
|
STATE_SUPERFLUOUS = 2
|
|
};
|
|
|
|
eMotorState g_State;
|
|
uint8_t g_RxBuffer[255];
|
|
uint8_t g_RxBufferLen = 0;
|
|
unsigned long g_LastRx = 0;
|
|
|
|
const sPacketDesc *g_pPacketDesc = NULL;
|
|
|
|
void OnData(uint8_t c)
|
|
{
|
|
if(!g_RxBufferLen)
|
|
g_LastRx = millis();
|
|
|
|
if(millis() - g_LastRx > 2 * BYTE_TIME)
|
|
{
|
|
g_State = STATE_IDLE;
|
|
g_RxBufferLen = 0;
|
|
g_pPacketDesc = NULL;
|
|
}
|
|
|
|
if(g_State == STATE_IDLE)
|
|
g_State = STATE_SUPERFLUOUS;
|
|
|
|
if(g_RxBufferLen >= sizeof(g_RxBuffer))
|
|
return;
|
|
|
|
g_RxBuffer[g_RxBufferLen++] = c;
|
|
|
|
if(g_pPacketDesc)
|
|
{
|
|
const sPacketDesc::sInfo &info = g_pPacketDesc->res;
|
|
|
|
if(info.len < 0)
|
|
{
|
|
g_State = STATE_IDLE;
|
|
return;
|
|
}
|
|
|
|
int readLen = info.len;
|
|
if(info.crc)
|
|
readLen++;
|
|
|
|
if(g_RxBufferLen < readLen)
|
|
return;
|
|
|
|
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;
|
|
return;
|
|
}
|
|
}
|
|
|
|
OnMotorResponse(g_pPacketDesc, g_RxBuffer, g_RxBufferLen);
|
|
g_State = STATE_IDLE;
|
|
g_RxBufferLen = 0;
|
|
g_pPacketDesc = NULL;
|
|
}
|
|
|
|
g_LastRx = millis();
|
|
}
|
|
|
|
void tick()
|
|
{
|
|
if((g_State == STATE_SUPERFLUOUS || g_State == STATE_AWAITING) && g_RxBufferLen && millis() - g_LastRx > 2 * BYTE_TIME)
|
|
{
|
|
OnMotorResponse(g_pPacketDesc, g_RxBuffer, g_RxBufferLen);
|
|
g_State = STATE_IDLE;
|
|
g_RxBufferLen = 0;
|
|
g_pPacketDesc = NULL;
|
|
}
|
|
}
|
|
|
|
void SendRawPacket(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen)
|
|
{
|
|
g_State = STATE_AWAITING;
|
|
g_RxBufferLen = 0;
|
|
g_pPacketDesc = packet;
|
|
MotorSerial.write(data, dataLen);
|
|
}
|
|
|
|
}
|
|
}
|