#include "TrackerRxTx.h"
|
#include <modules/my_task_creator/TaskCreator.h>
|
#include <modules/my_mavlink_tx/MavlinkTransmitter.h>
|
|
TrackerRxTx::TrackerRxTx()
|
{
|
mSerial=&Serial2;
|
}
|
|
TrackerRxTx::~TrackerRxTx()
|
{
|
|
}
|
|
void TrackerRxTx::Init(uint32_t baud)
|
{
|
#ifdef ARDUINO_WT32_ETH01
|
mSerial=&Serial2;
|
mSerial->begin(baud,SERIAL_8N1,HWSerial2Eth_rx_pin,HWSerial2Eth_tx_pin);
|
mSerial->setPins(HWSerial2Eth_rx_pin,HWSerial2Eth_tx_pin);//eth board
|
#elif defined ARDUINO_ESP32_DEV
|
mSerial=&Serial2;
|
mSerial->begin(baud,SERIAL_8N1,HWSerial2_rx_pin,HWSerial2_tx_pin);
|
#endif
|
}
|
|
void TrackerRxTx::TaskTrackerRxTx(void *pvParameters)
|
{
|
log_i("Task started");
|
|
TaskCreator *mCreator = (TaskCreator *)pvParameters;
|
TrackerRxTx *obj=&(mCreator->mTracker);
|
MavlinkTransmitter *mTransmiter=&(mCreator->mTransmitter);
|
while (1)
|
{
|
obj->ReadSerial();
|
obj->SendMavlik();
|
if(obj->mRedySend)
|
{
|
obj->mRedySend=false;
|
mTransmiter->SendMavlink(obj->mSenBuf,obj->mMsgSendLen);
|
}
|
vTaskDelay(1);
|
}
|
|
}
|
void TrackerRxTx::SetMavlikBuf(uint8_t *buf_in,int len)
|
{
|
mMutex.Lock();
|
if((mSendLen+len)<2*MAVLINK_MAX_PACKET_LEN)
|
{
|
memcpy(mBufferOutStream+mSendLen,buf_in,len);
|
mSendLen+=len;
|
mMavlinkNew=true;
|
}
|
else
|
log_i("TrackerTx buf over");
|
mMutex.Unlock();
|
}
|
void TrackerRxTx::SendMavlik()
|
{
|
if(mMavlinkNew)
|
{
|
mMutex.Lock();
|
mSerial->write(mBufferOutStream,mSendLen);
|
mMavlinkNew=false;
|
mSendLen=0;
|
mMutex.Unlock();
|
}
|
}
|
void TrackerRxTx::ReadSerial()
|
{
|
int len=mSerial->available();
|
while(len>2*MAVLINK_MAX_PACKET_LEN)
|
{
|
mSerial->read(mBuf,2*MAVLINK_MAX_PACKET_LEN);
|
len=mSerial->available();
|
}
|
|
if(len>0)
|
{
|
len=mSerial->read(mBuf,len);
|
mavlink_status_t status;
|
mavlink_message_t msg;
|
for(int i=0;i<len;i++)
|
{
|
ParseChar(mBuf[i]);
|
if(mavlink_parse_char(MAVLINK_COMM_1, mBuf[i], &msg, &status))
|
{
|
int mMsgSendLen= mavlink_msg_to_send_buffer(mSenBuf, &msg);
|
mRedySend=true;
|
}
|
}
|
|
|
}
|
}
|
|
void TrackerRxTx::GetTrackerData(uint16_t &x,uint16_t &y,uint8_t &v,uint8_t &mode,uint16_t &cnt)
|
{
|
mMutex.Lock();
|
x=mX;
|
y=mY;
|
v=mDataValid;
|
cnt=mMsgCnt;
|
mode=mMode;
|
mDataRedy=false;
|
mMutex.Unlock();
|
}
|
void TrackerRxTx::ParseChar(uint8_t c)
|
{
|
static uint8_t cnt_good=0;
|
static uint8_t cnt_bad=0;
|
switch (msg_state)
|
{
|
case ReadHeader:
|
if(c==0x1A)
|
{
|
msg_idx=0;
|
msg_state=ReadData;
|
msg_buf[0]=0x1A;
|
msg_idx++;
|
}
|
break;
|
case ReadData:
|
{
|
msg_idx++;
|
if(msg_idx<7)
|
{
|
msg_buf[msg_idx-1]=c;
|
}
|
else
|
{
|
msg_buf[msg_idx-1]=c;
|
msg_state=CalcCrc;
|
}
|
}
|
break;
|
case CalcCrc:
|
{
|
msg_buf[msg_idx]=c;
|
uint8_t crc=0xFF;
|
crc=crc8(crc,msg_buf,7);
|
if(crc==msg_buf[7])
|
{
|
for(int i=0;i<6;i++)
|
mBufferIn.buf[i]=msg_buf[i+1];
|
msg_idx=0;
|
msg_state=ReadHeader;
|
mMutex.Lock();
|
if(mBufferIn.data.msg_mode==0x80)
|
{
|
int32_t v=mBufferIn.data.msg_x+(mBufferIn.data.msg_y<<16);
|
mInt2Float.mInt=v;
|
mCamValue=mInt2Float.mFloat;
|
mCamDataRedy=true;
|
}
|
else
|
{
|
mX=mBufferIn.data.msg_x;
|
mY=mBufferIn.data.msg_y;
|
mDataValid=mBufferIn.data.msg_valid;
|
mMode=mBufferIn.data.msg_mode;
|
mDataRedy=true;
|
|
}
|
mMutex.Unlock();
|
cnt_good++;
|
mMsgCnt++;
|
if(cnt_good==20)
|
{
|
log_i("Tracker data x=%i y=%i v=%i m=%i",mX,mY,mDataValid,mMode);
|
cnt_good=0;
|
}
|
}
|
else
|
{
|
cnt_bad++;
|
if(cnt_bad==10)
|
{
|
log_i("Tracker data bad crc_msg=%i crc_calc=%i",c,crc);
|
cnt_bad=0;
|
}
|
mDataRedy=false;
|
bool ok=true;
|
uint8_t i=1;
|
while (ok)
|
{
|
if(msg_buf[i]==0x1A)
|
{
|
ok=false;
|
}
|
else
|
{
|
i++;
|
if(i==8)
|
ok=false;
|
}
|
}
|
if(i==8)
|
{
|
msg_idx=0;
|
msg_state=ReadHeader;
|
}
|
else
|
{
|
for(int j=0;j<8-i;j++)
|
{
|
msg_buf[j]=msg_buf[j+i];
|
}
|
msg_idx=8-i;
|
if(msg_idx==7)
|
msg_state=CalcCrc;
|
else
|
msg_state=ReadData;
|
}
|
}
|
}
|
break;
|
default:
|
break;
|
}
|
}
|
|
float TrackerRxTx::GetCamData()
|
{
|
float res;
|
mMutex.Lock();
|
res=mCamValue;
|
mCamDataRedy=false;
|
mMutex.Unlock();
|
return res;
|
}
|
|
unsigned char TrackerRxTx::crc8(unsigned char crc, unsigned char * dat, unsigned int len)
|
{
|
|
while(len--)
|
{
|
crc ^= *dat++;
|
|
for (unsigned int i = 0; i < 8; i++) crc = crc & 0x80 ? (crc << 1) ^ 0x31 : crc << 1;
|
}
|
|
return crc;
|
}
|
