int _write(int file, char *ptr, int len) {
int size;
#if 0
/* In case of the file is written in one piece (newlib), you can dump it now.
* But if using newlib-nano, it writes to the file byte by byte, so dumping is postponed
*/
/* construct gdb command string to write gcda file */
UTIL1_strcpy(gdb_cmd, sizeof(gdb_cmd), (unsigned char*)"dump binary memory ");
UTIL1_strcat(gdb_cmd, sizeof(gdb_cmd), COV_Buffer.fileName);
UTIL1_strcat(gdb_cmd, sizeof(gdb_cmd), (unsigned char*)" 0x");
UTIL1_strcatNum32Hex(gdb_cmd, sizeof(gdb_cmd), (uint32_t)ptr);
UTIL1_strcat(gdb_cmd, sizeof(gdb_cmd), (unsigned char*)" 0x");
UTIL1_strcatNum32Hex(gdb_cmd, sizeof(gdb_cmd), (uint32_t)(ptr+len));
#endif
if (file==COV_FILE_HANDLE) {
size = len;
while(size>0) {
if (COV_Buffer.bufPtr>=&COV_Buffer.buffer[COV_FILE_BUFFER_SIZE]) {
/* buffer overflow! */
for(;;);
}
*COV_Buffer.bufPtr++ = *ptr++;
size--;
}
COV_Buffer.fileSize = COV_Buffer.bufPtr-&COV_Buffer.buffer[0];
}
return len; /* on success, return number of bytes written */
}
int _close(int file) {
if (file==COV_FILE_HANDLE) {
/* construct gdb command string to write .gcda file */
UTIL1_strcpy(gdb_cmd, sizeof(gdb_cmd), (unsigned char*)"dump binary memory ");
UTIL1_strcat(gdb_cmd, sizeof(gdb_cmd), COV_Buffer.fileName);
UTIL1_strcat(gdb_cmd, sizeof(gdb_cmd), (unsigned char*)" 0x");
UTIL1_strcatNum32Hex(gdb_cmd, sizeof(gdb_cmd), (uint32_t)&COV_Buffer.buffer[0]);
UTIL1_strcat(gdb_cmd, sizeof(gdb_cmd), (unsigned char*)" 0x");
UTIL1_strcatNum32Hex(gdb_cmd, sizeof(gdb_cmd), (uint32_t)(&COV_Buffer.buffer[COV_Buffer.fileSize]));
}
return 0; /* success closing file */
}
int _write(int file, char *ptr, int len) {
static unsigned char gdb_cmd[128]; /* command line which can be used for gdb */
(void)file;
/* construct gdb command string */
UTIL1_strcpy(gdb_cmd, sizeof(gdb_cmd), (unsigned char*)"dump binary memory ");
UTIL1_strcat(gdb_cmd, sizeof(gdb_cmd), fileName);
UTIL1_strcat(gdb_cmd, sizeof(gdb_cmd), (unsigned char*)" 0x");
UTIL1_strcatNum32Hex(gdb_cmd, sizeof(gdb_cmd), (uint32_t)ptr);
UTIL1_strcat(gdb_cmd, sizeof(gdb_cmd), (unsigned char*)" 0x");
UTIL1_strcatNum32Hex(gdb_cmd, sizeof(gdb_cmd), (uint32_t)(ptr+len));
return 0;
}
/*
** ===================================================================
** Method : PTRC1_vGetGDBDumpCommand (component PercepioTrace)
** Description :
** Gets the gdb command to dump the trace data to a file.
** Useful for copy-pasting it to the gdb console.
** Parameters :
** NAME - DESCRIPTION
** * buf - Pointer to buffer for the command. Make it
** large enoug (about 64 bytes)
** bufSize - size of the buffer
** * fileName - Pointer to the file name string,
** e.g. "C:\\tmp\\trace.dump"
** Returns : Nothing
** ===================================================================
*/
void PTRC1_vGetGDBDumpCommand(uint8_t *buf, uint16_t bufSize, uint8_t *fileName)
{
#if TRC_CONFIG_USE_RTT_STREAMING
/* with RTT streaming, there is no trace buffer */
*buf = '\0';
#else
/* construct gdb command string: dump binary memory <file> <hexStartAddr> <hexEndAddr> */
uint8_t *ptr; /* pointer to data */
size_t len; /* size/length of data */
ptr = (uint8_t*)PTRC1_vTraceGetTraceBuffer();
len = PTRC1_uiTraceGetTraceBufferSize();
UTIL1_strcpy(buf, bufSize, (unsigned char*)"dump binary memory ");
UTIL1_strcat(buf, bufSize, fileName);
UTIL1_strcat(buf, bufSize, (unsigned char*)" 0x");
UTIL1_strcatNum32Hex(buf, bufSize, (uint32_t)ptr);
UTIL1_strcat(buf, bufSize, (unsigned char*)" 0x");
UTIL1_strcatNum32Hex(buf, bufSize, (uint32_t)(ptr+len));
#endif
}
static void BL_PrintStatus(CLS1_ConstStdIOType *io) {
unsigned char buf[32];
CLS1_SendStatusStr((unsigned char*)"BL", (const unsigned char*)"\r\n", io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
UTIL1_strcatNum32Hex(buf, sizeof(buf), MIN_APP_FLASH_ADDRESS);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"..0x");
UTIL1_strcatNum32Hex(buf, sizeof(buf), MAX_APP_FLASH_ADDRESS);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" App Flash", buf, io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" @0x");
UTIL1_strcatNum32Hex(buf, sizeof(buf), MIN_APP_FLASH_ADDRESS);
CLS1_SendStatusStr(buf, (unsigned char*)"", io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
UTIL1_strcatNum32Hex(buf, sizeof(buf), *((uint32_t*)MIN_APP_FLASH_ADDRESS));
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" 0x");
UTIL1_strcatNum32Hex(buf, sizeof(buf), *((uint32_t*)MIN_APP_FLASH_ADDRESS+4));
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStr(buf, io->stdOut);
}
static uint8_t BL_LoadS19(CLS1_ConstStdIOType *io) {
unsigned char buf[16];
uint8_t res = ERR_OK;
/* first, erase flash */
if (BL_EraseAppFlash(io)!=ERR_OK) {
return ERR_FAILED;
}
/* load S19 file */
CLS1_SendStr((unsigned char*)"Waiting for the S19 file...", io->stdOut);
parserInfo.GetCharIterator = GetChar;
parserInfo.voidP = (void*)io;
parserInfo.S19Flash = BL_onS19Flash;
parserInfo.status = S19_FILE_STATUS_NOT_STARTED;
parserInfo.currType = 0;
parserInfo.currAddress = 0;
parserInfo.codeSize = 0;
parserInfo.codeBuf = codeBuf;
parserInfo.codeBufSize = sizeof(codeBuf);
while (AS1_GetCharsInRxBuf()>0) { /* clear any pending characters in rx buffer */
AS1_ClearRxBuf();
WAIT1_Waitms(100);
}
do {
if (S19_ParseLine(&parserInfo)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"ERROR!\r\nFailed at address 0x", io->stdErr);
buf[0] = '\0';
UTIL1_strcatNum32Hex(buf, sizeof(buf), parserInfo.currAddress);
CLS1_SendStr(buf, io->stdErr);
CLS1_SendStr((unsigned char*)"\r\n", io->stdErr);
res = ERR_FAILED;
break;
} else {
CLS1_SendStr((unsigned char*)"\r\nS", io->stdOut);
buf[0] = parserInfo.currType;
buf[1] = '\0';
CLS1_SendStr(buf, io->stdOut);
CLS1_SendStr((unsigned char*)" address 0x", io->stdOut);
buf[0] = '\0';
UTIL1_strcatNum32Hex(buf, sizeof(buf), parserInfo.currAddress);
CLS1_SendStr(buf, io->stdOut);
}
if (parserInfo.currType=='7' || parserInfo.currType=='8' || parserInfo.currType=='9') {
/* end of records */
break;
}
} while (1);
if (res==ERR_OK) {
CLS1_SendStr((unsigned char*)"\r\ndone!\r\n", io->stdOut);
} else {
while (AS1_GetCharsInRxBuf()>0) {/* clear buffer */
AS1_ClearRxBuf();
WAIT1_Waitms(100);
}
CLS1_SendStr((unsigned char*)"\r\nfailed!\r\n", io->stdOut);
/* erase flash again to be sure we do not have illegal application image */
if (BL_EraseAppFlash(io)!=ERR_OK) {
res = ERR_FAILED;
}
}
return res;
}
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
int32_t pid, speed, speedL, speedR;
#if PID_DEBUG
unsigned char buf[16];
static uint8_t cnt = 0;
#endif
uint8_t errorPercent;
MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;
pid = PID(currLine, setLine, config);
errorPercent = errorWithinPercent(currLine-setLine);
/* transform into different speed for motors. The PID is used as difference value to the motor PWM */
if (errorPercent <= 20) { /* pretty on center: move forward both motors with base speed */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed;
speedL = speed-pid;
} else { /* turn left */
speedR = speed+pid;
speedL = speed;
}
} else if (errorPercent <= 40) {
/* outside left/right halve position from center, slow down one motor and speed up the other */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*8/10; /* 80% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed+pid; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = speed-pid; /* decrease speed */
}
} else if (errorPercent <= 70) {
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*6/10; /* %60 */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = 0 /*maxSpeed+pid*/; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = 0 /*maxSpeed-pid*/; /* decrease speed */
}
} else {
/* line is far to the left or right: use backward motor motion */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %80 */
if (pid<0) { /* turn right */
speedR = -speed+pid; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = -speed-pid; /* decrease speed */
}
speedL = Limit(speedL, -speed, speed);
speedR = Limit(speedR, -speed, speed);
directionL = AbsSpeed(&speedL);
directionR = AbsSpeed(&speedR);
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speedL>0xFFFF) {
speedL = 0xFFFF;
} else if (speedL<0) {
speedL = 0;
}
if (speedR>0xFFFF) {
speedR = 0xFFFF;
} else if (speedR<0) {
speedR = 0;
}
/* send new speed values to motor */
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
#if PID_DEBUG /* debug diagnostic */
{
cnt++;
if (cnt>10) { /* limit number of messages to the console */
CLS1_StdIO_OutErr_FctType ioOut = CLS1_GetStdio()->stdOut;
cnt = 0;
CLS1_SendStr((unsigned char*)"line:", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16u(buf, sizeof(buf), currLine);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" sum:", ioOut);
buf[0] = '\0';
UTIL1_strcatNum32Hex(buf, sizeof(buf), integral);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" left:", ioOut);
CLS1_SendStr(directionL==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16Hex(buf, sizeof(buf), speedL);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" right:", ioOut);
CLS1_SendStr(directionR==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16Hex(buf, sizeof(buf), speedR);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)"\r\n", ioOut);
}
}
#endif
}
