void Drone::Connected()
{
std::cout << "Connected\n";
m_Client = m_Server.nextPendingConnection(); //m_Client on connection
connect(m_Client, SIGNAL(readyRead()), this, SLOT(ReadCmd())); //If recive data call ReadCmd()
m_Connected = true;
SendAnswer("Hello Client\n");
}
void Drone::ReadCmd()
{
GLfloat x, y, z;
//When recive data
char buffer[1024] = {0};
m_Client->read(buffer, m_Client->bytesAvailable());
//std::string answer("My Z crd = ");
//answer.append(std::to_string(center_z));
//m_Client->write(answer.c_str());
//Decode cmd there
std::string cmd(buffer);
if(cmd.find("M_") != std::string::npos)
{
cmd = cmd.substr(2);
int pos = 0, size = cmd.size();
char symb = cmd[pos];
std::string tempStr;
while(symb != ':')
{
tempStr.push_back(symb);
pos++;
if(pos == size)
{
std::string answer("Error cmd:");
answer.append(buffer);
SendAnswer(answer);
return;
}
symb = cmd[pos];
}
x = atof(tempStr.c_str());
tempStr.clear();
pos++;
symb = cmd[pos];
while(symb != ':')
{
tempStr.push_back(symb);
pos++;
if(pos == size)
{
std::string answer("Error cmd:");
answer.append(buffer);
SendAnswer(answer);
return;
}
symb = cmd[pos];
}
y = atof(tempStr.c_str());
tempStr.clear();
pos++;
symb = cmd[pos];
while(symb != '|')
{
tempStr.push_back(symb);
pos++;
if(pos == size)
{
std::string answer("Error cmd:");
answer.append(buffer);
SendAnswer(answer);
return;
}
symb = cmd[pos];
}
z = atof(tempStr.c_str());
add_path(x, y, z);
std::string answer("Good cmd: add Path\n");
answer.append(std::to_string(x));
answer.push_back(' ');
answer.append(std::to_string(y));
answer.push_back(' ');
answer.append(std::to_string(z));
answer.push_back(' ');
SendAnswer(answer);
}
else
{
std::string answer("Error cmd:");
answer.append(buffer);
SendAnswer(answer);
}
//Do Task there
//add_path(x, y, z);
//TODO: добавить в парсер возможность других команд, кроме движения, исп add_command(int);
}
//------------------------------------------------------------------------------------------------------------------------------------------------------
err_t CallbackOnRecieve(void *_arg, struct tcp_pcb *_tpcb, struct pbuf *_p, err_t _err)
{
err_t ret_err;
LWIP_ASSERT("arg != NULL", _arg != NULL);
struct State *ss = (struct State*)_arg;
if (_p == NULL)
{
// remote host closed connection
ss->state = S_CLOSING;
if (ss->p == NULL)
{
// we're done sending, close it
CloseConnection(_tpcb, ss);
}
else
{
// we're not done yet
//tcp_sent(_tpcb, CallbackOnSent);
}
ret_err = ERR_OK;
}
else if (_err != ERR_OK)
{
// cleanup, for unkown reason
if (_p != NULL)
{
ss->p = NULL;
pbuf_free(_p);
}
ret_err = _err;
}
else if (ss->state == S_ACCEPTED)
{
if (ss->numPort == POLICY_PORT)
{
pbuf_free(_p);
ss->state = S_RECIEVED;
SendAnswer(ss, _tpcb);
ss->state = S_CLOSING;
ret_err = ERR_OK;
}
else
{
// first data chunk in _p->payload
ss->state = S_RECIEVED;
// store reference to incoming pbuf (chain)
ss->p = _p;
Send(_tpcb, ss);
ret_err = ERR_OK;
}
}
else if (ss->state == S_RECIEVED)
{
// read some more data
if (ss->p == NULL)
{
//ss->p = _p;
//tcp_sent(_tpcb, CallbackOnSent);
//Send(_tpcb, ss);
SocketFuncReciever((char*)_p->payload, _p->len);
u8_t freed = 0;
do
{
// try hard to free pbuf
freed = pbuf_free(_p);
} while (freed == 0);
}
else
{
struct pbuf *ptr;
// chain pbufs to the end of what we recv'ed previously
ptr = ss->p;
pbuf_chain(ptr, _p);
}
ret_err = ERR_OK;
}
else if (ss->state == S_CLOSING)
{
// odd case, remote side closing twice, trash data
tcp_recved(_tpcb, _p->tot_len);
ss->p = NULL;
pbuf_free(_p);
ret_err = ERR_OK;
}
else
{
// unknown ss->state, trash data
tcp_recved(_tpcb, _p->tot_len);
ss->p = NULL;
pbuf_free(_p);
ret_err = ERR_OK;
}
return ret_err;
}
