static void StrCatDate(unsigned char *buf, size_t bufSize, uint8_t day, uint8_t month, uint16_t year) {
UTIL1_strcatNum16uFormatted(buf, bufSize, day, '0', 2);
UTIL1_chcat(buf, bufSize, '.');
UTIL1_strcatNum16uFormatted(buf, bufSize, month, '0', 2);
UTIL1_chcat(buf, bufSize, '.');
UTIL1_strcatNum16uFormatted(buf, bufSize, year, '0', 4);
}
static void StrCatTime(unsigned char *buf, size_t bufSize, uint8_t hour, uint8_t minute, uint8_t second) {
UTIL1_strcatNum16uFormatted(buf, bufSize, hour, '0', 2);
UTIL1_chcat(buf, bufSize, ':');
UTIL1_strcatNum16uFormatted(buf, bufSize, minute, '0', 2);
UTIL1_chcat(buf, bufSize, ':');
UTIL1_strcatNum16uFormatted(buf, bufSize, second, '0', 2);
}
void RMAC_DecodeType(uint8_t *buf, size_t bufSize, RPHY_PacketDesc *packet) {
RMAC_MsgType type;
bool first = TRUE;
type = RMAC_BUF_TYPE(packet->phyData);
buf[0] = '\0';
UTIL1_chcat(buf, bufSize, '(');
if (type&RMAC_MSG_TYPE_REQ_ACK) {
UTIL1_strcat(buf, bufSize, (unsigned char*)"RACK");
first = FALSE;
}
if (type&RMAC_MSG_TYPE_DATA) {
if (!first) {
UTIL1_chcat(buf, bufSize, '|');
}
UTIL1_strcat(buf, bufSize, (unsigned char*)"DATA");
first = FALSE;
}
if (type&RMAC_MSG_TYPE_ACK) {
if (!first) {
UTIL1_chcat(buf, bufSize, '|');
}
UTIL1_strcat(buf, bufSize, (unsigned char*)"ACK");
first = FALSE;
}
if (type&RMAC_MSG_TYPE_CMD) {
UTIL1_strcat(buf, bufSize, (unsigned char*)"CMD");
first = FALSE;
}
UTIL1_chcat(buf, bufSize, ')');
}
static uint8_t Tune(const CLS1_StdIOType *io, uint8_t channel, MOT_MotorDevice *motorHandle) {
#define TUNE_MOTOR_PERCENT 20
uint16_t dac;
int i;
QuadTime_t timing;
uint8_t buf[48];
uint8_t res;
//#if PL_HAS_DRIVE
// DRV_SetMode(DRV_MODE_NONE); /* turn off drive mode */
//#endif
MOT_SetSpeedPercent(motorHandle, TUNE_MOTOR_PERCENT);
CLS1_SendStr((uint8_t*)"Tuning channel...\r\n", io->stdOut);
res = ERR_FAILED;
for(i=0,dac=0;dac<=MCP4728_MAX_DAC_VAL;i++) {
UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"Channel: ");
UTIL1_chcat(buf, sizeof(buf), (uint8_t)('A'+channel)); /* 0:A, 1:B, 2:C, 3:D */
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" DAC: 0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), dac);
UTIL1_chcat(buf, sizeof(buf), ' ');
CLS1_SendStr(buf, io->stdOut);
if (MCP4728_FastWriteDAC(channel, dac)!=ERR_OK) { /* writes single channel DAC value, not updating EEPROM */
CLS1_SendStr((uint8_t*)"ERROR writing DAC channel!\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
WAIT1_WaitOSms(100); /* wait some time to allow DAC and OP-Amp change */
if (Measure(channel, &timing)==ERR_OK) {
buf[0] = '\0';
UTIL1_strcatNum8u(buf, sizeof(buf), timing.highPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"% high, low ");
UTIL1_strcatNum8u(buf, sizeof(buf), timing.lowPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"%\r\n");
CLS1_SendStr(buf, io->stdOut);
if (timing.highPercent==50 || timing.lowPercent==50) {
CLS1_SendStr((uint8_t*)"Set!\r\n", io->stdErr);
CLS1_SendStr((uint8_t*)"Writing to EEPROM...\r\n", io->stdOut);
if (MCP4728_WriteDACandEE(channel, dac)!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR writing DAC/EEPROM\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
CLS1_SendStr((uint8_t*)"...done!\r\n", io->stdOut);
res = ERR_OK;
break; /* go to next channel */
}
dac += 0x1; /* smaller increase */
} else {
CLS1_SendStr((uint8_t*)"No signal\r\n", io->stdErr);
dac += 0x10; /* larger increase */
}
} /* for finding DAC value */
MOT_SetSpeedPercent(motorHandle, 0); /* turn off again */
if (res!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR!\r\n", io->stdErr);
}
CLS1_SendStr((uint8_t*)"Tuning finished!\r\n", io->stdOut);
return res;
}
static uint8_t PrintStatus(CLS1_ConstStdIOType *io) {
w5100_config_t config;
uint8_t buf[24];
int i;
CLS1_SendStatusStr((unsigned char*)"w5100", (unsigned char*)"\r\n", io->stdOut);
if (W5100_ReadConfig(&config)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"ReadConfig() failed!\r\n", io->stdErr);
return ERR_FAILED;
}
/* gateway address */
buf[0] = '\0';
for(i=0;i<sizeof(config.gateway);i++) {
UTIL1_strcatNum8u(buf, sizeof(buf), config.gateway[i]);
if (i<sizeof(config.gateway)-1) {
UTIL1_chcat(buf, sizeof(buf), '.');
} else {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
}
}
CLS1_SendStatusStr((unsigned char*)" Gateway", buf, io->stdOut);
/* netmask */
buf[0] = '\0';
for(i=0;i<sizeof(config.netmask);i++) {
UTIL1_strcatNum8u(buf, sizeof(buf), config.netmask[i]);
if (i<sizeof(config.netmask)-1) {
UTIL1_chcat(buf, sizeof(buf), '.');
} else {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
}
}
CLS1_SendStatusStr((unsigned char*)" NetMask", buf, io->stdOut);
/* HW/MAC address */
buf[0] = '\0';
for(i=0;i<sizeof(config.hwaddr);i++) {
UTIL1_strcatNum8Hex(buf, sizeof(buf), config.hwaddr[i]);
if (i<sizeof(config.hwaddr)-1) {
UTIL1_chcat(buf, sizeof(buf), '-');
} else {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
}
}
CLS1_SendStatusStr((unsigned char*)" MAC", buf, io->stdOut);
/* IP address */
buf[0] = '\0';
for(i=0;i<sizeof(config.ipaddr);i++) {
UTIL1_strcatNum8u(buf, sizeof(buf), config.ipaddr[i]);
if (i<sizeof(config.ipaddr)-1) {
UTIL1_chcat(buf, sizeof(buf), ':');
} else {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
}
}
CLS1_SendStatusStr((unsigned char*)" IP", buf, io->stdOut);
return ERR_OK;
}
static byte PrintStatus(CLS1_ConstStdIOType *io) {
uint8_t buf[48];
CLS1_SendStatusStr((unsigned char*)"Hoval", (const unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" Scheduler", mySchedule.isSchedulerOn ? (const unsigned char*)"on\r\n":(const unsigned char*)"off\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" Switch", SW1_GetVal()==0 ? (const unsigned char*)"ON\r\n":(const unsigned char*)"OFF\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" Hoval", isHovalManualMode ? (const unsigned char*)"manual mode on\r\n":(const unsigned char*)"manual mode off\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" Pump", isPumpOn ? (const unsigned char*)"on\r\n":(const unsigned char*)"off\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" Relay 1", isRelay1on ? (const unsigned char*)"on (button)\r\n":(const unsigned char*)"off (pump)\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" Relay 2", isRelay2on ? (const unsigned char*)"on (pump)\r\n":(const unsigned char*)"off (manual mode)\r\n", io->stdOut);
buf[0]='\0';
#if PL_HAS_DATE_SUPPORT
StrCatDate(buf, sizeof(buf), mySchedule.on.date.day, mySchedule.on.date.month, mySchedule.on.date.year);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", ");
#endif
StrCatTime(buf, sizeof(buf), mySchedule.on.time.hour, mySchedule.on.time.min, mySchedule.on.time.sec);
UTIL1_chcat(buf, sizeof(buf), '-');
#if PL_HAS_DATE_SUPPORT
StrCatDate(buf, sizeof(buf), mySchedule.off.date.day, mySchedule.off.date.month, mySchedule.off.date.year);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", ");
#endif
StrCatTime(buf, sizeof(buf), mySchedule.off.time.hour, mySchedule.off.time.min, mySchedule.off.time.sec);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" time on", (unsigned char*)buf, io->stdOut);
return ERR_OK;
}
uint8_t ESP_OpenConnection(int8_t ch_id, bool isTCP, const uint8_t *IPAddrStr, uint16_t port, uint16_t msTimeout, const CLS1_StdIOType *io) {
/* AT+CIPSTART=4,"TCP","184.106.153.149",80 */
uint8_t txBuf[54];
uint8_t rxBuf[72];
uint8_t expected[72];
UTIL1_strcpy(txBuf, sizeof(txBuf), "AT+CIPSTART=");
if (ch_id>=0) {
UTIL1_strcatNum16u(txBuf, sizeof(txBuf), ch_id);
UTIL1_chcat(txBuf, sizeof(txBuf), ',');
}
if (isTCP) {
UTIL1_strcat(txBuf, sizeof(txBuf), "\"TCP\",\"");
} else {
UTIL1_strcat(txBuf, sizeof(txBuf), "\"UDP\",\"");
}
UTIL1_strcat(txBuf, sizeof(txBuf), IPAddrStr);
UTIL1_strcat(txBuf, sizeof(txBuf), "\",");
UTIL1_strcatNum16u(txBuf, sizeof(txBuf), port);
UTIL1_strcpy(expected, sizeof(expected), txBuf);
UTIL1_strcat(expected, sizeof(expected), "\r\r\n\r\nOK\r\nLinked\r\n");
UTIL1_strcat(txBuf, sizeof(txBuf), "\r\n");
return ESP_SendATCommand(txBuf, rxBuf, sizeof(rxBuf), expected, msTimeout, io);
}
uint8_t RNETA_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
uint8_t res = ERR_OK;
const uint8_t *p;
uint16_t val16;
if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"app help")==0) {
PrintHelp(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"app status")==0) {
*handled = TRUE;
return PrintStatus(io);
} else if (UTIL1_strncmp((char*)cmd, (char*)"app saddr", sizeof("app saddr")-1)==0) {
p = cmd + sizeof("app saddr")-1;
*handled = TRUE;
if (UTIL1_ScanHex16uNumber(&p, &val16)==ERR_OK) {
(void)RNWK_SetThisNodeAddr((RNWK_ShortAddrType)val16);
} else {
CLS1_SendStr((unsigned char*)"ERR: wrong address\r\n", io->stdErr);
return ERR_FAILED;
}
} else if (UTIL1_strncmp((char*)cmd, (char*)"app daddr", sizeof("app daddr")-1)==0) {
p = cmd + sizeof("app daddr")-1;
*handled = TRUE;
if (UTIL1_ScanHex16uNumber(&p, &val16)==ERR_OK) {
APP_dstAddr = val16;
} else {
CLS1_SendStr((unsigned char*)"ERR: wrong address\r\n", io->stdErr);
return ERR_FAILED;
}
#if PL_HAS_RSTDIO
} else if (UTIL1_strncmp((char*)cmd, (char*)"app send", sizeof("app send")-1)==0) {
unsigned char buf[32];
RSTDIO_QueueType queue;
if (UTIL1_strncmp((char*)cmd, (char*)"app send in", sizeof("app send in")-1)==0) {
queue = RSTDIO_QUEUE_TX_IN;
cmd += sizeof("app send in");
} else if (UTIL1_strncmp((char*)cmd, (char*)"app send out", sizeof("app send out")-1)==0) {
queue = RSTDIO_QUEUE_TX_OUT;
cmd += sizeof("app send out");
} else if (UTIL1_strncmp((char*)cmd, (char*)"app send err", sizeof("app send err")-1)==0) {
queue = RSTDIO_QUEUE_TX_ERR;
cmd += sizeof("app send err");
} else {
return ERR_OK; /* not handled */
}
UTIL1_strcpy(buf, sizeof(buf), cmd);
UTIL1_chcat(buf, sizeof(buf), '\n');
buf[sizeof(buf)-2] = '\n'; /* have a '\n' in any case */
if (RSTDIO_SendToTxStdio(queue, buf, UTIL1_strlen((char*)buf))!=ERR_OK) {
CLS1_SendStr((unsigned char*)"failed!\r\n", io->stdErr);
}
*handled = TRUE;
#endif
}
return res;
}
void LogToFile(DataBuffer* myBuf,char annotation, char* fileName) {
int16_t currentBuf = myBuf->currentReadBuffer;
//uint8_t write_buf[48];
UINT bw;
/* open file */
if (FAT1_open(&fp, fileName, FA_OPEN_ALWAYS|FA_WRITE)!=FR_OK) {
Err(/*OPEN_FILE_ERROR*/);
}
/* move to the end of the file */
if (FAT1_lseek(&fp, fp.fsize) != FR_OK || fp.fptr != fp.fsize) {
Err(/*END_OF_FILE_ERROR*/);
}
/* write data */
write_buf[0] = '\0';
//time_t run_time;
for(int cur_buf_index=0;cur_buf_index<BUFFER_SIZE;++cur_buf_index)
{
/** Time Components */
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), myBuf->dataBuffer[currentBuf][cur_buf_index].time.hour);
UTIL1_chcat(write_buf, sizeof(write_buf), ':');
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), myBuf->dataBuffer[currentBuf][cur_buf_index].time.minute);
UTIL1_chcat(write_buf, sizeof(write_buf), ':');
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), myBuf->dataBuffer[currentBuf][cur_buf_index].time.second);
UTIL1_chcat(write_buf, sizeof(write_buf), ':');
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), myBuf->dataBuffer[currentBuf][cur_buf_index].time.milliBig);
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), myBuf->dataBuffer[currentBuf][cur_buf_index].time.milliSmall);
UTIL1_chcat(write_buf, sizeof(write_buf), ',');
;
/** Accelerometer Components */
UTIL1_strcatNum16s(write_buf, sizeof(write_buf), myBuf->dataBuffer[currentBuf][cur_buf_index].x);
UTIL1_chcat(write_buf, sizeof(write_buf), ',');
UTIL1_strcatNum16s(write_buf, sizeof(write_buf), myBuf->dataBuffer[currentBuf][cur_buf_index].y);
UTIL1_chcat(write_buf, sizeof(write_buf), ',');
UTIL1_strcatNum16s(write_buf, sizeof(write_buf), myBuf->dataBuffer[currentBuf][cur_buf_index].z);
UTIL1_chcat(write_buf, sizeof(write_buf), ',');
/** Annotation mode **/
UTIL1_chcat(write_buf, sizeof(write_buf), annotation);
UTIL1_strcat(write_buf, sizeof(write_buf), (unsigned char*)"\n");
/** Write to file **/
if (FAT1_write(&fp, write_buf, UTIL1_strlen((char*)write_buf), &bw)!=FR_OK) {
(void)FAT1_close(&fp);
Err(/*WRITE_ERROR*/);
}
memset(write_buf,'\0',48);
}
if(myBuf->currentReadBuffer == currentBuf)
myBuf->currentReadBuffer=-1;
/* closing file */
(void)FAT1_close(&fp);
}
static void AddToDisplayText(char_t *buf, size_t bufSize, RunnerMsg *runner) {
UTIL1_chcat(buf, bufSize, '[');
UTIL1_strcatNum16uFormatted(buf, bufSize, runner->index, '0', 3);
UTIL1_strcat(buf, bufSize, "] ");
strcatTimeFormatted(buf, bufSize,
runner->runTime.hour,
runner->runTime.minute,
runner->runTime.second,
runner->runTime.hsecond/10);
if (runner->nr != 0) {
UTIL1_chcat(buf, bufSize, ' ');
UTIL1_strcatNum16uFormatted(buf, bufSize, runner->nr, '0', 3);
if (runner->name[0]!='\0') {
UTIL1_chcat(buf, bufSize, ' ');
UTIL1_strcat(buf, bufSize, runner->name);
}
}
UTIL1_chcat(buf, bufSize, '\n');
}
static uint8_t Test(CLS1_ConstStdIOTypePtr io) {
static FIL fp;
UINT bw;
uint8_t read_buf[16];
uint8_t write_buf[10];
uint8_t i;
if (FAT1_isWriteProtected()) {
CLS1_SendStr((unsigned char*)"disk is write protected!\r\n", io->stdErr);
return ERR_FAILED;
}
/* write file */
CLS1_SendStr((const unsigned char*)"Creating test.txt...\r\n", io->stdOut);
if (FAT1_open(&fp, "./test.txt", FA_CREATE_ALWAYS|FA_WRITE)!=FR_OK) {
CLS1_SendStr((const unsigned char*)"*** Failed creating file!\r\n", io->stdErr);
return ERR_FAILED;
}
/* write text */
if (FAT1_write(&fp, "Hello world ", sizeof("Hello world ")-1, &bw)!=FR_OK) {
CLS1_SendStr((const unsigned char*)"*** Failed writing string!\r\n", io->stdErr);
(void)FAT1_close(&fp);
return ERR_FAILED;
}
write_buf[0] = '\0';
for(i=0;i<4;i++) {
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), i);
UTIL1_chcat(write_buf, sizeof(write_buf), ' ');
}
if (FAT1_write(&fp, write_buf, UTIL1_strlen((char*)write_buf), &bw)!=FR_OK) {
CLS1_SendStr((const unsigned char*)"*** Failed writing string!\r\n", io->stdErr);
(void)FAT1_close(&fp);
return ERR_FAILED;
}
/* closing file */
(void)FAT1_close(&fp);
/* read from file */
CLS1_SendStr((const unsigned char*)"Read from file...\r\n", io->stdOut);
if (FAT1_open(&fp, "./test.txt", FA_READ)!=FR_OK) {
CLS1_SendStr((const unsigned char*)"*** Failed opening file!\r\n", io->stdErr);
return ERR_FAILED;
}
if (FAT1_read(&fp, &read_buf[0], sizeof(read_buf)-2, &bw)!=FR_OK) {
CLS1_SendStr((const unsigned char*)"*** Failed reading file!\r\n", io->stdErr);
(void)FAT1_close(&fp);
return ERR_FAILED;
}
read_buf[sizeof(read_buf)-2] = '\0'; /* terminate string */
UTIL1_strcat(read_buf, sizeof(read_buf), (unsigned char*)"\r\n");
CLS1_SendStr(read_buf, io->stdOut);
CLS1_SendStr((const unsigned char*)"\r\n", io->stdOut);
/* close file */
(void)FAT1_close(&fp);
return ERR_OK;
}
uint8_t ESP_PrepareMsgSend(int8_t ch_id, size_t msgSize, uint16_t msTimeout, const CLS1_StdIOType *io) {
/* AT+CIPSEND=<ch_id>,<size> */
uint8_t cmd[24], rxBuf[48], expected[48];
UTIL1_strcpy(cmd, sizeof(cmd), "AT+CIPSEND="); /* parameters are <ch_id>,<size> */
UTIL1_strcatNum8u(cmd, sizeof(cmd), ch_id);
UTIL1_chcat(cmd, sizeof(cmd), ',');
UTIL1_strcatNum16u(cmd, sizeof(cmd), msgSize);
UTIL1_strcpy(expected, sizeof(expected), cmd); /* we expect the echo of our command */
UTIL1_strcat(expected, sizeof(expected), "\r\r\n> "); /* expect "> " */
UTIL1_strcat(cmd, sizeof(cmd), "\r\n");
return ESP_SendATCommand(cmd, rxBuf, sizeof(rxBuf), expected, msTimeout, io);
}
static void LogToFile(int16_t x, int16_t y, int16_t z) {
uint8_t write_buf[48];
UINT bw;
TIMEREC time;
/* open file */
if (FAT1_open(&fp, "./log.txt", FA_OPEN_ALWAYS|FA_WRITE)!=FR_OK) {
Err();
}
/* move to the end of the file */
if (FAT1_lseek(&fp, fp.fsize) != FR_OK || fp.fptr != fp.fsize) {
Err();
}
/* get time */
if (TmDt1_GetTime(&time)!=ERR_OK) {
Err();
}
/* write data */
write_buf[0] = '\0';
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), time.Hour);
UTIL1_chcat(write_buf, sizeof(write_buf), ':');
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), time.Min);
UTIL1_chcat(write_buf, sizeof(write_buf), ':');
UTIL1_strcatNum8u(write_buf, sizeof(write_buf), time.Sec);
UTIL1_chcat(write_buf, sizeof(write_buf), '\t');
UTIL1_strcatNum16s(write_buf, sizeof(write_buf), x);
UTIL1_chcat(write_buf, sizeof(write_buf), '\t');
UTIL1_strcatNum16s(write_buf, sizeof(write_buf), y);
UTIL1_chcat(write_buf, sizeof(write_buf), '\t');
UTIL1_strcatNum16s(write_buf, sizeof(write_buf), z);
UTIL1_strcat(write_buf, sizeof(write_buf), (unsigned char*)"\r\n");
if (FAT1_write(&fp, write_buf, UTIL1_strlen((char*)write_buf), &bw)!=FR_OK) {
(void)FAT1_close(&fp);
Err();
}
/* closing file */
(void)FAT1_close(&fp);
}
/*!
\brief This routine is called as callback by the radio driver on reception of a data packet
\param msg Pointer to the message we received.
*/
static void RADIO_HandleMessage(uint8_t *msg) {
char buf[32];
uint8_t i, size;
if (RADIO_isSniffing && *msg==RADIO_QUEUE_MSG_SNIFF) {
msg++;
size = *msg++;
UTIL1_strcpy(buf, sizeof(buf), "\r\nch #:"); /* use new line at the beginning, as the hex dump at the end might be fill up buffer completely */
UTIL1_strcatNum16s(buf, sizeof(buf), RADIO_Channel);
UTIL1_strcat(buf, sizeof(buf), " size:");
UTIL1_strcatNum16s(buf, sizeof(buf), size);
UTIL1_strcat(buf, sizeof(buf), " ASCII: ");
SHELL_SendMessage(buf);
buf[0] = '\0';
/* write as string */
for(i=0;i<size && i<sizeof(buf);i++) {
UTIL1_chcat(buf, sizeof(buf), msg[i]);
}
SHELL_SendMessage(buf);
/* write as hex */
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), " hex: ");
for(i=0; i<size;i++) {
UTIL1_strcatNum8Hex(buf, sizeof(buf), msg[i]);
UTIL1_strcat(buf, sizeof(buf), " ");
}
SHELL_SendMessage(buf);
SHELL_SendMessage("\r\n");
#if PL_HAS_REMOTE && PL_HAS_MOTOR
/*! \todo Implement handling for your remote control */
} else if (*msg==RADIO_QUEUE_MSG_ACCEL) {
msg++;
size = *msg++;
REMOTE_ParseMsg((const unsigned char*)msg+sizeof(RADIO_PREFIX_STR)-1, size-sizeof(RADIO_PREFIX_STR)-1);
#endif
}
}
static uint8_t PrintStatus(const CLS1_StdIOType *io) {
int i;
uint8_t buf[16], buf2[32];
CLS1_SendStatusStr((uint8_t*)"Floppy", (uint8_t*)"\r\n", io->stdOut);
UTIL1_Num32sToStr(buf, sizeof(buf), FLOPPY_NoteOffset);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"\r\n");
CLS1_SendStatusStr((uint8_t*)" note offset", buf, io->stdOut);
for(i=0;i<FLOPPY_NOF_DRIVES;i++) {
UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)" drive ");
UTIL1_strcatNum16u(buf, sizeof(buf), (uint16_t)i);
UTIL1_chcat(buf, sizeof(buf), ':');
buf2[0] = '\0';
if (FLOPPY_Drives[i].forward) {
UTIL1_strcat(buf2, sizeof(buf2), (uint8_t*)"fw ");
} else {
UTIL1_strcat(buf2, sizeof(buf2), (uint8_t*)"bw ");
}
UTIL1_strcatNum16s(buf2, sizeof(buf2), FLOPPY_Drives[i].pos);
UTIL1_strcat(buf2, sizeof(buf2), (uint8_t*)"\r\n");
CLS1_SendStatusStr(buf, buf2, io->stdOut);
}
return ERR_OK;
}
static void Log(unsigned char *msg, const CLS1_StdIOType *io) {
RTC1_TTIME currTime;
RTC1_TDATE currDate;
unsigned char buf[32];
if (RTC1_GetRTCDate(&currDate)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"*** Failed to get RTC date\r\n", io->stdErr);
} else {
buf[0] = '\0';
StrCatDate(buf, sizeof(buf), currDate.day, currDate.month, currDate.year+2000);
CLS1_SendStr(buf, io->stdOut);
}
if (RTC1_GetRTCTime(&currTime)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"*** Failed to get RTC time\r\n", io->stdErr);
} else {
buf[0] = '\0';
UTIL1_chcat(buf, sizeof(buf), ' ');
StrCatTime(buf, sizeof(buf), currTime.hour, currTime.min, currTime.sec);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)": ");
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr(msg, io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
}
static uint8_t Tune(const CLS1_StdIOType *io) {
uint8_t channel;
uint16_t dac;
int i;
QuadTime_t timing;
uint8_t buf[32];
uint8_t res;
uint16_t dacArr[4];
uint8_t pd[4];
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 10);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 10);
for(i=0;i<4;i++) { /* init */
dacArr[i] = 0;
pd[i] = 0;
}
CLS1_SendStr((uint8_t*)"Tuning....\r\n", io->stdOut);
res = ERR_OK;
for(channel=0;channel<NOF_SIGNALS;channel++) {
for(i=0,dac=0;i<80 && dac<MPC4728_MAX_DAC_VAL;i++,dac += 0x10) {
UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"Channel: ");
UTIL1_strcatNum8u(buf, sizeof(buf), channel);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" DAC: 0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), dac);
UTIL1_chcat(buf, sizeof(buf), ' ');
CLS1_SendStr(buf, io->stdOut);
dacArr[channel] = dac;
if (MPC4728_FastWriteDAC(dacArr, sizeof(dacArr), pd, sizeof(pd))!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR writing DAC/EE\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
WAIT1_Waitms(50); /* wait some time to allow change */
if (Measure(channel, &timing)==ERR_OK) {
buf[0] = '\0';
UTIL1_strcatNum8u(buf, sizeof(buf), timing.highPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"% high, low ");
UTIL1_strcatNum8u(buf, sizeof(buf), timing.lowPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"%\r\n");
CLS1_SendStr(buf, io->stdOut);
if (timing.highPercent==50 || timing.lowPercent==50) {
CLS1_SendStr((uint8_t*)"Set!\r\n", io->stdErr);
break;
}
} else {
CLS1_SendStr((uint8_t*)"No signal\r\n", io->stdErr);
}
} /* for */
if (res!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR!\r\n", io->stdErr);
break;
}
} /* for */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
CLS1_SendStr((uint8_t*)"Writing to EEPROM...\r\n", io->stdOut);
for (channel=0;channel<NOF_SIGNALS;channel++) {
if (MPC4728_SingleWriteDACandEE(channel, dacArr[channel])!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR writing DAC/EE\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
WAIT1_Waitms(500); /* give EEPROM time to write data */
}
CLS1_SendStr((uint8_t*)"finished!\r\n", io->stdOut);
return res;
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-5 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button, toggle remote*/
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500");
DRV_SetMode(DRV_MODE_SPEED);
} else if (val=='G') { /* center joystick button: horn*/
SHELL_ParseCmd((unsigned char*)"buzzer buz 2000 300");
} else if (val=='A')
{
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(STRAIGHT_HAND);
LF_StartFollowing();
} else if (val=='C') { /* 'C' button: motor stop*/
DRV_SetMode(DRV_MODE_STOP);
} else if (val=='B') { /* 'B' button: start right-hand algorithm */
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(RIGHT_HAND);
LF_StartFollowing();
} else if (val=='D') { /* 'D' button: start left-hand algorithm */
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(LEFT_HAND);
LF_StartFollowing();
} else if (val=='E') {
REF_CalibrateStartStop();
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t direction, speed;
int16_t x1000, y1000;
*handled = TRUE;
direction = *data; /* get x data value */
speed = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"d/s: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)direction);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)speed);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
//x1000 = scaleJoystickTo1K(x);
//y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(direction, speed, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button, disable remote */
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500");
REMOTE_SetOnOff(FALSE);
DRV_SetSpeed(0,0); /* turn off motors */
SHELL_SendString("Remote OFF\r\n");
} else if (val=='G') { /* center joystick button: enable remote */
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 1000");
REMOTE_SetOnOff(TRUE);
DRV_SetMode(DRV_MODE_SPEED);
SHELL_SendString("Remote ON\r\n");
} else if (val=='C') { /* red 'C' button */
/*! \todo add functionality */
} else if (val=='A') { /* green 'A' button */
/*! \todo add functionality */
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-15 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button - disable remote, drive mode none */
DRV_SetMode(DRV_MODE_NONE);
REMOTE_SetOnOff(FALSE);
SHELL_SendString("Remote OFF\r\n");
} else if (val=='G') { /* center joystick button: enable remote */
REMOTE_SetOnOff(TRUE);
#if PL_CONFIG_HAS_DRIVE
DRV_SetMode(DRV_MODE_SPEED);
#endif
SHELL_SendString("Remote ON\r\n");
}
#if PL_CONFIG_HAS_LINE_MAZE
else if (val=='A') { /* green 'A' button */
SHELL_SendString("Button A pressed\r\n");
// Start Maze solving
if(!LF_IsFollowing()){
LF_StartFollowing();
}
} else if (val=='K') { /* green A button longpress -> 'K' */
SHELL_SendString("Clear Maze\r\n");
// Clear old maze solution, ready for restart
MAZE_ClearSolution();
} else if (val =='B'){ /* yellow 'B' button */
SHELL_SendString("Right hand rule!\r\n");
LF_SetRule(FALSE);
} else if (val=='E') { /* button 'E' pressed */
SHELL_SendString("Stop Following! \r\n");
if(LF_IsFollowing()){
LF_StopFollowing();
}
} else if (val=='D') { /* blue 'D' button */
SHELL_SendString("Left hand rule!\r\n");
LF_SetRule(TRUE);
}
#endif
else if (val=='C') { /* red 'C' button */
NITRO = TRUE;
SHELL_SendString("Nitrooooooo!!!\r\n");
BUZ_Beep(1000,1000);
} else if (val=='J') { /* button 'C' released */
NITRO = FALSE;
SHELL_SendString("Stop Nitro \r\n");
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
uint8_t RNETA_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
uint8_t res = ERR_OK;
const uint8_t *p;
uint16_t val16;
uint8_t val8;
if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"rapp help")==0) {
PrintHelp(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"rapp status")==0) {
*handled = TRUE;
return PrintStatus(io);
} else if (UTIL1_strncmp((char*)cmd, (char*)"rapp saddr", sizeof("rapp saddr")-1)==0) {
p = cmd + sizeof("rapp saddr")-1;
*handled = TRUE;
if (UTIL1_ScanHex16uNumber(&p, &val16)==ERR_OK) {
(void)RNWK_SetThisNodeAddr((RNWK_ShortAddrType)val16);
} else {
CLS1_SendStr((unsigned char*)"ERR: wrong address\r\n", io->stdErr);
return ERR_FAILED;
}
} else if (UTIL1_strncmp((char*)cmd, (char*)"rapp send val", sizeof("rapp send val")-1)==0) {
p = cmd + sizeof("rapp send val")-1;
*handled = TRUE;
if (UTIL1_ScanDecimal8uNumber(&p, &val8)==ERR_OK) {
(void)RAPP_SendPayloadDataBlock(&val8, sizeof(val8), (uint8_t)RAPP_MSG_TYPE_DATA, APP_dstAddr, RPHY_PACKET_FLAGS_NONE); /* only send low byte */
} else {
CLS1_SendStr((unsigned char*)"ERR: wrong number format\r\n", io->stdErr);
return ERR_FAILED;
}
} else if (UTIL1_strncmp((char*)cmd, (char*)"rapp daddr", sizeof("rapp daddr")-1)==0) {
p = cmd + sizeof("rapp daddr")-1;
*handled = TRUE;
if (UTIL1_ScanHex16uNumber(&p, &val16)==ERR_OK) {
APP_dstAddr = val16;
} else {
CLS1_SendStr((unsigned char*)"ERR: wrong address\r\n", io->stdErr);
return ERR_FAILED;
}
#if RNET_CONFIG_REMOTE_STDIO
} else if (UTIL1_strncmp((char*)cmd, (char*)"rapp send", sizeof("rapp send")-1)==0) {
unsigned char buf[32];
RSTDIO_QueueType queue;
if (UTIL1_strncmp((char*)cmd, (char*)"rapp send in", sizeof("rapp send in")-1)==0) {
queue = RSTDIO_QUEUE_TX_IN;
cmd += sizeof("rapp send in");
} else if (UTIL1_strncmp((char*)cmd, (char*)"rapp send out", sizeof("rapp send out")-1)==0) {
queue = RSTDIO_QUEUE_TX_OUT;
cmd += sizeof("rapp send out");
} else if (UTIL1_strncmp((char*)cmd, (char*)"rapp send err", sizeof("rapp send err")-1)==0) {
queue = RSTDIO_QUEUE_TX_ERR;
cmd += sizeof("rapp send err");
} else {
return ERR_OK; /* not handled */
}
UTIL1_strcpy(buf, sizeof(buf), cmd);
UTIL1_chcat(buf, sizeof(buf), '\n');
buf[sizeof(buf)-2] = '\n'; /* have a '\n' in any case */
if (RSTDIO_SendToTxStdio(queue, buf, UTIL1_strlen((char*)buf))!=ERR_OK) {
CLS1_SendStr((unsigned char*)"failed!\r\n", io->stdErr);
}
*handled = TRUE;
#endif
}
return res;
}
static portTASK_FUNCTION(TraceTask, pvParameters) {
unsigned char buf[32];
(void)pvParameters;
//ACCEL_LowLevelInit(); /* cannot do this in ACCEL_Init(), as there interrupts are disabled */
for(;;) {
if (traceChannel==TRACE_TO_NONE) {
/* do nothing */
} else if (traceChannel==TRACE_TO_SHELL) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" => ");
#if PL_HAS_MOTOR
if (traceMotor) {
buf[0] = '\0';
if (MOT_GetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT))==MOT_DIR_FORWARD) {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: fw 0x");
} else {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: bw 0x");
}
UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT)));
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_MOTOR_TACHO
if (traceTacho) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" V:");
UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(FALSE), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ' ');
UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(TRUE), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_QUADRATURE
if (traceQuad) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" P:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_GetPos(), ' ', 6);
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_GetPos(), ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; E:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_NofErrors(), ' ', 6);
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_NofErrors(), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_ACCEL
if (traceAccel) {
int16_t x, y, z;
buf[0] = '\0';
ACCEL_GetValues(&x, &y, &z);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" X:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), x, ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Y:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), y, ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Z:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), z, ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"\r\n");
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
FRTOS1_vTaskDelay(20/portTICK_RATE_MS); /* slow down writing to console */
}
FRTOS1_vTaskDelay(20/portTICK_RATE_MS);
} /* for */
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
KEY_DATA val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-5 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
switch(val) {
case A_p:
SHELL_ParseCmd((unsigned char*)"buzzer play tune");
break;
case A_lp:
nitro = TRUE;
break;
case A_r:
nitro = FALSE;
break;
case B_p:
SHELL_ParseCmd((unsigned char*)"line start");
break;
case B_lp:
break;
case B_r:
break;
case C_p:
break;
case C_lp:
break;
case C_r:
break;
case D_p:
SHELL_ParseCmd((unsigned char*)"line stop");
SHELL_ParseCmd((unsigned char*)"drive mode speed");
break;
case D_lp:
break;
case D_r:
break;
case E_p:
SHELL_ParseCmd((unsigned char*)"line righthand");
break;
case E_lp:
break;
case E_r:
break;
case F_p:
SHELL_ParseCmd((unsigned char*)"line lefthand");
break;
case F_lp:
break;
case F_r:
break;
case G_p:
break;
case G_lp:
break;
case G_r:
break;
default:
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 1000");
break;
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
