aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256/Source
diff options
context:
space:
mode:
authorJohn Hansen2010-10-16 18:12:46 +0000
committerNicolas Schodet2011-07-04 00:37:03 +0200
commit565424ed7a688f910f63fc1ce82e39ebf0dfb28f (patch)
treeb0debc1bc66133151654a885b96414e382b62b93 /AT91SAM7S256/Source
parentfb5f5e4551230270344616648c78cc4896f6626c (diff)
NBCNXC_131
git-svn-id: https://mindboards.svn.sourceforge.net/svnroot/mindboards/lms_nbcnxc_128/trunk@21 c9361245-7fe8-9947-84e8-057757c4e366
Diffstat (limited to 'AT91SAM7S256/Source')
-rw-r--r--AT91SAM7S256/Source/BtTest.inc1
-rw-r--r--AT91SAM7S256/Source/cCmdWriteIOMapOffsetsFile.txt125
-rw-r--r--AT91SAM7S256/Source/c_cmd.c194
-rw-r--r--AT91SAM7S256/Source/c_cmd.h13
-rw-r--r--AT91SAM7S256/Source/c_cmd.iom12
-rw-r--r--AT91SAM7S256/Source/c_cmd_drawing.inc2
-rw-r--r--AT91SAM7S256/Source/c_comm.c148
-rw-r--r--AT91SAM7S256/Source/c_comm.iom14
-rw-r--r--AT91SAM7S256/Source/c_display.c4
-rw-r--r--AT91SAM7S256/Source/c_input.c28
-rw-r--r--AT91SAM7S256/Source/c_loader.c5
-rw-r--r--AT91SAM7S256/Source/c_loader.iom6
-rw-r--r--AT91SAM7S256/Source/c_output.c2
-rw-r--r--AT91SAM7S256/Source/d_loader.c10
-rw-r--r--AT91SAM7S256/Source/d_loader.h6
-rw-r--r--AT91SAM7S256/Source/d_lowspeed.r44
-rw-r--r--AT91SAM7S256/Source/d_output.c366
17 files changed, 485 insertions, 495 deletions
diff --git a/AT91SAM7S256/Source/BtTest.inc b/AT91SAM7S256/Source/BtTest.inc
index ff0de90..d2ab31d 100644
--- a/AT91SAM7S256/Source/BtTest.inc
+++ b/AT91SAM7S256/Source/BtTest.inc
@@ -4,7 +4,6 @@
#include "Test2.txt"
//#define TESTPRG // If defined the test program will be included
-
extern void BtIo(void);
const char BUILD_DATE[] = __DATE__;
diff --git a/AT91SAM7S256/Source/cCmdWriteIOMapOffsetsFile.txt b/AT91SAM7S256/Source/cCmdWriteIOMapOffsetsFile.txt
new file mode 100644
index 0000000..a317c79
--- /dev/null
+++ b/AT91SAM7S256/Source/cCmdWriteIOMapOffsetsFile.txt
@@ -0,0 +1,125 @@
+#if WRITE_IOMAP_OFFSETS
+void cCmdWriteIOMapOffsetsFile()
+{
+ LOADER_STATUS LStatus;
+ UBYTE Handle;
+ ULONG BenchFileSize;
+ ULONG Length;
+ UBYTE Buffer[256];
+
+ //Remove old benchmark file, create a new one
+ strcpy((char *)Buffer, "offsets.txt");
+ pMapLoader->pFunc(DELETE, Buffer, NULL, NULL);
+ BenchFileSize = 2048;
+ LStatus = pMapLoader->pFunc(OPENWRITEDATA, Buffer, NULL, &BenchFileSize);
+
+ if (!LOADER_ERR(LStatus))
+ {
+ //Write Benchmark file
+ Handle = LOADER_HANDLE(LStatus);
+
+ //Header
+ sprintf((char *)Buffer, "%s Offsets\r\n", "Comm Module");
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "pFunc: %d\r\n", (ULONG)pMapComm->pFunc - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "pFunc2: %d\r\n", (ULONG)pMapComm->pFunc2 - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "BtDeviceTable: %d\r\n", (ULONG)pMapComm->BtDeviceTable - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "BtConnectTable: %d\r\n", (ULONG)pMapComm->BtConnectTable - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "BrickData: %d\r\n", (ULONG)pMapComm->BrickData.Name - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "BtInBuf: %d\r\n", (ULONG)pMapComm->BtInBuf.Buf - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "BtOutBuf: %d\r\n", (ULONG)pMapComm->BtOutBuf.Buf - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "HsInBuf: %d\r\n", (ULONG)pMapComm->HsInBuf.Buf - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "HsOutBuf: %d\r\n", (ULONG)pMapComm->HsOutBuf.Buf - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "UsbInBuf: %d\r\n", (ULONG)pMapComm->UsbInBuf.Buf - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "UsbOutBuf: %d\r\n", (ULONG)pMapComm->UsbOutBuf.Buf - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "UsbPollBuf: %d\r\n", (ULONG)pMapComm->UsbPollBuf.Buf - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "BtDeviceCnt: %d\r\n", (ULONG)&(pMapComm->BtDeviceCnt) - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "BtDeviceNameCnt: %d\r\n", (ULONG)&(pMapComm->BtDeviceNameCnt) - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "HsFlags: %d\r\n", (ULONG)&(pMapComm->HsFlags) - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "HsSpeed: %d\r\n", (ULONG)&(pMapComm->HsSpeed) - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "HsState: %d\r\n", (ULONG)&(pMapComm->HsState) - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "UsbState: %d\r\n", (ULONG)&(pMapComm->UsbState) - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "HsMode: %d\r\n", (ULONG)&(pMapComm->HsMode) - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "BtDataMode: %d\r\n", (ULONG)&(pMapComm->BtDataMode) - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "HsDataMode: %d\r\n", (ULONG)&(pMapComm->HsDataMode) - (ULONG)pMapComm);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "HsDataMode = %d\r\n", pMapComm->HsDataMode);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+
+ sprintf((char *)Buffer, "BtDataMode = %d\r\n", pMapComm->BtDataMode);
+ Length = strlen((char *)Buffer);
+ LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
+/*
+
+ UBYTE Spare1;
+*/
+ //close file
+ LStatus = pMapLoader->pFunc(CLOSE, &Handle, NULL, NULL);
+ }
+}
+#endif
diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c
index 4b25362..3fc28e4 100644
--- a/AT91SAM7S256/Source/c_cmd.c
+++ b/AT91SAM7S256/Source/c_cmd.c
@@ -1,11 +1,11 @@
//
// Date init 14.12.2004
//
-// Revision date $Date: 17-02-09 7:30 $
+// Revision date $Date: 26-02-10 11:38 $
//
// Filename $Workfile:: c_cmd.c $
//
-// Version $Revision: 13 $
+// Version $Revision: 15 $
//
// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_cmd. $
//
@@ -256,7 +256,7 @@ static pSysCall SysCallFuncs[SYSCALL_COUNT] =
cCmdWrapDrawEllipse,
cCmdWrapDrawFont, // 95
cCmdWrapMemoryManager,
- cCmdWrapUndefinedSysCall,
+ cCmdWrapReadLastResponse,
cCmdWrapUndefinedSysCall,
cCmdWrapUndefinedSysCall // 99 --> 100 system call slots
@@ -409,6 +409,100 @@ UBYTE cCmdBTGetDeviceType(UBYTE *pCOD)
return (Result);
}
+UBYTE CMD_RESPONSE_LENGTH[255] =
+{
+ 3, // DCStartProgram (x00)
+ 3, // DCStopProgram (x01)
+ 3, // DCPlaySoundFile (x02)
+ 3, // DCPlayTone (x03)
+ 3, // DCSetOutputState (x04)
+ 3, // DCSetInputMode (x05)
+ 25, // DCGetOutputState (x06)
+ 16, // DCGetInputValues (x07)
+ 3, // DCResetInputScaledValue (x08)
+ 3, // DCMessageWrite (x09)
+ 3, // DCResetMotorPosition (x0a)
+ 5, // DCGetBatteryLevel (x0b)
+ 3, // DCStopSoundPlayback (x0c)
+ 7, // DCKeepAlive (x0d)
+ 4, // DCLSGetStatus (x0e)
+ 3, // DCLSWrite (x0f)
+ 20, // DCLSRead (x10)
+ 23, // DCGetCurrentProgramName (x11)
+ 0, // DCGetButtonState (not implemented) (x12)
+ 64, // DCMessageRead (x13)
+ 0, // DCRESERVED1 (x14)
+ 0, // DCRESERVED2 (x15)
+ 0, // DCRESERVED3 (x16)
+ 0, // DCRESERVED4 (x17)
+ 0, // DCRESERVED5 (x18)
+ 64, // DCDatalogRead (1.28+) (x19)
+ 3, // DCDatalogSetTimes (1.28+) (x1a)
+ 4, // DCBTGetContactCount (1.28+) (x1b)
+ 21, // DCBTGetContactName (1.28+) (x1c)
+ 4, // DCBTGetConnCount (1.28+) (x1d)
+ 21, // DCBTGetConnName (1.28+) (x1e)
+ 3, // DCSetProperty(1.28+) (x1f)
+ 7, // DCGetProperty (1.28+) (x20)
+ 3, // DCUpdateResetCount (1.28+) (x21)
+ 7, // RC_SET_VM_STATE (enhanced only) (x22)
+ 7, // RC_GET_VM_STATE (enhanced only) (x23)
+ 15, // RC_SET_BREAKPOINTS (enhanced only) (x24)
+ 15, // RC_GET_BREAKPOINTS (enhanced only) (x25)
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // (x26-x2f)
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // (x30-x3f)
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // (x40-x4f)
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // (x50-x5f)
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // (x60-x6f)
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // (x70-x7f)
+ 8, // OPENREAD = 0x80,
+ 4, // OPENWRITE = 0x81,
+ 64, // READ = 0x82, (actually is a variable length response)
+ 6, // WRITE = 0x83,
+ 4, // CLOSE = 0x84,
+ 23, // DELETE = 0x85,
+ 28, // FINDFIRST = 0x86,
+ 28, // FINDNEXT = 0x87,
+ 7, // VERSIONS = 0x88,
+ 4, // OPENWRITELINEAR = 0x89,
+ 7, // OPENREADLINEAR = 0x8A, (not actually implemented)
+ 4, // OPENWRITEDATA = 0x8B,
+ 8, // OPENAPPENDDATA = 0x8C,
+ 4, // CROPDATAFILE = 0x8D, /* New cmd for datalogging */
+ 0, // XXXXXXXXXXXXXX = 0x8E,
+ 0, // XXXXXXXXXXXXXX = 0x8F,
+ 34, // FINDFIRSTMODULE = 0x90,
+ 34, // FINDNEXTMODULE = 0x91,
+ 4, // CLOSEMODHANDLE = 0x92,
+ 0, // XXXXXXXXXXXXXX = 0x93,
+ 64, // IOMAPREAD = 0x94, (actually is a variable length response)
+ 9, // IOMAPWRITE = 0x95,
+ 0, // XXXXXXXXXXXXXX = 0x96,
+ 7, // BOOTCMD = 0x97, (can only be executed via USB)
+ 3, // SETBRICKNAME = 0x98,
+ 0, // XXXXXXXXXXXXXX = 0x99,
+ 10, // BTGETADR = 0x9A,
+ 33, // DEVICEINFO = 0x9B,
+ 0, // XXXXXXXXXXXXXX = 0x9C,
+ 0, // XXXXXXXXXXXXXX = 0x9D,
+ 0, // XXXXXXXXXXXXXX = 0x9E,
+ 0, // XXXXXXXXXXXXXX = 0x9F,
+ 3, // DELETEUSERFLASH = 0xA0,
+ 5, // POLLCMDLEN = 0xA1,
+ 64, // POLLCMD = 0xA2,
+ 44, // RENAMEFILE = 0xA3,
+ 3, // BTFACTORYRESET = 0xA4,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // (xA5-xAF)
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // (xB0-xBf)
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // (xC0-xCf)
+ 0, // RESIZEDATAFILE = 0xD0,
+ 0, // SEEKFROMSTART = 0xD1,
+ 0, // SEEKFROMCURRENT = 0xD2,
+ 0, // SEEKFROMEND = 0xD3
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // (xD4-xDF)
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 // (xF0-xFF)
+};
+
//cCmdHandleRemoteCommands is the registered handler for "direct" command protocol packets
//It is only intended to be called via c_comm's main protocol handler
UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
@@ -1226,12 +1320,15 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
//If telegram doesn't check out, do nothing. No errors are ever returned for reply telegrams.
}
- break;
+ // fall through to the default case
+// break;
default:
{
//Unhandled reply telegram. Do nothing.
//!!! Could/should stash unhandled/all replies somewhere so a syscall could read them
+ VarsCmd.LastResponseLength = CMD_RESPONSE_LENGTH[pInBuf[0]];
+ memcpy((PSZ)VarsCmd.LastResponseBuffer, (PSZ)(&pInBuf[0]), VarsCmd.LastResponseLength-1);
}
break;
};
@@ -1418,10 +1515,9 @@ void cCmdCtrl(void)
IOMapCmd.ProgStatus = PROG_OK;
}
}
- else if (Status == CLUMP_SUSPEND || Status == CLUMP_DONE) {
+ else if (Status == CLUMP_SUSPEND || Status == CLUMP_DONE)
Continue = TRUE; // queue isn't empty, didn't timeout
//Only rotate RunQ on a "normal" finish, i.e. no error, clump end, or breakout request
- }
else if (Status == ROTATE_QUEUE) { // done and suspend do their own
cCmdRotateQ();
Continue= TRUE;
@@ -1434,7 +1530,6 @@ void cCmdCtrl(void)
{
Continue = FALSE;
VarsCmd.VMState = VM_RESET1;
-// IOMapCmd.ProgStatus = PROG_ERROR;
IOMapCmd.ProgStatus = Status;
}
else if (Status == STOP_REQ)
@@ -1476,7 +1571,6 @@ void cCmdCtrl(void)
//2. Proceed to VM_RESET1 (some unneeded work, yes, but preserves contract with UI
if (IS_ERR(Status))
{
-// IOMapCmd.ProgStatus = PROG_ERROR;
IOMapCmd.ProgStatus = Status;
VarsCmd.VMState = VM_RESET1;
}
@@ -2101,6 +2195,9 @@ NXT_STATUS cCmdActivateProgram(UBYTE * pFileName)
VarsCmd.Debugging = FALSE;
VarsCmd.PauseClump = NOT_A_CLUMP;
VarsCmd.PausePC = 0xFFFF;
+ // restore default data mode values
+ pMapComm->BtDataMode = DATA_MODE_NXT|DATA_MODE_UPDATE;
+ pMapComm->HsDataMode = DATA_MODE_RAW|DATA_MODE_UPDATE;
return (Status);
}
@@ -2151,8 +2248,7 @@ void cCmdDeactivateProgram()
tmp = i;
//Close file
if (*(VarsCmd.FileHandleTable[i]) != 0)
- pMapLoader->pFunc(CLOSE, &tmp, NULL, NULL);
-// pMapLoader->pFunc(CROPDATAFILE, &tmp, NULL, NULL);
+ pMapLoader->pFunc(CROPDATAFILE, &tmp, NULL, NULL); /*CLOSE*/
}
//Clear FileHandleTable
@@ -4469,33 +4565,6 @@ NXT_STATUS cCmdInterpFromClump()
pInstr = pClumpRec->PC; // abs
lastClumpInstr= pClumpRec->CodeEnd; // abs
-/*
- // are we free running and reach a breakpoint?
- if (VarsCmd.VMState == VM_RUN_FREE)
- {
- CLUMP_BREAK_REC* pBreakpoints = pClumpRec->Breakpoints;
- for(int j = 0; j < MAX_BREAKPOINTS; j++)
- {
- if (pBreakpoints[j].Enabled &&
- (pBreakpoints[j].Location == (CODE_INDEX)(pClumpRec->PC-pClumpRec->CodeStart)))
- {
- VarsCmd.VMState = VM_RUN_PAUSE;
- return BREAKOUT_REQ;
- }
- }
- // auto pause at clump == pauseClump and relative PC = pausePC
- if ((Clump == VarsCmd.PauseClump) &&
- ((CODE_INDEX)(pClumpRec->PC-pClumpRec->CodeStart) == VarsCmd.PausePC))
- {
- VarsCmd.VMState = VM_RUN_PAUSE;
- // turn off the auto pause flags
- VarsCmd.PauseClump = NOT_A_CLUMP;
- VarsCmd.PausePC = 0xFFFF;
- return BREAKOUT_REQ;
- }
- }
-*/
-
if(VarsCmd.VMState == VM_RUN_FREE)
i = pClumpRec->Priority;
else
@@ -5056,7 +5125,11 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode)
if (TypeCode2 == TC_FLOAT)
{
FltArgVal2 = cCmdGetFloatValFromDataArg(Arg2, 0);
- Count = sprintf(Buffer, "%.4f", FltArgVal2);
+ if ((FltArgVal2 > (float)99999999999.9999)||(FltArgVal2 < (float)-9999999999.9999)){ // these are the widest %.4f numbers that will fit on display
+ Count = sprintf(Buffer, "%.6g", FltArgVal2);
+ }
+ else
+ Count = sprintf(Buffer, "%.4f", FltArgVal2);
Count++; //add room for null terminator
// remove trailing zeros
while (Buffer[Count-2] == 0x30) {
@@ -5984,7 +6057,7 @@ ULONG cCmdBinop(CODE_WORD const Code, ULONG LeftOp, ULONG RightOp, TYPE_CODE Lef
case OP_XOR:
{
- return ((LeftOp | RightOp) & (~(LeftOp & RightOp))); // LeftOp ^ RightOp
+ return ((LeftOp | RightOp) & (~(LeftOp & RightOp)));
}
case OP_CMP:
@@ -6229,7 +6302,7 @@ NXT_STATUS cCmdMove(DATA_ARG Arg1, DATA_ARG Arg2)
Status= NO_ERR;
}
}
- else if(tc1 == TC_FLOAT && tc2 == TC_FLOAT) {
+ else if(tc1 == TC_FLOAT && tc2 == TC_FLOAT) { // may also need to speed up float to int and int to float conversions
moveFloat++;
pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset;
pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset;
@@ -9539,6 +9612,47 @@ NXT_STATUS cCmdWrapMemoryManager(UBYTE * ArgV[])
return (NO_ERR);
}
+//cCmdWrapReadLastResponse
+//ArgV[0]: (return) Status byte, SBYTE
+//ArgV[1]: Clear?, UBYTE (true or false)
+//ArgV[2]: Length, UBYTE out
+//ArgV[3]: Command, UBYTE out
+//ArgV[4]: Buffer, out
+NXT_STATUS cCmdWrapReadLastResponse(UBYTE * ArgV[])
+{
+ SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
+ UWORD bufLen = 0;
+ if (VarsCmd.LastResponseLength > 0)
+ bufLen = VarsCmd.LastResponseLength-2;
+
+ //Resolve array arguments
+ // output buffer
+ DV_INDEX DVIndex = *(DV_INDEX *)(ArgV[4]);
+ //Size Buffer to Length
+ NXT_STATUS Status = cCmdDVArrayAlloc(DVIndex, bufLen);
+ if (IS_ERR(Status))
+ return Status;
+ UBYTE* pBuf = cCmdDVPtr(DVIndex);
+ ArgV[4] = pBuf;
+ *(ArgV[2]) = bufLen; // Length
+ *pReturnVal = NO_ERR;
+
+ if (bufLen > 0)
+ {
+ memset(pBuf, 0, bufLen);
+ memcpy(pBuf, (PSZ)&(VarsCmd.LastResponseBuffer[2]), bufLen-1);
+ *pReturnVal = VarsCmd.LastResponseBuffer[1];
+ *(ArgV[3]) = VarsCmd.LastResponseBuffer[0];
+ }
+ // clear?
+ if (*(ArgV[1])) {
+ VarsCmd.LastResponseLength = 0;
+ memset(VarsCmd.LastResponseBuffer, 0, 64);
+ }
+
+ return (NO_ERR);
+}
+
NXT_STATUS cCmdWrapUndefinedSysCall(UBYTE * ArgV[])
{
return (NO_ERR);
diff --git a/AT91SAM7S256/Source/c_cmd.h b/AT91SAM7S256/Source/c_cmd.h
index 489e0cc..b23e9f6 100644
--- a/AT91SAM7S256/Source/c_cmd.h
+++ b/AT91SAM7S256/Source/c_cmd.h
@@ -63,7 +63,7 @@ void cCmdExit(void);
#define WRITE_IOMAP_OFFSETS 0
#if WRITE_IOMAP_OFFSETS
-void cCmdWriteIOMapOffsetsFile();
+void cCmdWriteIOMapOffsetsFile();
#endif
//
@@ -566,6 +566,10 @@ typedef struct
UBYTE PauseClump;
CODE_INDEX PausePC;
+ // add a buffer for storing the last response raw content (64 bytes)
+ UBYTE LastResponseBuffer[64];
+ UBYTE LastResponseLength;
+
#if VM_BENCHMARK
ULONG InstrCount;
ULONG Average;
@@ -575,10 +579,8 @@ typedef struct
ULONG CompactionCount;
ULONG LastCompactionTick;
ULONG MaxCompactionTime;
- ULONG CmdCtrlOverTimeCnt;
- ULONG MaxCmdCtrlOverTimeLen;
- ULONG OpcodeBenchmarks[OPCODE_COUNT][3];
- ULONG SyscallBenchmarks[SYSCALL_COUNT][3];
+ ULONG OpcodeBenchmarks[OPCODE_COUNT][4];
+ ULONG SyscallBenchmarks[SYSCALL_COUNT][4];
UBYTE Buffer[256];
#endif
@@ -906,6 +908,7 @@ NXT_STATUS cCmdWrapDrawPolygon(UBYTE * ArgV[]);
NXT_STATUS cCmdWrapDrawEllipse(UBYTE * ArgV[]);
NXT_STATUS cCmdWrapDrawFont(UBYTE * ArgV[]);
NXT_STATUS cCmdWrapMemoryManager(UBYTE * ArgV[]);
+NXT_STATUS cCmdWrapReadLastResponse(UBYTE * ArgV[]);
NXT_STATUS cCmdWrapUndefinedSysCall(UBYTE * ArgV[]);
diff --git a/AT91SAM7S256/Source/c_cmd.iom b/AT91SAM7S256/Source/c_cmd.iom
index 09a6477..820f01d 100644
--- a/AT91SAM7S256/Source/c_cmd.iom
+++ b/AT91SAM7S256/Source/c_cmd.iom
@@ -168,17 +168,7 @@ typedef SBYTE PROGRAM_STATUS;
#define PROG_ERROR 0x03
#define PROG_ABORT 0x04
#define PROG_RESET 0x05
-/*
-typedef enum
-{
- PROG_IDLE,
- PROG_OK,
- PROG_RUNNING,
- PROG_ERROR,
- PROG_ABORT,
- PROG_RESET
-} PROGRAM_STATUS;
-*/
+
//Maximum size of memory pool, in bytes
//!!! Code assumes this value is evenly divisible by 4!
#define POOL_MAX_SIZE 32768
diff --git a/AT91SAM7S256/Source/c_cmd_drawing.inc b/AT91SAM7S256/Source/c_cmd_drawing.inc
index 9a62c12..41e405a 100644
--- a/AT91SAM7S256/Source/c_cmd_drawing.inc
+++ b/AT91SAM7S256/Source/c_cmd_drawing.inc
@@ -1709,7 +1709,7 @@ void cCmdDrawString(UBYTE *pString, ULONG X, ULONG Y, UBYTE InvertMode, UBYTE Lo
//------------------------------------------------------------------
// cCmdRestoreDefaultScreen - Restore to Default 'Running' screen
-__ramfunc void cCmdRestoreDefaultScreen(void)
+void cCmdRestoreDefaultScreen(void)
{
//If this program has taken over the display, reset it for the UI
if (VarsCmd.DirtyDisplay == TRUE)
diff --git a/AT91SAM7S256/Source/c_comm.c b/AT91SAM7S256/Source/c_comm.c
index f550ca3..1c9db04 100644
--- a/AT91SAM7S256/Source/c_comm.c
+++ b/AT91SAM7S256/Source/c_comm.c
@@ -62,11 +62,11 @@ enum
dBtClearArm7CmdSignal();\
dBtInitReceive(VarsComm.BtModuleInBuf.Buf, (UBYTE)CMD_MODE, FALSE);
-#define SETBtDataState(_m) IOMapComm.BtInBuf.InPtr = 0;\
- VarsComm.BtState = _m;\
+#define SETBtDataState IOMapComm.BtInBuf.InPtr = 0;\
+ VarsComm.BtState = BT_ARM_DATA_MODE;\
dBtClearTimeOut(); /* stop cmd timeout because in datamode */\
dBtSetArm7CmdSignal();\
- dBtInitReceive(VarsComm.BtModuleInBuf.Buf, (UBYTE)STREAM_MODE, (_m == BT_ARM_DATA_MODE ? FALSE : TRUE));
+ dBtInitReceive(VarsComm.BtModuleInBuf.Buf, (UBYTE)STREAM_MODE, IOMapComm.BtDataMode != DATA_MODE_NXT);
#define SETBtOff VarsComm.BtState = BT_ARM_OFF;\
dBtSetBcResetPinLow()
@@ -160,9 +160,10 @@ void cCommInit(void* pHeader)
}
IOMapComm.BtDeviceCnt = 0;
IOMapComm.BrickData.BtStateStatus = 0;
- IOMapComm.HsSpeed = HS_BAUD_921600;
- IOMapComm.HsMode = HS_MODE_8N1;
- IOMapComm.BtState = BT_ARM_DATA_MODE;
+ IOMapComm.HsSpeed = HS_BAUD_921600;
+ IOMapComm.HsMode = HS_MODE_8N1;
+ IOMapComm.BtDataMode = DATA_MODE_NXT;
+ IOMapComm.HsDataMode = DATA_MODE_RAW;
cCommClrConnTable();
@@ -180,7 +181,18 @@ void cCommInit(void* pHeader)
void cCommCtrl(void)
{
-
+ // remove the update flag from the hi-speed data mode field
+ IOMapComm.HsDataMode &= ~DATA_MODE_UPDATE;
+
+ if (IOMapComm.BtDataMode & DATA_MODE_UPDATE)
+ {
+ // remove the update flag from the data mode field
+ IOMapComm.BtDataMode &= ~DATA_MODE_UPDATE;
+ // re-initialize the receiver (only changing the NoLengthBytes param)
+
+ dBtInitReceive(VarsComm.BtModuleInBuf.Buf, (UBYTE)((VarsComm.BtState == BT_ARM_CMD_MODE) ? CMD_MODE : STREAM_MODE), IOMapComm.BtDataMode != DATA_MODE_NXT);
+ }
+
if (FALSE == cCommReceivedBtData())
{
@@ -206,7 +218,7 @@ void cCommCtrl(void)
switch (VarsComm.BtState)
{
- /* Bluetooth device can either be in CMD, DATA, STREAM or OFF state at top level */
+ /* Bluetooth device can either be in CMD, DATA or OFF state at top level */
case BT_ARM_OFF:
{
}
@@ -215,14 +227,12 @@ void cCommCtrl(void)
{
if (VarsComm.BtBcPinLevel)
{
- SETBtDataState(IOMapComm.BtState);
+ SETBtDataState;
}
}
break;
case BT_ARM_DATA_MODE:
- case BT_ARM_GPS_MODE:
- case BT_ARM_RAW_MODE:
{
if (!(VarsComm.BtBcPinLevel))
{
@@ -232,8 +242,8 @@ void cCommCtrl(void)
break;
}
}
- // don't overwrite this byte when we are in GPS or RAW mode
- if (VarsComm.BtState != BT_ARM_GPS_MODE && VarsComm.BtState != BT_ARM_RAW_MODE)
+ // don't overwrite this byte when we are in DATA GPS or RAW mode
+ if ((VarsComm.BtState == BT_ARM_CMD_MODE) || (IOMapComm.BtDataMode == DATA_MODE_NXT))
IOMapComm.BtInBuf.Buf[BT_CMD_BYTE] = 0;
@@ -427,8 +437,10 @@ UWORD cCommInterprete(UBYTE *pInBuf, UBYTE *pOutBuf, UBYTE *pLength, UBYTE C
case REPLY_CMD:
{
- /* If this is a reply to a direct command opcode, pRCHandler will handle it */
- if (pInBuf[1] < NUM_RC_OPCODES)
+ // in the enhanced firmware all replies (system or direct) go to the RC Handler function
+ // since it stores the last response in VarsCmd.LastResponseBuffer field
+// /* If this is a reply to a direct command opcode, pRCHandler will handle it */
+// if (pInBuf[1] < NUM_RC_OPCODES)
pMapCmd->pRCHandler(&(pInBuf[0]), NULL, pLength);
/* No Reply ever required on REPLY_CMD messages */
@@ -478,6 +490,9 @@ UWORD cCommInterprete(UBYTE *pInBuf, UBYTE *pOutBuf, UBYTE *pLength, UBYTE C
break;
case REPLY_CMD:
{
+ // in the enhanced firmware all replies (system or direct) go to the RC Handler function
+ // since it stores the last response in VarsCmd.LastResponseBuffer field
+ pMapCmd->pRCHandler(&(pInBuf[0]), NULL, pLength);
}
break;
default:
@@ -671,13 +686,19 @@ UWORD cCommInterpreteCmd(UBYTE Cmd, UBYTE *pInBuf, UBYTE *pOutBuf, UBYTE *pL
Length = FileLength;
/* Here test for channel - USB can only handle a 64 byte return (- wrapping )*/
- if (CmdBit & USB_CMD_READY)
+ if ((CmdBit & BT_CMD_READY) != BT_CMD_READY)
{
- if (FileLength > (SIZE_OF_USBBUF - 6))
+ // USB or HS
+ UBYTE bufSize;
+ if (CmdBit & USB_CMD_READY)
+ bufSize = SIZE_OF_USBBUF;
+ else
+ bufSize = SIZE_OF_HSBUF;
+ if (FileLength > (bufSize - 6))
{
/* Buffer cannot hold the requested data adjust to buffer size */
- FileLength = (SIZE_OF_USBBUF - 6);
+ FileLength = (bufSize - 6);
}
*pLength = FileLength + 4;
Status = pMapLoader->pFunc(READ, &pInBuf[1], &pOutBuf[4], &FileLength);
@@ -890,13 +911,18 @@ UWORD cCommInterpreteCmd(UBYTE Cmd, UBYTE *pInBuf, UBYTE *pOutBuf, UBYTE *pL
FileLength <<= 8;
FileLength |= pInBuf[7];
- if (CmdBit & USB_CMD_READY)
+ if (!(CmdBit & BT_CMD_READY))
{
+ UBYTE bufSize;
+ if (CmdBit & USB_CMD_READY)
+ bufSize = SIZE_OF_USBBUF;
+ else
+ bufSize = SIZE_OF_HSBUF;
- /* test for USB buffer overrun */
- if (FileLength > (SIZE_OF_USBBUF - 9))
+ /* test for USB or HS buffer overrun */
+ if (FileLength > (bufSize - 9))
{
- FileLength = SIZE_OF_USBBUF - 9;
+ FileLength = bufSize - 9;
}
}
else
@@ -1097,10 +1123,14 @@ UWORD cCommInterpreteCmd(UBYTE Cmd, UBYTE *pInBuf, UBYTE *pOutBuf, UBYTE *pL
{
MaxBufData = (SIZE_OF_USBDATA - 5); /* Substract wrapping */
}
- else
+ else if (CmdBit & BT_CMD_READY)
{
MaxBufData = (SIZE_OF_BTBUF - 7); /* Substract wrapping + length bytes for BT*/
}
+ else // HS_CMD_READY
+ {
+ MaxBufData = (SIZE_OF_HSBUF - 5); /* Substract wrapping */
+ }
if (0x00 == pInBuf[1])
{
@@ -1157,12 +1187,23 @@ UWORD cCommInterpreteCmd(UBYTE Cmd, UBYTE *pInBuf, UBYTE *pOutBuf, UBYTE *pL
(*pLength) += 3; /* Add 3 bytes for the status byte, length byte and Buf no */
}
break;
+
+ case RENAMEFILE:
+ {
+ Status = pMapLoader->pFunc(RENAMEFILE, &pInBuf[1], &pInBuf[21], &FileLength);
+ pOutBuf[0] = LOADER_ERR_BYTE(Status);
+ pOutBuf[1] = LOADER_HANDLE(Status);
+ cCommCopyFileName(&pOutBuf[2], &pInBuf[1]);
+ cCommCopyFileName(&pOutBuf[22], &pInBuf[21]);
+ *pLength = 42;
+ }
+ break;
case BTFACTORYRESET:
{
UWORD RtnVal;
- if (CmdBit & USB_CMD_READY)
+ if ((CmdBit & USB_CMD_READY) || (CmdBit & HS_CMD_READY))
{
if (SUCCESS == cCommReq(FACTORYRESET, 0, 0, 0, NULL, &RtnVal))
{
@@ -1202,7 +1243,7 @@ UWORD cCommReceivedBtData(void)
UWORD BytesToGo;
UWORD RtnVal;
- RtnVal = dBtReceivedData(&NumberOfBytes, &BytesToGo);
+ RtnVal = dBtReceivedData(&NumberOfBytes, &BytesToGo, IOMapComm.BtDataMode != DATA_MODE_NXT);
if (TRUE == RtnVal)
{
@@ -1226,9 +1267,9 @@ UWORD cCommReceivedBtData(void)
/* ActiveUpdate has to be idle because BC4 can send stream data even if CMD */
/* mode has been requested - dont try to interprete the data */
/* VarsComm.CmdSwitchCnt != 0 if a transition to Cmd mode is in process */
- if (0 == VarsComm.CmdSwitchCnt)
+ if ((VarsComm.BtState == BT_ARM_DATA_MODE) && (0 == VarsComm.CmdSwitchCnt))
{
- if (VarsComm.BtState == BT_ARM_DATA_MODE)
+ if (IOMapComm.BtDataMode == DATA_MODE_NXT)
{
/* Move the inptr ahead */
@@ -1240,20 +1281,20 @@ UWORD cCommReceivedBtData(void)
/* call the data stream interpreter */
cCommInterprete(IOMapComm.BtInBuf.Buf, IOMapComm.BtOutBuf.Buf, &(IOMapComm.BtOutBuf.InPtr), (UBYTE) BT_CMD_READY, BytesToGo);
- /* if there is a reply to be sent then send it */
+ /* if there is a reply to be send then send it */
if (IOMapComm.BtOutBuf.InPtr)
{
dBtSendMsg(IOMapComm.BtOutBuf.Buf, IOMapComm.BtOutBuf.InPtr, IOMapComm.BtOutBuf.InPtr);
IOMapComm.BtOutBuf.InPtr = 0;
}
}
- else if (VarsComm.BtState == BT_ARM_GPS_MODE)
+ else if (IOMapComm.BtDataMode == DATA_MODE_GPS)
{
/* Move the inptr ahead */
IOMapComm.BtInBuf.InPtr = NumberOfBytes;
// interpret GPS sentence?
}
- else if (VarsComm.BtState == BT_ARM_RAW_MODE)
+ else if (IOMapComm.BtDataMode == DATA_MODE_RAW)
{
/* Move the inptr ahead */
IOMapComm.BtInBuf.InPtr = NumberOfBytes;
@@ -1470,18 +1511,45 @@ void cCommReceivedHiSpeedData(void)
if (NumberOfBytes != 0)
{
- for (Tmp = 0; Tmp < NumberOfBytes; Tmp++)
+ if (IOMapComm.HsDataMode != DATA_MODE_NXT)
{
- IOMapComm.HsInBuf.Buf[IOMapComm.HsInBuf.InPtr] = VarsComm.HsModuleInBuf.Buf[Tmp];
- IOMapComm.HsInBuf.InPtr++;
- if (IOMapComm.HsInBuf.InPtr > (SIZE_OF_HSBUF - 1))
+ // this is the normal way to handle incoming hi-speed data
+ for (Tmp = 0; Tmp < NumberOfBytes; Tmp++)
{
- IOMapComm.HsInBuf.InPtr = 0;
+ IOMapComm.HsInBuf.Buf[IOMapComm.HsInBuf.InPtr] = VarsComm.HsModuleInBuf.Buf[Tmp];
+ IOMapComm.HsInBuf.InPtr++;
+ if (IOMapComm.HsInBuf.InPtr > (SIZE_OF_HSBUF - 1))
+ {
+ IOMapComm.HsInBuf.InPtr = 0;
+ }
+ VarsComm.HsModuleInBuf.Buf[Tmp] = 0;
}
- VarsComm.HsModuleInBuf.Buf[Tmp] = 0;
- }
/* Now new data is available from the HIGH SPEED port ! */
+ }
+ else
+ {
+ // receiving hi-speed data in NXT mode
+ /* Copy the bytes into the IOMapBuffer */
+ memcpy((IOMapComm.HsInBuf.Buf), (VarsComm.HsModuleInBuf.Buf), NumberOfBytes);
+
+
+ /* Move the inptr ahead */
+ IOMapComm.HsInBuf.InPtr = NumberOfBytes;
+
+ /* using the outbuf inptr in order to get the number of bytes in the return answer at the right place*/
+ IOMapComm.HsOutBuf.InPtr = NumberOfBytes;
+
+ /* call the data stream interpreter */
+ cCommInterprete(IOMapComm.HsInBuf.Buf, IOMapComm.HsOutBuf.Buf, &(IOMapComm.HsOutBuf.InPtr), (UBYTE) HS_CMD_READY, NumberOfBytes);
+
+ /* if there is a reply to be sent then send it */
+ if (IOMapComm.HsOutBuf.InPtr)
+ {
+ dHiSpeedSendData(IOMapComm.HsOutBuf.Buf, IOMapComm.HsOutBuf.InPtr);
+ IOMapComm.HsOutBuf.InPtr = 0;
+ }
+ }
}
}
@@ -1857,7 +1925,7 @@ void cCommUpdateBt(void)
{
IOMapComm.BtConnectTable[(VarsComm.BtUpdateDataConnectNr & ~0x80)].StreamStatus = 1;
*(VarsComm.pRetVal) = SUCCESS;
- SETBtDataState(IOMapComm.BtState);
+ SETBtDataState;
SETBtStateIdle;
}
}
@@ -2653,7 +2721,7 @@ void cCommUpdateBt(void)
{
IOMapComm.BtConnectTable[0].StreamStatus = 1;
*(VarsComm.pRetVal) = SUCCESS;
- SETBtDataState(IOMapComm.BtState);
+ SETBtDataState;
SETBtStateIdle;
}
}
@@ -3335,7 +3403,7 @@ void cCommsOpenStream(UBYTE *pNextState)
{
if (VarsComm.BtBcPinLevel)
{
- SETBtDataState(IOMapComm.BtState);
+ SETBtDataState;
IOMapComm.BtConnectTable[VarsComm.BtCmdData.ParamTwo].StreamStatus = 1;
VarsComm.StreamStateCnt = 0;
(*pNextState)++;
diff --git a/AT91SAM7S256/Source/c_comm.iom b/AT91SAM7S256/Source/c_comm.iom
index 0f6648b..98edff7 100644
--- a/AT91SAM7S256/Source/c_comm.iom
+++ b/AT91SAM7S256/Source/c_comm.iom
@@ -38,10 +38,15 @@ enum
BT_ARM_OFF,
BT_ARM_CMD_MODE,
BT_ARM_DATA_MODE,
- BT_ARM_GPS_MODE,
- BT_ARM_RAW_MODE
};
+// Constants refering to BtDataMode and HsDataMode
+#define DATA_MODE_NXT 0x00
+#define DATA_MODE_GPS 0x01
+#define DATA_MODE_RAW 0x02
+#define DATA_MODE_MASK 0x07
+#define DATA_MODE_UPDATE 0x08
+
//Constant reffering to BtStateStatus
#define BT_BRICK_VISIBILITY 0x01
#define BT_BRICK_PORT_OPEN 0x02
@@ -267,11 +272,12 @@ typedef struct
UBYTE HsState;
UBYTE UsbState;
+ UBYTE Spare1; // fill the vacant byte that would otherwise be here
UWORD HsMode;
- UBYTE BtState; // off, cmd, data, gps, raw
- UBYTE Unused1;
+ UBYTE BtDataMode; // nxt, gps, raw
+ UBYTE HsDataMode; // nxt, gps, raw
}IOMAPCOMM;
diff --git a/AT91SAM7S256/Source/c_display.c b/AT91SAM7S256/Source/c_display.c
index 0c6f1b2..1732667 100644
--- a/AT91SAM7S256/Source/c_display.c
+++ b/AT91SAM7S256/Source/c_display.c
@@ -336,8 +336,7 @@ void cDisplayLineX(UBYTE X1, UBYTE X2, UBYTE Y, UBYTE PixelMode)
M = 1 << (Y % 8);
Y >>= 3;
-
- for (X=X1; X<=X2; X++)
+ for (X = X1;X <= X2;X++)
{
switch (PixelMode)
{
@@ -461,6 +460,7 @@ void cDisplayFillScreen(SCREEN_CORDINATE *pCord, UBYTE PixelMode)
}
}
+
void cDisplayDraw(UBYTE Cmd,UBYTE PixelMode,UBYTE X1,UBYTE Y1,UBYTE X2,UBYTE Y2)
{
SCREEN_CORDINATE Coord;
diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c
index 2786326..4c4dc01 100644
--- a/AT91SAM7S256/Source/c_input.c
+++ b/AT91SAM7S256/Source/c_input.c
@@ -2,11 +2,11 @@
//
// Date init 14.12.2004
//
-// Revision date $Date:: 3/21/09 10:31a $
+// Revision date $Date:: 19-03-10 12:36 $
//
// Filename $Workfile:: c_input.c $
//
-// Version $Revision:: 39 $
+// Version $Revision:: 40 $
//
// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_inpu $
//
@@ -263,9 +263,12 @@ void cInputCtrl(void)
/* A type change has been carried out earlier - waiting for valid data */
/* The color sensor requires special startup sequence with communication */
- if ((sType == COLORFULL) || (sType == COLORRED) ||
- (sType == COLORGREEN)|| (sType == COLORBLUE) ||
- (sType == COLOREXIT) || (sType == COLORNONE))
+ if ((sType == COLORFULL) ||
+ (sType == COLORRED) ||
+ (sType == COLORGREEN)||
+ (sType == COLORBLUE) ||
+ (sType == COLOREXIT) ||
+ (sType == COLORNONE))
{
cInputCalcSensorValues(Tmp);
}
@@ -359,10 +362,11 @@ void cInputCalcSensorValues(UBYTE No)
&(VarsInput.EdgeCnt[No]),
((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
+
}
break;
- /* Triple case intended */
+ /* Tripple case intended */
case LOWSPEED:
case LOWSPEED_9V:
case HIGHSPEED:
@@ -382,11 +386,6 @@ void cInputCalcSensorValues(UBYTE No)
{
case SENSOROFF:
{
-
- /* Make sure that sensor data are invalid while unplugged*/
- VarsInput.InvalidTimer[No] = INVALID_RELOAD_COLOR;
- IOMapInput.Inputs[No].InvalidData = INVALID_DATA;
-
/* Check if sensor has been attached */
if (dInputCheckColorStatus(No))
{
@@ -449,11 +448,6 @@ void cInputCalcSensorValues(UBYTE No)
{
case SENSOROFF:
{
-
- /* Make sure that sensor data are invalid while unplugged */
- VarsInput.InvalidTimer[No] = INVALID_RELOAD_COLOR;
- IOMapInput.Inputs[No].InvalidData = INVALID_DATA;
-
/* Check if sensor has been attached */
if (dInputCheckColorStatus(No))
{
@@ -1109,6 +1103,8 @@ void cInputSetupType(UBYTE Port, UBYTE newType, UBYTE OldType)
dInputSetDirInDigi1(Port);
IOMapInput.Colors[Port].CalibrationState = SENSORCAL;
VarsInput.VarsColor[Port].ColorInitState = 0;
+
+ IOMapInput.Inputs[Port].SensorValue = BLACKCOLOR;
}
break;
diff --git a/AT91SAM7S256/Source/c_loader.c b/AT91SAM7S256/Source/c_loader.c
index d6ee342..b9f290f 100644
--- a/AT91SAM7S256/Source/c_loader.c
+++ b/AT91SAM7S256/Source/c_loader.c
@@ -312,10 +312,7 @@ UWORD cLoaderFileRq(UBYTE Cmd, UBYTE *pFileName, UBYTE *pBuffer, ULONG *pLen
case DELETE:
{
ReturnState = cLoaderDeleteFile(pFileName);
-/*
- ReturnState = dLoaderDelete(pFileName);
- IOMapLoader.FreeUserFlash = dLoaderReturnFreeUserFlash();
-*/
+
}
break;
case DELETEUSERFLASH:
diff --git a/AT91SAM7S256/Source/c_loader.iom b/AT91SAM7S256/Source/c_loader.iom
index 6ad2d53..c0549b1 100644
--- a/AT91SAM7S256/Source/c_loader.iom
+++ b/AT91SAM7S256/Source/c_loader.iom
@@ -1,11 +1,11 @@
//
// Date init 14.12.2004
//
-// Revision date $Date:: 16-12-08 12:11 $
+// Revision date $Date:: 19-03-10 12:36 $
//
// Filename $Workfile:: c_loader.iom $
//
-// Version $Revision:: 14 $
+// Version $Revision:: 17 $
//
// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_load $
//
@@ -21,7 +21,7 @@
//For example, version 1.5 would be 0x0105
//If these switch to little-endian, be sure to update
//definition and usages of VM_OLDEST_COMPATIBLE_VERSION, too!
-#define FIRMWAREVERSION 0x011C //1.28
+#define FIRMWAREVERSION 0x011F //1.31
#define PROTOCOLVERSION 0x017C //1.124
enum
diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c
index ca3c494..212cc89 100644
--- a/AT91SAM7S256/Source/c_output.c
+++ b/AT91SAM7S256/Source/c_output.c
@@ -92,7 +92,7 @@ void cOutputCtrl(void)
pOut->Flags &= ~UPDATE_SPEED;
if (pOut->Mode & MOTORON)
{
- dOutputSetSpeed(Tmp, pOut->RunState, pOut->Speed, pOut->SyncTurnParameter);
+ dOutputSetSpeed (Tmp, pOut->RunState, pOut->Speed, pOut->SyncTurnParameter);
}
}
if (pOut->Flags & UPDATE_TACHO_LIMIT)
diff --git a/AT91SAM7S256/Source/d_loader.c b/AT91SAM7S256/Source/d_loader.c
index 86f428f..99624e8 100644
--- a/AT91SAM7S256/Source/d_loader.c
+++ b/AT91SAM7S256/Source/d_loader.c
@@ -1,11 +1,11 @@
//
// Date init 14.12.2004
//
-// Revision date $Date:: 2-12-08 14:30 $
+// Revision date $Date:: 24-06-09 8:53 $
//
// Filename $Workfile:: d_loader.c $
//
-// Version $Revision:: 16 $
+// Version $Revision:: 18 $
//
// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/d_load $
//
@@ -20,7 +20,7 @@
#include <string.h>
#include <ctype.h>
-#define FILEVERSION (0x0000010CL)
+#define FILEVERSION (0x0000010DL) //(0x0000010CL)
#define MAX_FILES ((FILETABLE_SIZE) - 1) /* Last file entry is used for file version*/
#define FILEVERSIONINDEX ((FILETABLE_SIZE) - 1) /* Last file entry is used for file version*/
@@ -219,7 +219,9 @@ UWORD dLoaderDeleteFilePtr(UWORD Handle)
/* Update the HandleTable[].FileIndex */
for (LongCnt = 0; LongCnt < MAX_HANDLES; LongCnt++)
{
- if ((HandleTable[Handle].FileIndex <= HandleTable[LongCnt].FileIndex) && (FREE != HandleTable[LongCnt].Status))
+
+ /* FileIndex must not be decremented for to the file to be deleted (when Handle = LongCnt)*/
+ if ((HandleTable[Handle].FileIndex < HandleTable[LongCnt].FileIndex) && (FREE != HandleTable[LongCnt].Status))
{
(HandleTable[LongCnt].FileIndex)--;
}
diff --git a/AT91SAM7S256/Source/d_loader.h b/AT91SAM7S256/Source/d_loader.h
index 2f564ba..e5ecb82 100644
--- a/AT91SAM7S256/Source/d_loader.h
+++ b/AT91SAM7S256/Source/d_loader.h
@@ -1,11 +1,11 @@
//
// Date init 14.12.2004
//
-// Revision date $Date:: 3/21/09 10:03a $
+// Revision date $Date:: 24-06-09 12:15 $
//
// Filename $Workfile:: d_loader.h $
//
-// Version $Revision:: 17 $
+// Version $Revision:: 18 $
//
// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/d_load $
//
@@ -18,7 +18,7 @@
#define FILETABLE_SIZE ((2 * SECTORSIZE)/4)
#define STARTOFFILETABLE (0x140000L - (FILETABLE_SIZE*4))
#define FILEPTRTABLE ((const ULONG*)(0x140000L - (FILETABLE_SIZE*4)))
-#define STARTOFUSERFLASH (0x125800L)//(0x124600L)
+#define STARTOFUSERFLASH (0x125800L)//(0x124600L) 1.31 == (0x122100L)
#define SIZEOFUSERFLASH ((ULONG)STARTOFFILETABLE - STARTOFUSERFLASH)
#define SIZEOFFLASH 262144L
diff --git a/AT91SAM7S256/Source/d_lowspeed.r b/AT91SAM7S256/Source/d_lowspeed.r
index 3ca660d..30e71b8 100644
--- a/AT91SAM7S256/Source/d_lowspeed.r
+++ b/AT91SAM7S256/Source/d_lowspeed.r
@@ -144,87 +144,121 @@ const ULONG CLK_OR_DATA_PINS[4] = {CHANNEL_ONE_CLK | CHANNEL_ONE_DATA,
#define SETDebugOutputLow *AT91C_PIOA_CODR = 0x20000000
+
#define SETClkComOneHigh *AT91C_PIOA_SODR = CHANNEL_ONE_CLK
+
#define SETClkComOneLow *AT91C_PIOA_CODR = CHANNEL_ONE_CLK
+
#define GetClkComOnePinLevel *AT91C_PIOA_PDSR & CHANNEL_ONE_CLK
#define SETClkComTwoHigh *AT91C_PIOA_SODR = CHANNEL_TWO_CLK
+
#define SETClkComTwoLow *AT91C_PIOA_CODR = CHANNEL_TWO_CLK
+
#define GetClkComTwoPinLevel *AT91C_PIOA_PDSR & CHANNEL_TWO_CLK
#define SETClkComThreeHigh *AT91C_PIOA_SODR = CHANNEL_THREE_CLK
+
#define SETClkComThreeLow *AT91C_PIOA_CODR = CHANNEL_THREE_CLK
+
#define GetClkComThreePinLevel *AT91C_PIOA_PDSR & CHANNEL_THREE_CLK
#define SETClkComFourHigh *AT91C_PIOA_SODR = CHANNEL_FOUR_CLK
+
#define SETClkComFourLow *AT91C_PIOA_CODR = CHANNEL_FOUR_CLK
+
#define GetClkComFourPinLevel *AT91C_PIOA_PDSR & CHANNEL_FOUR_CLK
+
#define SETDataComOneHigh *AT91C_PIOA_SODR = CHANNEL_ONE_DATA
+
#define SETDataComOneLow *AT91C_PIOA_CODR = CHANNEL_ONE_DATA
+
#define GetDataComOnePinLevel *AT91C_PIOA_PDSR & CHANNEL_ONE_DATA
+
#define GETDataComOnePinDirection *AT91C_PIOA_OSR & CHANNEL_ONE_DATA
#define SETDataComTwoHigh *AT91C_PIOA_SODR = CHANNEL_TWO_DATA
+
#define SETDataComTwoLow *AT91C_PIOA_CODR = CHANNEL_TWO_DATA
+
#define GetDataComTwoPinLevel *AT91C_PIOA_PDSR & CHANNEL_TWO_DATA
+
#define GETDataComTwoPinDirection *AT91C_PIOA_OSR & CHANNEL_TWO_DATA
#define SETDataComThreeHigh *AT91C_PIOA_SODR = CHANNEL_THREE_DATA
+
#define SETDataComThreeLow *AT91C_PIOA_CODR = CHANNEL_THREE_DATA
+
#define GetDataComThreePinLevel *AT91C_PIOA_PDSR & CHANNEL_THREE_DATA
+
#define GETDataComThreePinDirection *AT91C_PIOA_OSR & CHANNEL_THREE_DATA
#define SETDataComFourHigh *AT91C_PIOA_SODR = CHANNEL_FOUR_DATA
+
#define SETDataComFourLow *AT91C_PIOA_CODR = CHANNEL_FOUR_DATA
+
#define GetDataComFourPinLevel *AT91C_PIOA_PDSR & CHANNEL_FOUR_DATA
+
#define GETDataComFourPinDirection *AT91C_PIOA_OSR & CHANNEL_FOUR_DATA
#define SETDataComOneToInput *AT91C_PIOA_ODR = CHANNEL_ONE_DATA;
+
#define SETDataComOneToOutput *AT91C_PIOA_OER = CHANNEL_ONE_DATA;
#define SETDataComTwoToInput *AT91C_PIOA_ODR = CHANNEL_TWO_DATA;
+
#define SETDataComTwoToOutput *AT91C_PIOA_OER = CHANNEL_TWO_DATA;
#define SETDataComThreeToInput *AT91C_PIOA_ODR = CHANNEL_THREE_DATA;
+
#define SETDataComThreeToOutput *AT91C_PIOA_OER = CHANNEL_THREE_DATA;
#define SETDataComFourToInput *AT91C_PIOA_ODR = CHANNEL_FOUR_DATA;
+
#define SETDataComFourToOutput *AT91C_PIOA_OER = CHANNEL_FOUR_DATA;
#define DISABLEPullupDataComOne *AT91C_PIOA_PPUDR = CHANNEL_ONE_DATA;
+
#define DISABLEPullupClkComOne *AT91C_PIOA_PPUDR = CHANNEL_ONE_CLK;
#define DISABLEPullupDataComTwo *AT91C_PIOA_PPUDR = CHANNEL_TWO_DATA;
+
#define DISABLEPullupClkComTwo *AT91C_PIOA_PPUDR = CHANNEL_TWO_CLK;
#define DISABLEPullupDataComThree *AT91C_PIOA_PPUDR = CHANNEL_THREE_DATA;
+
#define DISABLEPullupClkComThree *AT91C_PIOA_PPUDR = CHANNEL_THREE_CLK;
#define DISABLEPullupDataComFour *AT91C_PIOA_PPUDR = CHANNEL_FOUR_DATA;
+
#define DISABLEPullupClkComFour *AT91C_PIOA_PPUDR = CHANNEL_FOUR_CLK;
#define ENABLEPullupDataComOne *AT91C_PIOA_PPUER = CHANNEL_ONE_DATA;
+
#define ENABLEPullupClkComOne *AT91C_PIOA_PPUER = CHANNEL_ONE_CLK;
#define ENABLEPullupDataComTwo *AT91C_PIOA_PPUER = CHANNEL_TWO_DATA;
+
#define ENABLEPullupClkComTwo *AT91C_PIOA_PPUER = CHANNEL_TWO_CLK;
#define ENABLEPullupDataComThree *AT91C_PIOA_PPUER = CHANNEL_THREE_DATA;
+
#define ENABLEPullupClkComThree *AT91C_PIOA_PPUER = CHANNEL_THREE_CLK;
#define ENABLEPullupDataComFour *AT91C_PIOA_PPUER = CHANNEL_FOUR_DATA;
+
#define ENABLEPullupClkComFour *AT91C_PIOA_PPUER = CHANNEL_FOUR_CLK;
#define SETClkLow(ChannelNr) {\
*AT91C_PIOA_CODR = CLK_PINS[ChannelNr];\
LowSpeedData[ChannelNr].ClkStatus = 0;\
-}
+ }
#define SETClkHigh(ChannelNr) {\
*AT91C_PIOA_SODR = CLK_PINS[ChannelNr];\
LowSpeedData[ChannelNr].ClkStatus = 1;\
-}
+ }
#define SETDataLow(ChannelNr) {\
*AT91C_PIOA_CODR = DATA_PINS[ChannelNr];\
@@ -248,11 +282,11 @@ const ULONG CLK_OR_DATA_PINS[4] = {CHANNEL_ONE_CLK | CHANNEL_ONE_DATA,
#define ENABLEPWMTimerForLowCom {\
*AT91C_PWMC_ENA = AT91C_PWMC_CHID0; /* Enable PWM output channel 0 */\
-}
+ }
#define DISABLEPWMTimerForLowCom {\
*AT91C_PWMC_DIS = AT91C_PWMC_CHID0; /* Disable PWM output channel 0 */\
-}
+ }
#define OLD_DISABLEPWMTimerForLowCom {\
*AT91C_PWMC_DIS = AT91C_PWMC_CHID0; /* Disable PWM output channel 0 */\
@@ -609,7 +643,7 @@ __ramfunc void LowSpeedPwmIrqHandler(void)
LowSpeedData[ChannelNumber].RxState = RX_DATA_CLK_LOW;\
LowSpeedData[ChannelNumber].ReStartBit = (1 - (NoRestart & (1<<ChannelNumber)));\
LowSpeedData[ChannelNumber].RxWaitCnt = 0;\
-}
+ }
#define STATUSTxCom(ChannelNumber, Status) {\
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
index 843db57..c3fdeed 100644
--- a/AT91SAM7S256/Source/d_output.c
+++ b/AT91SAM7S256/Source/d_output.c
@@ -963,6 +963,16 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
SyncData.SyncTachoDif += SyncData.SyncTurnParameter;
+ if (SyncData.SyncTachoDif > 500)
+ {
+ SyncData.SyncTachoDif = 500;
+ }
+ if (SyncData.SyncTachoDif < -500)
+ {
+ SyncData.SyncTachoDif = -500;
+ }
+
+ /*
if ((SWORD)SyncData.SyncTachoDif > 500)
{
SyncData.SyncTachoDif = 500;
@@ -971,6 +981,7 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
{
SyncData.SyncTachoDif = -500;
}
+ */
PValue = (SWORD)SyncData.SyncTachoDif * (SWORD)(pOne->RegPParameter/REG_CONST_DIV);
@@ -1116,140 +1127,6 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
pTwo->MotorRunState = pTwo->RunStateAtLimit;
pTwo->RegulationMode = REGSTATE_IDLE;
}
-/*
- if (MotorNr == MOTOR_A)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne + 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & B
- MotorData[MotorOne].MotorSetSpeed = 0;
- MotorData[MotorOne].MotorTargetSpeed = 0;
- MotorData[MotorOne].MotorActualSpeed = 0;
- MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
- }
- else
- {
- MotorTwo = MotorOne + 2;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & C
- MotorData[MotorOne].MotorSetSpeed = 0;
- MotorData[MotorOne].MotorTargetSpeed = 0;
- MotorData[MotorOne].MotorActualSpeed = 0;
- MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
- }
- else
- {
- //Only Motor A has Sync setting => Stop normal
- MotorData[MotorNr].MotorSetSpeed = 0;
- MotorData[MotorNr].MotorTargetSpeed = 0;
- MotorData[MotorNr].MotorActualSpeed = 0;
- MotorData[MotorNr].MotorRunState = pOne->RunStateAtLimit;
- MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
- }
- }
- }
- if (MotorNr == MOTOR_B)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne - 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & B
- MotorData[MotorOne].MotorSetSpeed = 0;
- MotorData[MotorOne].MotorTargetSpeed = 0;
- MotorData[MotorOne].MotorActualSpeed = 0;
- MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
- }
- MotorTwo = MotorOne + 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor B & C
- MotorData[MotorOne].MotorSetSpeed = 0;
- MotorData[MotorOne].MotorTargetSpeed = 0;
- MotorData[MotorOne].MotorActualSpeed = 0;
- MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
- }
- else
- {
- //Only Motor B has Sync settings => Stop normal
- MotorData[MotorNr].MotorSetSpeed = 0;
- MotorData[MotorNr].MotorTargetSpeed = 0;
- MotorData[MotorNr].MotorActualSpeed = 0;
- MotorData[MotorNr].MotorRunState = pOne->RunStateAtLimit;
- MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
- }
- }
- if (MotorNr == MOTOR_C)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne - 2;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & C
- MotorData[MotorOne].MotorSetSpeed = 0;
- MotorData[MotorOne].MotorTargetSpeed = 0;
- MotorData[MotorOne].MotorActualSpeed = 0;
- MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
- }
- MotorTwo = MotorOne - 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor B & C
- MotorData[MotorOne].MotorSetSpeed = 0;
- MotorData[MotorOne].MotorTargetSpeed = 0;
- MotorData[MotorOne].MotorActualSpeed = 0;
- MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
- }
- else
- {
- //Only Motor C has Sync settings => Stop normal
- MotorData[MotorNr].MotorSetSpeed = 0;
- MotorData[MotorNr].MotorTargetSpeed = 0;
- MotorData[MotorNr].MotorActualSpeed = 0;
- MotorData[MotorNr].MotorRunState = pOne->RunStateAtLimit;
- MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
- }
- }
-*/
}
else
{
@@ -1281,83 +1158,6 @@ void dOutputSyncTachoLimitControl(UBYTE MotorNr)
}
if (MotorTwo == 0xFF)
MotorOne = 0xFF;
-/*
- if (MotorNr == MOTOR_A)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne + 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & B
- }
- else
- {
- MotorTwo = MotorOne + 2;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & C
- }
- else
- {
- //Only Motor A has Sync setting => Stop normal
- MotorOne = 0xFF;
- MotorTwo = 0xFF;
- }
- }
- }
- if (MotorNr == MOTOR_B)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne - 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & B, which has already been called when running throught motor A
- //MotorOne = 0xFF;
- //MotorTwo = 0xFF;
- }
- else
- {
- MotorTwo = MotorOne + 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor B & C
- }
- else
- {
- //Only Motor B has Sync settings => Stop normal
- MotorOne = 0xFF;
- MotorTwo = 0xFF;
- }
- }
- }
- if (MotorNr == MOTOR_C)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne - 2;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & C, which has already been called when running throught motor A
- //MotorOne = 0xFF;
- //MotorTwo = 0xFF;
- }
- else
- {
- MotorTwo = MotorOne - 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor B & C, which has already been called when running throught motor B
- //MotorOne = 0xFF;
- //MotorTwo = 0xFF;
- }
- else
- {
- //Only Motor C has Sync settings => Stop normal
- MotorOne = 0xFF;
- MotorTwo = 0xFF;
- }
- }
- }
-*/
if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
{
MOTORDATA * pOne = &(MotorData[MotorOne]);
@@ -1523,77 +1323,6 @@ void dOutputResetSyncMotors(UBYTE MotorNr)
}
if (MotorTwo == 0xFF)
MotorOne = 0xFF;
-/*
- if (MotorNr == MOTOR_A)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne + 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & B
- }
- else
- {
- MotorTwo = MotorOne + 2;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & C
- }
- else
- {
- //Only Motor A has Sync setting => Stop normal
- MotorOne = 0xFF;
- MotorTwo = 0xFF;
- }
- }
- }
- if (MotorNr == MOTOR_B)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne - 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & B
- }
- else
- {
- MotorTwo = MotorOne + 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor B & C
- }
- else
- {
- //Only Motor B has Sync settings => Stop normal
- MotorOne = 0xFF;
- MotorTwo = 0xFF;
- }
- }
- }
- if (MotorNr == MOTOR_C)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne - 2;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & C
- }
- else
- {
- MotorTwo = MotorOne - 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor B & C
- }
- else
- {
- //Only Motor C has Sync settings => Stop normal
- MotorOne = 0xFF;
- MotorTwo = 0xFF;
- }
- }
- }
-*/
MOTORDATA * pMD = &(MotorData[MotorNr]);
if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
{
@@ -1627,79 +1356,6 @@ void dOutputRampDownSynch(UBYTE MotorNr)
}
if (MotorTwo == 0xFF)
MotorOne = 0xFF;
-/*
- if (MotorNr == MOTOR_A)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne + 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & B
- }
- else
- {
- MotorTwo = MotorOne + 2;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & C
- }
- else
- {
- //Only Motor A has Sync setting => Stop normal
- MotorOne = 0xFF;
- MotorTwo = 0xFF;
- }
- }
- }
- if (MotorNr == MOTOR_B)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne - 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & B, which has already been called when running throught motor A
- //MotorOne = 0xFF;
- //MotorTwo = 0xFF;
- }
- else
- {
- MotorTwo = MotorOne + 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor B & C
- }
- else
- {
- //Only Motor B has Sync settings => Stop normal
- MotorOne = 0xFF;
- MotorTwo = 0xFF;
- }
- }
- }
- if (MotorNr == MOTOR_C)
- {
- MotorOne = MotorNr;
- MotorTwo = MotorOne - 2;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor A & C, which has already been called when running throught motor A
- }
- else
- {
- MotorTwo = MotorOne - 1;
- if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
- {
- //Synchronise motor B & C,, which has already been called when running throught motor B
- }
- else
- {
- //Only Motor C has Sync settings => Stop normal
- MotorOne = 0xFF;
- MotorTwo = 0xFF;
- }
- }
- }
-*/
if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
{
MOTORDATA * pOne = &(MotorData[MotorOne]);