279 lines
6.2 KiB
C++
279 lines
6.2 KiB
C++
#include <Arduino.h>
|
|
|
|
#include "utils.h"
|
|
#include "main.h"
|
|
#include "bafang.h"
|
|
|
|
namespace Bafang {
|
|
|
|
HardwareSerial &DisplaySerial = Serial1;
|
|
HardwareSerial &MotorSerial = Serial2;
|
|
|
|
bool g_Proxy = true;
|
|
|
|
static unsigned long s_LastDisplayRx = 0;
|
|
static unsigned long s_LastMotorRx = 0;
|
|
|
|
static bool s_BlockDisplay = false;
|
|
static bool s_BlockMotor = false;
|
|
|
|
static uint8_t s_MotorBuffer[255];
|
|
static uint8_t s_MotorBufferLen = 0;
|
|
|
|
enum eJobState {
|
|
STATE_NEW = 0,
|
|
STATE_SEND = 1,
|
|
STATE_RECV = 2
|
|
};
|
|
|
|
struct sJob {
|
|
uint8_t *pData;
|
|
uint8_t dataLen;
|
|
uint16_t waitMs;
|
|
FCallback pfnCallback;
|
|
void *pUser;
|
|
};
|
|
|
|
static const int s_QueueSize = 8;
|
|
static sJob s_aQueue[s_QueueSize];
|
|
static int s_QueueRdIdx = 0;
|
|
static int s_QueueWrIdx = 0;
|
|
static int s_QueueLen = 0;
|
|
|
|
static eJobState s_JobState;
|
|
static unsigned long s_JobFinishTime = 0;
|
|
static void s_JobDone();
|
|
|
|
|
|
void OnDisplayRx(uint8_t c)
|
|
{
|
|
s_LastDisplayRx = millis();
|
|
|
|
if(s_BlockDisplay)
|
|
return;
|
|
|
|
MotorSerial.write(c);
|
|
}
|
|
|
|
void OnMotorRx(uint8_t c)
|
|
{
|
|
s_LastMotorRx = millis();
|
|
|
|
if(s_BlockMotor)
|
|
{
|
|
if(s_MotorBufferLen <= sizeof(s_MotorBuffer))
|
|
s_MotorBuffer[s_MotorBufferLen++] = c;
|
|
return;
|
|
}
|
|
|
|
DisplaySerial.write(c);
|
|
}
|
|
|
|
void recv()
|
|
{
|
|
static int s_lastSerial = -1;
|
|
|
|
while(Bafang::DisplaySerial.available())
|
|
{
|
|
uint8_t c = Bafang::DisplaySerial.read();
|
|
|
|
if(Main::g_Debug[Main::g_CurrentChannel] > 10)
|
|
{
|
|
if(s_lastSerial != 1)
|
|
Main::channel()->write("\nD: ");
|
|
char sBuffer[4] = "XX ";
|
|
bytes2hex((uint8_t *)&c, 1, sBuffer, sizeof(sBuffer));
|
|
Main::channel()->write(sBuffer);
|
|
s_lastSerial = 1;
|
|
}
|
|
|
|
Bafang::OnDisplayRx(c);
|
|
}
|
|
|
|
while(Bafang::MotorSerial.available())
|
|
{
|
|
uint8_t c = Bafang::MotorSerial.read();
|
|
|
|
if(Main::g_Debug[Main::g_CurrentChannel] > 10)
|
|
{
|
|
if(s_lastSerial != 2)
|
|
Main::channel()->write("\nM: ");
|
|
char sBuffer[4] = "XX ";
|
|
bytes2hex((uint8_t *)&c, 1, sBuffer, sizeof(sBuffer));
|
|
Main::channel()->write(sBuffer);
|
|
s_lastSerial = 2;
|
|
}
|
|
|
|
Bafang::OnMotorRx(c);
|
|
}
|
|
}
|
|
|
|
void tick()
|
|
{
|
|
recv();
|
|
|
|
bool bProxy = g_Proxy;
|
|
|
|
sJob *pJob = NULL;
|
|
if(s_QueueLen)
|
|
pJob = &s_aQueue[s_QueueRdIdx];
|
|
|
|
if(pJob)
|
|
{
|
|
bProxy = false;
|
|
|
|
if(s_BlockDisplay && s_BlockMotor)
|
|
{
|
|
switch(s_JobState)
|
|
{
|
|
case STATE_NEW:
|
|
{
|
|
s_MotorBufferLen = 0;
|
|
s_JobFinishTime = millis() + (pJob->dataLen + 2) * BYTE_TIME + pJob->waitMs;
|
|
s_JobState = STATE_SEND;
|
|
MotorSerial.write(pJob->pData, pJob->dataLen);
|
|
} break;
|
|
case STATE_SEND:
|
|
{
|
|
if(s_MotorBufferLen)
|
|
{
|
|
s_JobState = STATE_RECV;
|
|
break;
|
|
}
|
|
|
|
if(millis() >= s_JobFinishTime)
|
|
s_JobDone();
|
|
} break;
|
|
case STATE_RECV:
|
|
{
|
|
if(millis() - s_LastMotorRx > 2 * BYTE_TIME)
|
|
s_JobDone();
|
|
} break;
|
|
}
|
|
}
|
|
}
|
|
else if(s_MotorBufferLen && millis() - s_LastMotorRx > 2 * BYTE_TIME)
|
|
{
|
|
PrintCallback(Main::channel(), s_MotorBuffer, s_MotorBufferLen);
|
|
s_MotorBufferLen = 0;
|
|
}
|
|
|
|
if(!bProxy)
|
|
{
|
|
if(millis() - s_LastDisplayRx > 2 * BYTE_TIME)
|
|
s_BlockDisplay = true;
|
|
|
|
if(s_BlockDisplay && millis() - s_LastMotorRx > 2 * BYTE_TIME)
|
|
s_BlockMotor = true;
|
|
}
|
|
else
|
|
{
|
|
if(s_BlockDisplay && millis() - s_LastDisplayRx > 2 * BYTE_TIME)
|
|
{
|
|
s_BlockDisplay = false;
|
|
s_BlockMotor = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void s_JobDone()
|
|
{
|
|
if(!s_QueueLen)
|
|
return;
|
|
|
|
sJob &Job = s_aQueue[s_QueueRdIdx];
|
|
|
|
if(Job.pfnCallback)
|
|
Job.pfnCallback(Job.pUser, s_MotorBuffer, s_MotorBufferLen);
|
|
|
|
delete Job.pData;
|
|
Job.pData = NULL;
|
|
Job.dataLen = 0;
|
|
Job.waitMs = 0;
|
|
Job.pfnCallback = NULL;
|
|
Job.pUser = NULL;
|
|
|
|
s_QueueLen--;
|
|
s_QueueRdIdx++;
|
|
if(s_QueueRdIdx >= s_QueueSize)
|
|
s_QueueRdIdx = 0;
|
|
|
|
s_JobState = STATE_NEW;
|
|
}
|
|
|
|
bool QueueMotor(const uint8_t *pData, uint8_t dataLen, FCallback pfnCallback, void *pUser, uint16_t waitMs)
|
|
{
|
|
if(!dataLen)
|
|
return false;
|
|
|
|
if(s_QueueLen >= s_QueueSize)
|
|
return false;
|
|
|
|
sJob &Job = s_aQueue[s_QueueWrIdx];
|
|
Job.pData = new uint8_t[dataLen];
|
|
memcpy(Job.pData, pData, dataLen);
|
|
Job.dataLen = dataLen;
|
|
Job.waitMs = waitMs;
|
|
Job.pfnCallback = pfnCallback;
|
|
Job.pUser = pUser;
|
|
|
|
s_QueueLen++;
|
|
s_QueueWrIdx++;
|
|
if(s_QueueWrIdx >= s_QueueSize)
|
|
s_QueueWrIdx = 0;
|
|
|
|
s_JobState = STATE_NEW;
|
|
return true;
|
|
}
|
|
|
|
void PrintCallback(void *pUser, const uint8_t *pData, uint8_t dataLen)
|
|
{
|
|
int channel = (int)pUser;
|
|
if(channel == Main::CHANNEL_NONE)
|
|
return;
|
|
|
|
for(int i = 0; i < dataLen; i++)
|
|
Main::channel(channel)->printf("%02X", pData[i]);
|
|
Main::channel(channel)->write('\n');
|
|
}
|
|
|
|
void OnDisplayRequest(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen)
|
|
{
|
|
char aBuf[512];
|
|
bytes2hex(data, dataLen, aBuf, sizeof(aBuf));
|
|
Serial.printf("Display: %s\n", aBuf);
|
|
}
|
|
|
|
void OnMotorResponse(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen)
|
|
{
|
|
char aBuf[512];
|
|
bytes2hex(data, dataLen, aBuf, sizeof(aBuf));
|
|
Serial.printf("Motor: %s\n", aBuf);
|
|
}
|
|
|
|
int TransformPASCode(uint8_t PasCode)
|
|
{
|
|
static const uint8_t PasLevelArray[] = {
|
|
PAS_LEVEL_0, PAS_LEVEL_1, PAS_LEVEL_2, PAS_LEVEL_3,
|
|
PAS_LEVEL_4, PAS_LEVEL_5, PAS_LEVEL_6, PAS_LEVEL_7,
|
|
PAS_LEVEL_8, PAS_LEVEL_9, PAS_LEVEL_PUSH
|
|
};
|
|
|
|
for(int i = 0; i < sizeof(PasLevelArray)/sizeof(*PasLevelArray); i++)
|
|
{
|
|
if(PasCode == PasLevelArray[i])
|
|
return i;
|
|
}
|
|
|
|
return -1;
|
|
}
|
|
|
|
// rpm = 60 * v / (2 * pi * r)
|
|
// Example: 29inch tire, 30km/h
|
|
// v: 30 km/h / 3.6 = 8.33 m/s
|
|
// r: d = 29in * 0.7366m = -> r = 0.7366m / 2 = 0.3683m
|
|
// rps = 8.33 / (2 * pi * 0.3683) = 3.60
|
|
// rpm = 60 * 3.60 = 216
|
|
|
|
}
|