aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256/Source
diff options
context:
space:
mode:
authorJohn Hansen2011-01-21 00:44:37 +0100
committerNicolas Schodet2011-01-21 23:15:36 +0100
commit13f8dd7556fd756722cd669c1f5d5b9311a49777 (patch)
treed7b241206e7587a8697613881bf3908ace7b98a7 /AT91SAM7S256/Source
parentd6bd4ec76414e535a297e37aef9de066c20b7e10 (diff)
replace many array indexes with pointer access
Imported from NXT Enhanced Firmware.
Diffstat (limited to 'AT91SAM7S256/Source')
-rw-r--r--AT91SAM7S256/Source/c_cmd.c213
-rw-r--r--AT91SAM7S256/Source/c_input.c19
-rw-r--r--AT91SAM7S256/Source/c_output.c141
3 files changed, 196 insertions, 177 deletions
diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c
index fc74b3a..6f562ca 100644
--- a/AT91SAM7S256/Source/c_cmd.c
+++ b/AT91SAM7S256/Source/c_cmd.c
@@ -461,32 +461,34 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
case RC_SET_OUT_STATE:
{
+ UBYTE Port = pInBuf[1];
//Don't do anything if illegal port specification is made
- if (pInBuf[1] >= NO_OF_OUTPUTS && pInBuf[1] != 0xFF)
+ if (Port >= NO_OF_OUTPUTS && Port != 0xFF)
{
RCStatus = ERR_RC_ILLEGAL_VAL;
break;
}
//0xFF is protocol defined to mean "all ports".
- if (pInBuf[1] == 0xFF)
+ if (Port == 0xFF)
{
FirstPort = 0;
LastPort = NO_OF_OUTPUTS - 1;
}
else
- FirstPort = LastPort = pInBuf[1];
+ FirstPort = LastPort = Port;
for (i = FirstPort; i <= LastPort; i++)
{
- pMapOutPut->Outputs[i].Speed = pInBuf[2];
- pMapOutPut->Outputs[i].Mode = pInBuf[3];
- pMapOutPut->Outputs[i].RegMode = pInBuf[4];
- pMapOutPut->Outputs[i].SyncTurnParameter = pInBuf[5];
- pMapOutPut->Outputs[i].RunState = pInBuf[6];
- memcpy((PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), (PSZ)(&pInBuf[7]), 4);
-
- pMapOutPut->Outputs[i].Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT;
+ OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
+ pOut->Speed = pInBuf[2];
+ pOut->Mode = pInBuf[3];
+ pOut->RegMode = pInBuf[4];
+ pOut->SyncTurnParameter = pInBuf[5];
+ pOut->RunState = pInBuf[6];
+ memcpy((PSZ)(&(pOut->TachoLimit)), (PSZ)(&pInBuf[7]), 4);
+
+ pOut->Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT;
}
}
break;
@@ -502,12 +504,13 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
RCStatus = ERR_RC_ILLEGAL_VAL;
break;
}
+ INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
- pMapInput->Inputs[i].SensorType = pInBuf[2];
- pMapInput->Inputs[i].SensorMode = pInBuf[3];
+ pIn->SensorType = pInBuf[2];
+ pIn->SensorMode = pInBuf[3];
//Set InvalidData flag automatically since type may have changed
- pMapInput->Inputs[i].InvalidData = TRUE;
+ pIn->InvalidData = TRUE;
}
break;
@@ -525,45 +528,46 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
ResponseLen += 22;
break;
}
+ OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
//Echo port
pOutBuf[ResponseLen] = i;
ResponseLen++;
//Power
- pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Speed;
+ pOutBuf[ResponseLen] = pOut->Speed;
ResponseLen++;
//Mode
- pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Mode;
+ pOutBuf[ResponseLen] = pOut->Mode;
ResponseLen++;
//RegMode
- pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RegMode;
+ pOutBuf[ResponseLen] = pOut->RegMode;
ResponseLen++;
//TurnRatio
- pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].SyncTurnParameter;
+ pOutBuf[ResponseLen] = pOut->SyncTurnParameter;
ResponseLen++;
//RunState
- pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RunState;
+ pOutBuf[ResponseLen] = pOut->RunState;
ResponseLen++;
//TachoLimit ULONG
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), 4);
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->TachoLimit)), 4);
ResponseLen += 4;
//TachoCount SLONG
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoCnt)), 4);
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->TachoCnt)), 4);
ResponseLen += 4;
//BlockTachoCount SLONG
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].BlockTachoCount)), 4);
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->BlockTachoCount)), 4);
ResponseLen += 4;
//RotationCount SLONG
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].RotationCount)), 4);
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->RotationCount)), 4);
ResponseLen += 4;
NXT_ASSERT(ResponseLen == 23);
@@ -590,8 +594,10 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
pOutBuf[ResponseLen] = i;
ResponseLen++;
+ INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
+
//Set "Valid?" boolean
- if (pMapInput->Inputs[i].InvalidData)
+ if (pIn->InvalidData)
pOutBuf[ResponseLen] = FALSE;
else
pOutBuf[ResponseLen] = TRUE;
@@ -603,24 +609,24 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
pOutBuf[ResponseLen] = FALSE;
ResponseLen++;
- pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorType;
+ pOutBuf[ResponseLen] = pIn->SensorType;
ResponseLen++;
- pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorMode;
+ pOutBuf[ResponseLen] = pIn->SensorMode;
ResponseLen++;
//Set Raw, Normalized, and Scaled values
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].ADRaw)), 2);
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->ADRaw)), 2);
ResponseLen += 2;
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorRaw)), 2);
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorRaw)), 2);
ResponseLen += 2;
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorValue)), 2);
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorValue)), 2);
ResponseLen += 2;
//!!! Return normalized raw value in place of calibrated value for now -- see comment above
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorRaw)), 2);
+ memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorRaw)), 2);
ResponseLen += 2;
NXT_ASSERT(ResponseLen == 14);
@@ -683,14 +689,7 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
//pInBuf[2] is a selector
//FALSE: Position relative to start of last program
//TRUE: Position relative to start of last motor control block
- if (pInBuf[2] == FALSE)
- {
- pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_ROTATION_COUNT;
- }
- else
- {
- pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_BLOCK_COUNT;
- }
+ pMapOutPut->Outputs[i].Flags |= (pInBuf[2] ? UPDATE_RESET_BLOCK_COUNT : UPDATE_RESET_ROTATION_COUNT);
}
break;
@@ -1092,32 +1091,34 @@ void cCmdInit(void* pHeader)
//Initialize IO_PTRS_OUT
for (i = 0; i < NO_OF_OUTPUTS; i++)
{
- IO_PTRS_OUT[IO_OUT_FLAGS + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Flags);
- IO_PTRS_OUT[IO_OUT_MODE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Mode);
- IO_PTRS_OUT[IO_OUT_SPEED + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Speed);
- IO_PTRS_OUT[IO_OUT_ACTUAL_SPEED + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].ActualSpeed);
- IO_PTRS_OUT[IO_OUT_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].TachoCnt);
- IO_PTRS_OUT[IO_OUT_TACH_LIMIT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].TachoLimit);
- IO_PTRS_OUT[IO_OUT_RUN_STATE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RunState);
- IO_PTRS_OUT[IO_OUT_TURN_RATIO + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].SyncTurnParameter);
- IO_PTRS_OUT[IO_OUT_REG_MODE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegMode);
- IO_PTRS_OUT[IO_OUT_OVERLOAD + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Overloaded);
- IO_PTRS_OUT[IO_OUT_REG_P_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegPParameter);
- IO_PTRS_OUT[IO_OUT_REG_I_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegIParameter);
- IO_PTRS_OUT[IO_OUT_REG_D_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegDParameter);
- IO_PTRS_OUT[IO_OUT_BLOCK_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].BlockTachoCount);
- IO_PTRS_OUT[IO_OUT_ROTATION_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RotationCount);
+ OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
+ IO_PTRS_OUT[IO_OUT_FLAGS + i * IO_OUT_FPP] = (void*)&(pOut->Flags);
+ IO_PTRS_OUT[IO_OUT_MODE + i * IO_OUT_FPP] = (void*)&(pOut->Mode);
+ IO_PTRS_OUT[IO_OUT_SPEED + i * IO_OUT_FPP] = (void*)&(pOut->Speed);
+ IO_PTRS_OUT[IO_OUT_ACTUAL_SPEED + i * IO_OUT_FPP] = (void*)&(pOut->ActualSpeed);
+ IO_PTRS_OUT[IO_OUT_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->TachoCnt);
+ IO_PTRS_OUT[IO_OUT_TACH_LIMIT + i * IO_OUT_FPP] = (void*)&(pOut->TachoLimit);
+ IO_PTRS_OUT[IO_OUT_RUN_STATE + i * IO_OUT_FPP] = (void*)&(pOut->RunState);
+ IO_PTRS_OUT[IO_OUT_TURN_RATIO + i * IO_OUT_FPP] = (void*)&(pOut->SyncTurnParameter);
+ IO_PTRS_OUT[IO_OUT_REG_MODE + i * IO_OUT_FPP] = (void*)&(pOut->RegMode);
+ IO_PTRS_OUT[IO_OUT_OVERLOAD + i * IO_OUT_FPP] = (void*)&(pOut->Overloaded);
+ IO_PTRS_OUT[IO_OUT_REG_P_VAL + i * IO_OUT_FPP] = (void*)&(pOut->RegPParameter);
+ IO_PTRS_OUT[IO_OUT_REG_I_VAL + i * IO_OUT_FPP] = (void*)&(pOut->RegIParameter);
+ IO_PTRS_OUT[IO_OUT_REG_D_VAL + i * IO_OUT_FPP] = (void*)&(pOut->RegDParameter);
+ IO_PTRS_OUT[IO_OUT_BLOCK_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->BlockTachoCount);
+ IO_PTRS_OUT[IO_OUT_ROTATION_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->RotationCount);
}
//Initialize IO_PTRS_IN
for (i = 0; i < NO_OF_INPUTS; i++)
{
- IO_PTRS_IN[IO_IN_TYPE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorType);
- IO_PTRS_IN[IO_IN_MODE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorMode);
- IO_PTRS_IN[IO_IN_ADRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].ADRaw);
- IO_PTRS_IN[IO_IN_NORMRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorRaw);
- IO_PTRS_IN[IO_IN_SCALEDVAL + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorValue);
- IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].InvalidData);
+ INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
+ IO_PTRS_IN[IO_IN_TYPE + i * IO_IN_FPP] = (void*)&(pIn->SensorType);
+ IO_PTRS_IN[IO_IN_MODE + i * IO_IN_FPP] = (void*)&(pIn->SensorMode);
+ IO_PTRS_IN[IO_IN_ADRAW + i * IO_IN_FPP] = (void*)&(pIn->ADRaw);
+ IO_PTRS_IN[IO_IN_NORMRAW + i * IO_IN_FPP] = (void*)&(pIn->SensorRaw);
+ IO_PTRS_IN[IO_IN_SCALEDVAL + i * IO_IN_FPP] = (void*)&(pIn->SensorValue);
+ IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pIn->InvalidData);
}
//Clear memory pool and initialize VarsCmd (cCmdDeactivateProgram effectively re-inits VarsCmd)
@@ -1962,29 +1963,31 @@ void cCmdResetDevices(void)
for (i = 0; i < NO_OF_INPUTS; i++)
{
+ INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
//Clear type and mode to defaults
- pMapInput->Inputs[i].SensorType = NO_SENSOR;
- pMapInput->Inputs[i].SensorMode = RAWMODE;
+ pIn->SensorType = NO_SENSOR;
+ pIn->SensorMode = RAWMODE;
//Reset input values to 0 prior to running (clear things like stale rotation counts)
- pMapInput->Inputs[i].ADRaw = 0;
- pMapInput->Inputs[i].SensorRaw = 0;
- pMapInput->Inputs[i].SensorValue = 0;
+ pIn->ADRaw = 0;
+ pIn->SensorRaw = 0;
+ pIn->SensorValue = 0;
//Assert invalid data flag so future code is aware of these changes
- pMapInput->Inputs[i].InvalidData = TRUE;
+ pIn->InvalidData = TRUE;
}
for (i = 0; i < NO_OF_OUTPUTS; i++)
{
//Coast and reset all motor parameters
- pMapOutPut->Outputs[i].Mode = 0;
- pMapOutPut->Outputs[i].RegMode = REGULATION_MODE_IDLE;
- pMapOutPut->Outputs[i].RunState = MOTOR_RUN_STATE_IDLE;
- pMapOutPut->Outputs[i].Speed = 0;
- pMapOutPut->Outputs[i].TachoLimit = 0;
- pMapOutPut->Outputs[i].SyncTurnParameter = 0;
- pMapOutPut->Outputs[i].Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT | UPDATE_RESET_COUNT | UPDATE_RESET_BLOCK_COUNT | UPDATE_RESET_ROTATION_COUNT;
+ OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
+ pOut->Mode = 0;
+ pOut->RegMode = REGULATION_MODE_IDLE;
+ pOut->RunState = MOTOR_RUN_STATE_IDLE;
+ pOut->Speed = 0;
+ pOut->TachoLimit = 0;
+ pOut->SyncTurnParameter = 0;
+ pOut->Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT | UPDATE_RESET_COUNT | UPDATE_RESET_BLOCK_COUNT | UPDATE_RESET_ROTATION_COUNT;
}
//Lowspeed init, INSERT CODE !!!
@@ -3243,25 +3246,27 @@ NXT_STATUS cCmdDatalogRead(UBYTE * pBuffer, UWORD Length, UBYTE Remove)
NXT_STATUS cCmdColorSensorRead (UBYTE Port, SWORD * SensorValue, UWORD * RawArray, UWORD * NormalizedArray,
SWORD * ScaledArray, UBYTE * InvalidData)
{
- ULONG i;
+ ULONG i;
//Make sure Port is valid for Color Sensor
- if (!(pMapInput->Inputs[Port].SensorType == COLORFULL || pMapInput->Inputs[Port].SensorType == COLORRED
- || pMapInput->Inputs[Port].SensorType == COLORGREEN || pMapInput->Inputs[Port].SensorType == COLORBLUE
- || pMapInput->Inputs[Port].SensorType == COLORNONE))
+ INPUTSTRUCT * pIn = &(pMapInput->Inputs[Port]);
+ UBYTE sType = pIn->SensorType;
+ if (!(sType == COLORFULL || sType == COLORRED || sType == COLORGREEN ||
+ sType == COLORBLUE || sType == COLORNONE))
{
return (ERR_COMM_CHAN_NOT_READY); //TODO - is this the right error?
}
//Copy Detected Color
- *SensorValue = pMapInput->Inputs[Port].SensorValue;
+ *SensorValue = pIn->SensorValue;
//Copy all raw, normalized and scaled data from I/O Map
for (i=0; i<NO_OF_COLORS; i++){
- RawArray[i] = pMapInput->Colors[Port].ADRaw[i];
- NormalizedArray[i] = pMapInput->Colors[Port].SensorRaw[i];
- ScaledArray[i] = pMapInput->Colors[Port].SensorValue[i];
+ COLORSTRUCT * pColor = &(pMapInput->Colors[Port]);
+ RawArray[i] = pColor->ADRaw[i];
+ NormalizedArray[i] = pColor->SensorRaw[i];
+ ScaledArray[i] = pColor->SensorValue[i];
}
//Copy the Invalid Data Flag
- *InvalidData = pMapInput->Inputs[Port].InvalidData;
+ *InvalidData = pIn->InvalidData;
return NO_ERR;
@@ -6326,10 +6331,12 @@ NXT_STATUS cCmdLSCheckStatus(UBYTE Port)
return (ERR_COMM_CHAN_INVALID);
}
+ INPUTSTRUCT * pInput = &(pMapInput->Inputs[Port]);
+
//If port is not configured properly ahead of time, report that error
//!!! This seems like the right policy, but may restrict otherwise valid read operations...
- if (!(pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED)
- || !(pMapInput->Inputs[Port].InvalidData == FALSE))
+ if (!(pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED)
+ || !(pInput->InvalidData == FALSE))
{
return (ERR_COMM_CHAN_NOT_READY);
}
@@ -6346,13 +6353,15 @@ UBYTE cCmdLSCalcBytesReady(UBYTE Port)
//Expect callers to validate Port, but short circuit here to be safe.
if (Port >= NO_OF_LOWSPEED_COM_CHANNEL)
return 0;
+
+ LSBUF * pInBuf = &(pMapLowSpeed->InBuf[Port]);
//Normally, bytes available is a simple difference.
- Tmp = pMapLowSpeed->InBuf[Port].InPtr - pMapLowSpeed->InBuf[Port].OutPtr;
+ Tmp = pInBuf->InPtr - pInBuf->OutPtr;
//If InPtr is actually behind OutPtr, circular buffer has wrapped. Account for wrappage...
if (Tmp < 0)
- Tmp = (pMapLowSpeed->InBuf[Port].InPtr + (SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr));
+ Tmp = (pInBuf->InPtr + (SIZE_OF_LSBUF - pInBuf->OutPtr));
return (UBYTE)(Tmp);
}
@@ -6371,20 +6380,24 @@ NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseL
return (ERR_INVALID_SIZE);
}
+ INPUTSTRUCT * pInput = &(pMapInput->Inputs[Port]);
+ UBYTE * pChState = &(pMapLowSpeed->ChannelState[Port]);
+ LSBUF * pOutBuf = &(pMapLowSpeed->OutBuf[Port]);
+
//Only start writing process if port is properly configured and c_lowspeed module is ready
- if ((pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED)
- && (pMapInput->Inputs[Port].InvalidData == FALSE)
- && (pMapLowSpeed->ChannelState[Port] == LOWSPEED_IDLE) || (pMapLowSpeed->ChannelState[Port] == LOWSPEED_ERROR))
+ if ((pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED)
+ && (pInput->InvalidData == FALSE)
+ && (*pChState == LOWSPEED_IDLE) || (*pChState == LOWSPEED_ERROR))
{
- pMapLowSpeed->OutBuf[Port].InPtr = 0;
- pMapLowSpeed->OutBuf[Port].OutPtr = 0;
+ pOutBuf->InPtr = 0;
+ pOutBuf->OutPtr = 0;
- memcpy(pMapLowSpeed->OutBuf[Port].Buf, pBuf, BufLength);
- pMapLowSpeed->OutBuf[Port].InPtr = (UBYTE)BufLength;
+ memcpy(pOutBuf->Buf, pBuf, BufLength);
+ pOutBuf->InPtr = (UBYTE)BufLength;
pMapLowSpeed->InBuf[Port].BytesToRx = ResponseLength;
- pMapLowSpeed->ChannelState[Port] = LOWSPEED_INIT;
+ *pChState = LOWSPEED_INIT;
pMapLowSpeed->State |= (COM_CHANNEL_ONE_ACTIVE << Port);
return (NO_ERR);
@@ -6422,18 +6435,20 @@ NXT_STATUS cCmdLSRead(UBYTE Port, UBYTE BufLength, UBYTE * pBuf)
BytesToRead = BufLength;
+ LSBUF * pInBuf = &(pMapLowSpeed->InBuf[Port]);
+
//If the bytes we want to read wrap around the end, we must first read the end, then reset back to the beginning
- if (pMapLowSpeed->InBuf[Port].OutPtr + BytesToRead >= SIZE_OF_LSBUF)
+ if (pInBuf->OutPtr + BytesToRead >= SIZE_OF_LSBUF)
{
- BytesToRead = SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr;
- memcpy(pBuf, pMapLowSpeed->InBuf[Port].Buf + pMapLowSpeed->InBuf[Port].OutPtr, BytesToRead);
- pMapLowSpeed->InBuf[Port].OutPtr = 0;
+ BytesToRead = SIZE_OF_LSBUF - pInBuf->OutPtr;
+ memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead);
+ pInBuf->OutPtr = 0;
pBuf += BytesToRead;
BytesToRead = BufLength - BytesToRead;
}
- memcpy(pBuf, pMapLowSpeed->InBuf[Port].Buf + pMapLowSpeed->InBuf[Port].OutPtr, BytesToRead);
- pMapLowSpeed->InBuf[Port].OutPtr += BytesToRead;
+ memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead);
+ pInBuf->OutPtr += BytesToRead;
return (NO_ERR);
}
diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c
index 47ca7c0..ffd129b 100644
--- a/AT91SAM7S256/Source/c_input.c
+++ b/AT91SAM7S256/Source/c_input.c
@@ -226,8 +226,11 @@ void cInputCtrl(void)
for (Tmp = 0; Tmp < NO_OF_INPUTS; Tmp++)
{
+ UBYTE sType = IOMapInput.Inputs[Tmp].SensorType;
+ UBYTE *pType = &IOMapInput.Inputs[Tmp].SensorType;
+ UBYTE oldType = VarsInput.OldSensorType[Tmp];
- if ((IOMapInput.Inputs[Tmp].SensorType) != (VarsInput.OldSensorType[Tmp]))
+ if (sType != oldType)
{
/* Clear all variables for this sensor */
@@ -239,9 +242,10 @@ void cInputCtrl(void)
memset(&(VarsInput.VarsColor[Tmp]),0 ,sizeof(VarsInput.VarsColor[Tmp]));
/* Setup the pins for the new sensortype */
- cInputSetupType(Tmp, &(IOMapInput.Inputs[Tmp].SensorType), VarsInput.OldSensorType[Tmp]);
+ cInputSetupType(Tmp, pType, oldType);
+ sType = *pType;
IOMapInput.Inputs[Tmp].InvalidData = INVALID_DATA;
- VarsInput.OldSensorType[Tmp] = IOMapInput.Inputs[Tmp].SensorType;
+ VarsInput.OldSensorType[Tmp] = sType;
}
else
{
@@ -250,12 +254,9 @@ 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 (((IOMapInput.Inputs[Tmp].SensorType) == COLORFULL) ||
- ((IOMapInput.Inputs[Tmp].SensorType) == COLORRED) ||
- ((IOMapInput.Inputs[Tmp].SensorType) == COLORGREEN)||
- ((IOMapInput.Inputs[Tmp].SensorType) == COLORBLUE) ||
- ((IOMapInput.Inputs[Tmp].SensorType) == COLOREXIT) ||
- ((IOMapInput.Inputs[Tmp].SensorType) == COLORNONE))
+ if ((sType == COLORFULL) || (sType == COLORRED) ||
+ (sType == COLORGREEN)|| (sType == COLORBLUE) ||
+ (sType == COLOREXIT) || (sType == COLORNONE))
{
cInputCalcSensorValues(Tmp);
}
diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c
index 9566938..e2115a7 100644
--- a/AT91SAM7S256/Source/c_output.c
+++ b/AT91SAM7S256/Source/c_output.c
@@ -45,15 +45,16 @@ void cOutputInit(void* pHeader)
for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++)
{
- IOMapOutput.Outputs[Tmp].Mode = 0x00;
- IOMapOutput.Outputs[Tmp].Speed = 0x00;
- IOMapOutput.Outputs[Tmp].ActualSpeed = 0x00;
- IOMapOutput.Outputs[Tmp].TachoCnt = 0x00;
- IOMapOutput.Outputs[Tmp].RunState = 0x00;
- IOMapOutput.Outputs[Tmp].TachoLimit = 0x00;
- IOMapOutput.Outputs[Tmp].RegPParameter = DEFAULT_P_GAIN_FACTOR;
- IOMapOutput.Outputs[Tmp].RegIParameter = DEFAULT_I_GAIN_FACTOR;
- IOMapOutput.Outputs[Tmp].RegDParameter = DEFAULT_D_GAIN_FACTOR;
+ OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]);
+ pOut->Mode = 0x00;
+ pOut->Speed = 0x00;
+ pOut->ActualSpeed = 0x00;
+ pOut->TachoCnt = 0x00;
+ pOut->RunState = 0x00;
+ pOut->TachoLimit = 0x00;
+ pOut->RegPParameter = DEFAULT_P_GAIN_FACTOR;
+ pOut->RegIParameter = DEFAULT_I_GAIN_FACTOR;
+ pOut->RegDParameter = DEFAULT_D_GAIN_FACTOR;
}
VarsOutput.TimeCnt = 0;
dOutputInit();
@@ -65,72 +66,73 @@ void cOutputCtrl(void)
for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++)
{
- if (IOMapOutput.Outputs[Tmp].Flags != 0)
- {
- if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_RESET_ROTATION_COUNT)
+ OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]);
+ if (pOut->Flags != 0)
+ {
+ if (pOut->Flags & UPDATE_RESET_ROTATION_COUNT)
{
- IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_RESET_ROTATION_COUNT;
+ pOut->Flags &= ~UPDATE_RESET_ROTATION_COUNT;
dOutputResetRotationCaptureCount(Tmp);
}
- if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_RESET_COUNT)
- {
- IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_RESET_COUNT;
- dOutputResetTachoLimit(Tmp);
- }
- if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_RESET_BLOCK_COUNT)
+ if (pOut->Flags & UPDATE_RESET_COUNT)
+ {
+ pOut->Flags &= ~UPDATE_RESET_COUNT;
+ dOutputResetTachoLimit(Tmp);
+ }
+ if (pOut->Flags & UPDATE_RESET_BLOCK_COUNT)
+ {
+ pOut->Flags &= ~UPDATE_RESET_BLOCK_COUNT;
+ dOutputResetBlockTachoLimit(Tmp);
+ }
+ if (pOut->Flags & UPDATE_SPEED)
{
- IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_RESET_BLOCK_COUNT;
- dOutputResetBlockTachoLimit(Tmp);
+ pOut->Flags &= ~UPDATE_SPEED;
+ if (pOut->Mode & MOTORON)
+ {
+ dOutputSetSpeed(Tmp, pOut->RunState, pOut->Speed, pOut->SyncTurnParameter);
+ }
}
- if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_SPEED)
- {
- IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_SPEED;
- if (IOMapOutput.Outputs[Tmp].Mode & MOTORON)
- {
- dOutputSetSpeed (Tmp, IOMapOutput.Outputs[Tmp].RunState, IOMapOutput.Outputs[Tmp].Speed, IOMapOutput.Outputs[Tmp].SyncTurnParameter);
- }
- }
- if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_TACHO_LIMIT)
- {
- IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_TACHO_LIMIT;
- dOutputSetTachoLimit(Tmp, IOMapOutput.Outputs[Tmp].TachoLimit);
- }
- if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_MODE)
- {
- IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_MODE;
- if (IOMapOutput.Outputs[Tmp].Mode & BRAKE)
+ if (pOut->Flags & UPDATE_TACHO_LIMIT)
+ {
+ pOut->Flags &= ~UPDATE_TACHO_LIMIT;
+ dOutputSetTachoLimit(Tmp, pOut->TachoLimit);
+ }
+ if (pOut->Flags & UPDATE_MODE)
+ {
+ pOut->Flags &= ~UPDATE_MODE;
+ if (pOut->Mode & BRAKE)
{
// Motor is Braked
dOutputSetMode(Tmp, BRAKE);
- }
+ }
else
{
// Motor is floated
dOutputSetMode(Tmp, 0x00);
}
- if (IOMapOutput.Outputs[Tmp].Mode & MOTORON)
- {
- if (IOMapOutput.Outputs[Tmp].Mode & REGULATED)
- {
- dOutputEnableRegulation(Tmp, IOMapOutput.Outputs[Tmp].RegMode);
- }
- else
- {
- dOutputDisableRegulation(Tmp);
- }
- }
- else
- {
- dOutputSetSpeed(Tmp, 0x00, 0x00, 0x00);
- dOutputDisableRegulation(Tmp);
- }
- }
- if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_PID_VALUES)
- {
- IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_PID_VALUES;
- dOutputSetPIDParameters(Tmp, IOMapOutput.Outputs[Tmp].RegPParameter, IOMapOutput.Outputs[Tmp].RegIParameter, IOMapOutput.Outputs[Tmp].RegDParameter);
- }
- }
+ if (pOut->Mode & MOTORON)
+ {
+ if (pOut->Mode & REGULATED)
+ {
+ dOutputEnableRegulation(Tmp, pOut->RegMode);
+ }
+ else
+ {
+ dOutputDisableRegulation(Tmp);
+ }
+ }
+ else
+ {
+ dOutputSetSpeed(Tmp, 0x00, 0x00, 0x00);
+ dOutputDisableRegulation(Tmp);
+ }
+ }
+ if (pOut->Flags & UPDATE_PID_VALUES)
+ {
+ pOut->Flags &= ~UPDATE_PID_VALUES;
+ dOutputSetPIDParameters(Tmp, pOut->RegPParameter, pOut->RegIParameter, pOut->RegDParameter);
+ }
+ }
}
dOutputCtrl();
cOutputUpdateIomap();
@@ -151,14 +153,15 @@ void cOutputUpdateIomap(void)
for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++)
{
- IOMapOutput.Outputs[Tmp].ActualSpeed = TempCurrentMotorSpeed[Tmp];
- IOMapOutput.Outputs[Tmp].TachoCnt = TempTachoCount[Tmp];
- IOMapOutput.Outputs[Tmp].BlockTachoCount = TempBlockTachoCount[Tmp];
- IOMapOutput.Outputs[Tmp].RotationCount = TempRotationCount[Tmp];
- IOMapOutput.Outputs[Tmp].Overloaded = TempMotorOverloaded[Tmp];
- if (!(IOMapOutput.Outputs[Tmp].Flags & PENDING_UPDATES))
+ OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]);
+ pOut->ActualSpeed = TempCurrentMotorSpeed[Tmp];
+ pOut->TachoCnt = TempTachoCount[Tmp];
+ pOut->BlockTachoCount = TempBlockTachoCount[Tmp];
+ pOut->RotationCount = TempRotationCount[Tmp];
+ pOut->Overloaded = TempMotorOverloaded[Tmp];
+ if (!(pOut->Flags & PENDING_UPDATES))
{
- IOMapOutput.Outputs[Tmp].RunState = TempRunState[Tmp];
+ pOut->RunState = TempRunState[Tmp];
}
}
}