/*
** Convert a string to an address. Do any necessary linearization.
*/
ULONG StrToAddr(char *str, int type)
{
ULONG val;
char *end;
val = strtoul(str, &end, 16);
if((val & 0xffff0000) != 0)
return val;
DispatchCommand(DBG_C_ReadReg);
switch(type) {
case TOADDR_CODE: return Linearize(val, debugBuffer.CS);
case TOADDR_DATA: return Linearize(val, debugBuffer.DS);
case TOADDR_STACK: return Linearize(val, debugBuffer.SS);
}
return 0;
}
double Shooter::Linearize(double x) {
if (x >= 0.0) {
return constants_->shooterCoeffA * pow(x, 4) + constants_->shooterCoeffB * pow(x, 3) +
constants_->shooterCoeffC * pow(x, 2) + constants_->shooterCoeffD * x;
} else {
// Rotate the linearization function by 180.0 degrees to handle negative input.
return -Linearize(-x);
}
}
double Shooter::ConveyorLinearize(double x) {
if (fabs(x) < 0.01 ) {
return 0;
} else if (x > 0) {
return constants_->conveyorCoeffA * pow(x, 3) + constants_->conveyorCoeffB * pow(x, 2) +
constants_->conveyorCoeffC * x + constants_->conveyorCoeffD;
} else {
// Rotate the linearization function by 180.0 degrees to handle negative input.
return -Linearize(-x);
}
}
void TransformNDOF::GetJacobian(const scalarArray &q, se3Array &J)
{
SE3 T = GetTransform(q);
scalarArray dq(m_iDOF);
for ( int i = 0; i < m_iDOF; i++ ) dq[i] = q[i];
for ( int i = 0; i < m_iDOF; i++ )
{
dq[i] += m_rEPS;
J[i] = (SCALAR_1 / m_rEPS) * Linearize(T % GetTransform(dq));
dq[i] -= m_rEPS;
}
}
/*
** Take a string which is supposed to be an executable address,
** and find out what that address is.
*/
ULONG FindExecAddr(char *label, char **brkDesc)
{
ULONG addr = 0;
char *desc;
char *dummy;
ULONG lineNum;
DebugModule *module;
char funcName[MAX_FUNCNAME];
char sourceName[CCHMAXPATH];
/*
** If there is no label, give up the ghost!
*/
if(label == NULL)
return 0;
/*
** Find the address of the line given a line number.
*/
if(label[0] == '.') {
module = FindModule(debugBuffer.MTE, NULL);
FindSource(module, Linearize(debugBuffer.EIP, debugBuffer.CS),
funcName, sourceName, &lineNum);
/* Cannot find source module. */
if(strcmp(sourceName, "UNKNOWN") == 0) {
return 0;
}
lineNum = strtol(&label[1], &dummy, 0);
addr = FindSourceLine(module, lineNum, sourceName);
/*
** Build a string which describes the breakpoint.
*/
desc = malloc(strlen(label) + strlen(sourceName) + 1);
strcpy(desc, sourceName);
strcat(desc, ":");
strcat(desc, &label[1]);
*brkDesc = desc;
return addr;
}
/*
** If we have a '!' in the string, then it is a compound
** filename/line number
*/
if(strchr(label, '!') != NULL) {
char *line;
line = strchr(label, '!');
*line = '\0';
module = FindModule(debugBuffer.MTE, NULL);
FindSource(module, Linearize(debugBuffer.EIP, debugBuffer.CS),
funcName, sourceName, &lineNum);
/* Cannot find source module. */
if(strcmp(sourceName, "UNKNOWN") == 0) {
return 0;
}
lineNum = strtol(&label[1], &dummy, 0);
addr = FindSourceLine(module, lineNum, sourceName);
/*
** Build a string which describes the breakpoint.
*/
desc = malloc(strlen(label) + strlen(sourceName) + 1);
strcpy(desc, sourceName);
strcat(desc, ":");
strcat(desc, &label[1]);
*brkDesc = desc;
return addr;
}
/*
** Try finding the name as a function
*/
if((addr = FindFuncAddr(NULL, label)) != 0) {
desc = malloc(strlen(label) + 1);
strcpy(desc, label);
*brkDesc = desc;
return addr;
}
/*
** If we could not find a function, try using the label as
** a hex offset to break at.
*/
addr = StrToAddr(label, TOADDR_CODE);
desc = malloc(strlen(label) + 1);
strcpy(desc, label);
*brkDesc = desc;
return addr;
}
/*
** Do a single step to the next source instruction.
*/
int CommandStep(char **ptrs)
{
int i;
ULONG addr;
ULONG lineNum;
Breakpoint *bp;
DebugModule *module;
char funcName[MAX_FUNCNAME];
char funcName2[MAX_FUNCNAME];
char sourceName[CCHMAXPATH];
/*
** Provide a reference to keep from getting a compile warning.
*/
ptrs;
/*
** Find the address specified.
*/
module = FindModule(debugBuffer.MTE, NULL);
FindSource(module, Linearize(debugBuffer.EIP, debugBuffer.CS),
funcName, sourceName, &lineNum);
for(i=0; i<100; i++) {
lineNum++;
addr = FindSourceLine(module, lineNum, sourceName);
if(addr != 0) {
FindSource(module, addr, funcName2, sourceName, &lineNum);
if(strcmp(funcName2, funcName) == 0)
break;
fprintf(logFile, "Unable to find next line. Next function found!\n");
return -1;
}
}
/*
** Did we find a line
*/
if(addr == 0) {
fprintf(logFile, "Unable to find next line.\n");
return -1;
}
/*
** Set the breakpoint at the address specified.
*/
debugBuffer.Addr = addr;
debugBuffer.Len = 1;
debugBuffer.Index = 0;
debugBuffer.Value = DBG_W_Local | DBG_W_Execute;
DispatchCommand(DBG_C_SetWatch);
/*
** Add the breakpoint to the list, and tag it as a 'go'
** breakpoint which will unconditionally be cleared the
** next time we get back from the debuggee.
*/
if(Breakpoints) {
for(i=0, bp=Breakpoints; bp->next; i++, bp=bp->next) ;
bp->next = malloc(sizeof(Breakpoint));
bp = bp->next;
} else {
i = 0;
Breakpoints = bp = malloc(sizeof(Breakpoint));
}
bp->id = debugBuffer.Index;
bp->addr = addr;
bp->next = NULL;
bp->desc = NULL;
bp->oneTimeFlag = 1;
return DBG_C_Go;
}
/*
** Dump the stack frame.
*/
void DumpStack(int threadID)
{
ULONG lineNum;
struct {
ULONG ebp;
ULONG eip;
} info;
DebugModule *module;
ULONG oldTid;
ULONG objectNum;
ULONG baseOffset;
USHORT lastCS;
char funcName[MAX_FUNCNAME];
char sourceName[CCHMAXPATH];
/*
** Find the base and then extract out the EIP. Follow the chain
** until EBP == 0
*/
oldTid = debugBuffer.Tid;
debugBuffer.Tid = threadID;
DispatchCommand(DBG_C_ReadReg);
lastCS = debugBuffer.CS;
info.ebp = Linearize(debugBuffer.EBP, debugBuffer.SS);
info.eip = Linearize(debugBuffer.EIP, debugBuffer.CS);
while(1) {
/*
** End of chain.
*/
if((info.ebp == 0) || (info.eip == 0)) {
debugBuffer.Tid = oldTid;
return;
}
/*
** Find the module.
*/
debugBuffer.Addr = info.eip;
DispatchCommand(DBG_C_AddrToObject);
module = FindModule(debugBuffer.MTE, NULL);
if(module == NULL) {
fprintf(logFile, "MODULE NOT FOUND!\n");
debugBuffer.Tid = oldTid;
return;
}
/*
** Dump EBP:EIP of the current stack frame.
*/
baseOffset = debugBuffer.Buffer;
fprintf(logFile, "EBP:\t%08x\tEIP:\t%08x\n", info.ebp, info.eip);
fprintf(logFile, " Base:\t%08x\tRel:\t%08x\tLen:\t%08x\n",
baseOffset, info.eip - baseOffset, debugBuffer.Len);
/*
** Find the object number associated with the address.
*/
for(objectNum=1;;objectNum++) {
debugBuffer.Value = (ULONG) objectNum;
debugBuffer.MTE = module->MTE;
if(DispatchCommand(DBG_C_NumToAddr) != DBG_N_Success)
break;
if(debugBuffer.Addr == baseOffset)
break;
}
fprintf(logFile, " Object: %08x\n", objectNum);
/*
** Dump the values.
*/
FindSource(module, info.eip,
funcName, sourceName, &lineNum);
if(lineNum != 0)
fprintf(logFile, " Module: %s\n"
" Size: %u\n"
" Timestamp:%s\n"
" Function: %s\n"
" Source: %s\n"
" Line: %d\n\n",
module->name, module->fileSize, ctime(&module->fTimestamp),
funcName, sourceName, lineNum);
else
fprintf(logFile, " Module: %s\n"
" Size: %u\n"
" Timestamp:%s\n"
" Lo Function: %s\n"
" Hi Function: %s\n\n",
module->name, module->fileSize, ctime(&module->fTimestamp),
funcName, sourceName);
#ifdef SHERLOCK
{
DebugModule *module;
char *mod;
debugBuffer.Addr = info.eip;
DispatchCommand(DBG_C_AddrToObject);
if((debugBuffer.Cmd == DBG_N_Success) &&
(debugBuffer.Value & 0x10000000)) {
module = FindModule(debugBuffer.MTE, NULL);
if(module == NULL)
mod = "UNKNOWN";
else
mod = module->name;
FindSource(module, info.eip,
funcName, sourceName, &lineNum);
fprintf(logFile, "EIP: %08x, DLL: %s Func: %s\n",
info.eip, mod, funcName);
DisplaySource(module, sourceName, lineNum);
fprintf(logFile, "\n\n");
}
}
#endif
/*
** Get prior EBP, EIP
*/
if(module->typeFlags & FAPPTYP_32BIT) {
debugBuffer.Addr = info.ebp;
debugBuffer.Len = 8;
debugBuffer.Buffer = (ULONG) &info.ebp;
info.ebp = info.eip = 0;
DispatchCommand(DBG_C_ReadMemBuf);
} else {
USHORT codePtr[2];
/* Get the new code pointer. */
debugBuffer.Addr = info.ebp+2;
debugBuffer.Len = 4;
debugBuffer.Buffer = (ULONG) &codePtr;
info.eip = 0;
DispatchCommand(DBG_C_ReadMemBuf);
/* Now, get the new base pointer. */
debugBuffer.Addr = info.ebp;
debugBuffer.Len = 2;
debugBuffer.Buffer = (ULONG) &info.ebp;
info.ebp = 0;
DispatchCommand(DBG_C_ReadMemBuf);
info.ebp = Linearize(info.ebp, debugBuffer.SS);
/*
** Now for real hocus pocus. Try to find out
** if the pointer is a near or far call!
**
** First, check for NULL pointer, Must be end of chain.
*/
if((codePtr[0] == 0) && (codePtr[1] == 0)) {
info.eip = 0;
} else {
USHORT tmp;
/*
** If supposidly ring 0 or ring 1 caller, then
** that cannot be correct, must be a near call.
*/
tmp = codePtr[1] & 0x03;
if((tmp == 0) || (tmp == 1)) {
info.eip = Linearize(codePtr[0], lastCS);
} else {
/*
** Assume that it is a far pointer.
*/
lastCS = codePtr[1];
info.eip = Linearize(codePtr[0], lastCS);
}
}
}
}
debugBuffer.Tid = oldTid;
return;
}
/*
** View the source for the lines specified.
*/
int CommandView(char **ptrs)
{
DebugModule *module;
char *srcEnd;
ULONG addr;
ULONG lineNum;
char funcName[MAX_FUNCNAME];
char sourceName[CCHMAXPATH];
/*
** Get the common data.
*/
module = FindModule(debugBuffer.MTE, NULL);
FindSource(module, Linearize(debugBuffer.EIP, debugBuffer.CS),
funcName, sourceName, &lineNum);
/*
** View the next lines to be displayed.
*/
if(ptrs[2] == NULL) {
DisplaySource(module, sourceName, GetLastLine(module) + 5);
return -1;
}
/*
** View a line.
*/
if(ptrs[2][0] == '.') {
/*
** Find the line number or the file name/line number
*/
if(isdigit(ptrs[2][1])) {
lineNum = atol(&ptrs[2][1]);
} else {
strcpy(sourceName, &ptrs[2][1]);
*strrchr(sourceName, ':') = 0;
lineNum = atol(strrchr(ptrs[2], ':') + 1);
}
DisplaySource(module, sourceName, lineNum);
return -1;
}
/*
** Get a view at a given offset.
*/
if(isxdigit(ptrs[2][0])) {
/*
** Find the module associated with the address specified.
*/
debugBuffer.Addr = addr = StrToAddr(ptrs[2], TOADDR_CODE);
DispatchCommand(DBG_C_AddrToObject);
/*
** Find the module/source associated with the information given.
*/
module = FindModule(debugBuffer.MTE, NULL);
FindSource(NULL, addr, funcName, sourceName, &lineNum);
DisplaySource(module, sourceName, lineNum);
return -1;
}
/*
** ERROR!
*/
fprintf(logFile, "Invalid syntax\n");
return -1;
}
/*
** Display the source file and line number.
*/
int DisplaySource(DebugModule *module, char *sourceName, int lineNum)
{
int i, lastChar, is32Bit;
int curLine;
FILE *file = NULL;
View *viewData = (View *) module->ViewData;
ULONG addr, addr2;
char buff[CCHMAXPATH];
char dummy[CCHMAXPATH];
/*
** Find out whether this is a 32 bit segment.
*/
DispatchCommand(DBG_C_ReadReg);
is32Bit = (debugBuffer.CSAtr & 0x80) != 0;
/*
** If the view data does not exist, create and initialize it.
*/
if(viewData == NULL) {
viewData = module->ViewData = calloc(sizeof(View), 1);
viewData->lastSource = strdup("");
viewData->basePath = strdup("");
}
/*
** Find and open the source file.
*/
if(strlen(sourceName) > 0) {
strcpy(buff, sourceName);
_splitpath(sourceName, drive, dir, fname, ext);
_splitpath(viewData->basePath, drive, dir, dummy, dummy);
_makepath(buff, drive, dir, fname, ext);
while((file = fopen(buff, "r")) == NULL) {
#ifdef SHERLOCK
buff[0] = 0;
#else
fprintf(logFile, "Please enter path for %s\n", sourceName);
fgets(buff, sizeof(buff), stdin);
#endif
while((strlen(buff) > 0) && isspace(*buff))
buff[strlen(buff) - 1] = 0;
if(strlen(buff) == 0)
return 0;
lastChar = strlen(buff) - 1;
while(isspace(buff[lastChar])) {
buff[lastChar] = 0;
lastChar--;
}
if((buff[lastChar] != '\\') && (buff[lastChar] != '/'))
strcat(buff, "/");
_splitpath(buff, drive, dir, dummy, dummy);
_makepath(buff, drive, dir, fname, ext);
}
}
/*
** Free/show the last source viewed.
*/
if(viewData->lastSource)
free(viewData->lastSource);
viewData->lastSource = strdup(buff);
/*
** Free the previous path spec.
*/
if(viewData->basePath)
free(viewData->basePath);
_splitpath(buff, drive, dir, fname, ext);
_makepath(buff, drive, dir, "", "");
viewData->basePath = strdup(buff);
/*
** Go to just before the line specific.
*/
if(file) {
curLine = 0;
for(curLine=1; curLine < lineNum - 5; curLine++) {
fgets(buff, sizeof(buff), file);
}
}
/*
** Now, display the source.
*/
if(file) {
for(i=0; i<10; i++, curLine++) {
if(file)
if(fgets(buff, sizeof(buff), file) == NULL)
break;
if(ShouldDumpSource) {
if(curLine == lineNum)
fprintf(logFile, "*%5d: %s", curLine, buff);
else
fprintf(logFile, " %5d: %s", curLine, buff);
}
if((addr = FindSourceLine(module, curLine, sourceName)) != 0) {
int j;
/*
** Find the next line.
*/
for(j=1; j < 10; j++) {
if((addr2=FindSourceLine(module,curLine+j,sourceName))!=0)
break;
}
}
if(addr2 == 0)
addr2 = addr + 0x10;
if(ShouldDumpAsm)
DumpAsm(addr, addr2-addr, is32Bit);
}
} else {
addr = Linearize(debugBuffer.EIP, debugBuffer.CS);
DumpAsm(addr, 0x20, is32Bit);
addr2 = addr + 0x20;
}
viewData->lastLine = curLine - 1;
viewData->lastAddr = addr2;
if(file)
fclose(file);
return 1;
}
/*
** View the assembler for the address/line specified.
*/
int CommandUnassemble(char **ptrs)
{
DebugModule *module;
View *viewData;
char *srcEnd;
ULONG addr;
int lineNum;
int is32Bit;
char sourceName[CCHMAXPATH];
/*
** Get the common data.
*/
module = FindModule(debugBuffer.MTE, NULL);
viewData = (View *) module->ViewData;
if(viewData == NULL) {
viewData = module->ViewData = calloc(sizeof(View), 1);
viewData->lastSource = strdup("");
viewData->basePath = strdup("");
}
is32Bit = (debugBuffer.CSAtr & 0x80) != 0;
addr = Linearize(debugBuffer.EIP, debugBuffer.CS);
/*
** View the next lines to be displayed.
*/
if(ptrs[2] == NULL) {
DumpAsm(viewData->lastAddr, 0x20, is32Bit);
viewData->lastAddr = addr + 0x20;
return -1;
}
/*
** View a source line.
*/
if(ptrs[2][0] == '.') {
/*
** Find the line number or the file name/line number
*/
if(isdigit(ptrs[2][1])) {
lineNum = atol(&ptrs[2][1]);
if(viewData->lastSource == NULL) {
fprintf(logFile, "No source yet displayed\n");
return -1;
}
strcpy(sourceName, viewData->lastSource);
} else {
strcpy(sourceName, &ptrs[2][1]);
*strrchr(sourceName, ':') = 0;
lineNum = atol(strrchr(ptrs[2], ':') + 1);
}
addr = FindSourceLine(module, lineNum, sourceName);
DumpAsm(addr, 0x20, is32Bit);
viewData->lastAddr = addr + 0x20;
return -1;
}
/*
** Get a view at a given offset.
*/
if(isxdigit(ptrs[2][0])) {
addr = StrToAddr(ptrs[2], TOADDR_CODE);
DumpAsm(addr, 0x20, is32Bit);
viewData->lastAddr = addr + 0x20;
return -1;
}
/*
** ERROR!
*/
fprintf(logFile, "Invalid syntax\n");
return -1;
}
void Shooter::SetLinearPower(double pwm) {
SetPower(Linearize(pwm));
}