static uint8_t PrintStatus(const CLS1_StdIOType *io) {
uint8_t buf[64], buf2[16];
QuadTime_t timing;
int i;
CLS1_SendStatusStr((unsigned char*)"quadcalib", (unsigned char*)"\r\n", io->stdOut);
for(i=0; i<NOF_SIGNALS; i++) {
if (Measure(i, &timing)==ERR_OK) {
buf[0] = '\0';
UTIL1_strcatNum8u(buf, sizeof(buf), timing.lowPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"% to ");
UTIL1_strcatNum8u(buf, sizeof(buf), timing.highPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"%, ");
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"high: ");
UTIL1_strcatNum32u(buf, sizeof(buf), timing.highTicks);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" ticks, low: ");
UTIL1_strcatNum32u(buf, sizeof(buf), timing.lowTicks);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" ticks\r\n");
} else {
UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"TIMEOUT\r\n");
}
if (i==0) {
UTIL1_strcpy(buf2, sizeof(buf2), (uint8_t*)" Right A,C0");
} else if (i==1) {
UTIL1_strcpy(buf2, sizeof(buf2), (uint8_t*)" Right B,C1");
} else if (i==2) {
UTIL1_strcpy(buf2, sizeof(buf2), (uint8_t*)" Left C,C2");
} else {
UTIL1_strcpy(buf2, sizeof(buf2), (uint8_t*)" Left D,C3");
}
CLS1_SendStatusStr(buf2, buf, io->stdOut);
}
return ERR_OK;
}
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;
}
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 RADIO_PrintStatus(const CLS1_StdIOType *io) {
uint8_t buf[24];
uint8_t val0, val1;
CLS1_SendStatusStr((unsigned char*)"Radio", (unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" state", RadioStateStr(RADIO_AppStatus), io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" sniff", RADIO_isSniffing?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
UTIL1_Num8uToStr(buf, sizeof(buf), RADIO_channel);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" channel", buf, io->stdOut);
(void)RF1_ReadObserveTxRegister(&val0, &val1);
UTIL1_Num8uToStr(buf, sizeof(buf), val0);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" lost, ");
UTIL1_strcatNum8u(buf, sizeof(buf), val1);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" retry\r\n");
CLS1_SendStatusStr((unsigned char*)" OBSERVE_TX", buf, io->stdOut);
#if 0 /* The RPD status will get reset very fast by another (e.g. WLAN) packet. So this is not really a useful feature :-( */
(void)RF1_ReadReceivedPowerDetector(&val0); /*! \todo only works in RX mode, but somehow this still does not work? */
if (val0&1) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"1, > -64 dBm\r\n");
} else {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0, < -64 dBm\r\n");
}
CLS1_SendStatusStr((unsigned char*)" RPD", buf, io->stdOut);
#endif
}
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_CloseConnection(uint8_t channel, const CLS1_StdIOType *io, uint16_t timeoutMs) {
/* AT+CIPCLOSE=<channel> */
uint8_t res;
uint8_t cmd[64];
UTIL1_strcpy(cmd, sizeof(cmd), "AT+CIPCLOSE=");
UTIL1_strcatNum8u(cmd, sizeof(cmd), channel);
UTIL1_strcat(cmd, sizeof(cmd), "\r\n");
res = ESP_SendATCommand(cmd, NULL, 0, "Unlink\r\n", timeoutMs, io);
return res;
}
void RUNNER_GetCmdString(char *buf, size_t bufSize) {
if (RUNNER_delLastRunner) {
UTIL1_strcat(buf, bufSize, "delrec last -y"); /* delete last runner record */
RUNNER_delLastRunner = FALSE; /* reset flag */
ResetRunnerDisplayList(); /* clear display list of runners */
RUNNER_listLastRecords = TRUE; /* request new list of runners to update display */
} else if (RUNNER_listLastRecords) {
UTIL1_strcat(buf, bufSize, "list records last "); /* list last runner records */
UTIL1_strcatNum8u(buf, bufSize, NOF_RUNNER_DISPLAY);
RUNNER_listLastRecords = FALSE; /* reset flag */
}
}
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);
}
static void dump_byte_array(uint8_t *buffer, uint8_t bufferSize, bool inHex) {
uint8_t buf[8];
for (uint8_t i = 0; i < bufferSize; i++) {
UTIL1_strcpy(buf, sizeof(buf), " ");
if (inHex) {
UTIL1_strcatNum8Hex(buf, sizeof(buf), buffer[i]);
} else {
UTIL1_strcatNum8u(buf, sizeof(buf), buffer[i]);
}
CLS1_SendStr(buf, CLS1_GetStdio()->stdOut);
}
}
uint8_t ESP_SetNumberOfConnections(uint8_t nof, const CLS1_StdIOType *io, uint16_t timeoutMs) {
/* AT+CIPMUX=<nof>, 0: single connection, 1: multiple connections */
uint8_t res;
uint8_t cmd[sizeof("AT+CIPMUX=12\r\n")];
uint8_t rxBuf[sizeof("AT+CIPMUX=12\r\n\r\nOK\r\n")+10];
if (nof>1) { /* only 0 and 1 allowed */
if (io!=NULL) {
CLS1_SendStr("Wrong number of connection parameter!\r\n", io->stdErr);
}
return ERR_FAILED;
}
UTIL1_strcpy(cmd, sizeof(cmd), "AT+CIPMUX=");
UTIL1_strcatNum8u(cmd, sizeof(cmd), nof);
UTIL1_strcat(cmd, sizeof(cmd), "\r\n");
res = ESP_SendATCommand(cmd, rxBuf, sizeof(rxBuf), "OK\r\n", timeoutMs, io);
return res;
}
void RAPP_SniffPacket(RPHY_PacketDesc *packet, bool isTx) {
uint8_t buf[32];
const CLS1_StdIOType *io;
int i;
uint8_t dataSize;
RNWK_ShortAddrType addr;
io = CLS1_GetStdio();
if (isTx) {
CLS1_SendStr((unsigned char*)"Packet Tx ", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)"Packet Rx ", io->stdOut);
}
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"flags: ");
UTIL1_strcatNum16s(buf, sizeof(buf), packet->flags);
CLS1_SendStr(buf, io->stdOut);
if (packet->flags!=RPHY_PACKET_FLAGS_NONE) {
CLS1_SendStr((unsigned char*)"(", io->stdOut);
if (packet->flags&RPHY_PACKET_FLAGS_IS_ACK) {
CLS1_SendStr((unsigned char*)"IS_ACK,", io->stdOut);
}
if (packet->flags&RPHY_PACKET_FLAGS_REQ_ACK) {
CLS1_SendStr((unsigned char*)"REQ_ACK", io->stdOut);
}
CLS1_SendStr((unsigned char*)")", io->stdOut);
}
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" size: ");
UTIL1_strcatNum16s(buf, sizeof(buf), packet->phySize);
CLS1_SendStr(buf, io->stdOut);
/* PHY */
CLS1_SendStr((unsigned char*)" PHY data: ", io->stdOut);
dataSize = RPHY_BUF_SIZE(packet->phyData);
for(i=0; i<dataSize+RPHY_HEADER_SIZE;i++) {
buf[0] = '\0';
UTIL1_strcatNum8Hex(buf, sizeof(buf), packet->phyData[i]);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ");
CLS1_SendStr(buf, io->stdOut);
}
/* MAC */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" MAC size:");
UTIL1_strcatNum8u(buf, sizeof(buf), dataSize);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" type:");
UTIL1_strcatNum8Hex(buf, sizeof(buf), RMAC_BUF_TYPE(packet->phyData));
CLS1_SendStr(buf, io->stdOut);
RMAC_DecodeType(buf, sizeof(buf), packet);
CLS1_SendStr(buf, io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" s#:");
UTIL1_strcatNum8Hex(buf, sizeof(buf), RMAC_BUF_SEQN(packet->phyData));
CLS1_SendStr(buf, io->stdOut);
/* NWK */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" NWK src:");
addr = RNWK_BUF_GET_SRC_ADDR(packet->phyData);
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(buf, sizeof(buf), addr);
#else
UTIL1_strcatNum16Hex(buf, sizeof(buf), addr);
#endif
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" dst:");
addr = RNWK_BUF_GET_DST_ADDR(packet->phyData);
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(buf, sizeof(buf), addr);
#else
UTIL1_strcatNum16Hex(buf, sizeof(buf), addr);
#endif
CLS1_SendStr(buf, io->stdOut);
/* APP */
if (dataSize>RMAC_HEADER_SIZE+RNWK_HEADER_SIZE) { /* there is application data */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" APP type:");
UTIL1_strcatNum8Hex(buf, sizeof(buf), RAPP_BUF_TYPE(packet->phyData));
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" size:");
UTIL1_strcatNum8Hex(buf, sizeof(buf), RAPP_BUF_SIZE(packet->phyData));
CLS1_SendStr(buf, 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;
}
