uint8_t NMX::sendCommand(uint8_t command, uint8_t dataLength, uint8_t *data)
{
if(!remote.nmx) return 0;
char hexbuf[3];
bt.sendCMD(PSTR("ATGW,0,28,0,0000000000FF"));
uint8tohex(hexbuf, nodeAddress);
bt.sendCMD((char*)hexbuf);
uint8tohex(hexbuf, motorAddress);
bt.sendCMD((char*)hexbuf);
uint8tohex(hexbuf, command);
bt.sendCMD((char*)hexbuf);
uint8tohex(hexbuf, dataLength);
bt.sendCMD((char*)hexbuf);
for(uint8_t i = 0; i < dataLength; i++)
{
uint8tohex(hexbuf, data[i]);
bt.sendCMD((char*)hexbuf);
}
bt.sendCMD(PSTR("\r"));
return bt.checkOK();
}
// returns data length in bytes (max 4)
uint32_t NMX::sendQuery(uint8_t command)
{
uint8_t b;
uint32_t value = 0;
if(sendCommand(command, 0, 0))
{
bt.sendCMD(PSTR("ATGR,0,24\r"));
if(bt.checkOK())
{
char *buf;
if(bt.waitEvent(STR("GATT_VAL"), &buf))
{
DEBUG(STR("GATT RESPONSE: "));
DEBUG(buf);
if(strncmp(buf, STR("GATT_VAL"), 8) == 0 && strlen(buf) >= 21)
{
uint8_t len;
if(hextouint8(&len, &buf[22]))
{
if(len <= 4)
{
for(uint8_t i = 0; i < len; i++)
{
hextouint8(&b, &buf[24 + i * 2]);
value += ((uint32_t)b << (8 * (len - i - 1)));
}
return value;
}
}
}
return 0;
}
}
}
return 0;
}
