From 6c5075bc99ba07f46562f7f10db14e153d7a93ec Mon Sep 17 00:00:00 2001 From: jhansen Date: Mon, 26 Apr 2010 21:49:44 +0000 Subject: git-svn-id: https://mindboards.svn.sourceforge.net/svnroot/mindboards/lms_nbcnxc_128/trunk@6 c9361245-7fe8-9947-84e8-057757c4e366 --- AT91SAM7S256/Source/c_lowspeed.c | 228 +++++++++++++++++++-------------------- 1 file changed, 114 insertions(+), 114 deletions(-) (limited to 'AT91SAM7S256/Source/c_lowspeed.c') diff --git a/AT91SAM7S256/Source/c_lowspeed.c b/AT91SAM7S256/Source/c_lowspeed.c index 26851db..de0cf77 100644 --- a/AT91SAM7S256/Source/c_lowspeed.c +++ b/AT91SAM7S256/Source/c_lowspeed.c @@ -58,147 +58,147 @@ void cLowSpeedCtrl(void) for (ChannelNumber = 0; ChannelNumber < NO_OF_LOWSPEED_COM_CHANNEL; ChannelNumber++) { //Lowspeed com is activated - switch (IOMapLowSpeed.ChannelState[ChannelNumber]) - { - case LOWSPEED_IDLE: - { - } - break; - - case LOWSPEED_INIT: - { - if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) + switch (IOMapLowSpeed.ChannelState[ChannelNumber]) + { + case LOWSPEED_IDLE: { - if (VarsLowSpeed.TimerState == TIMER_STOPPED) - { - dLowSpeedStartTimer(); - VarsLowSpeed.TimerState = TIMER_RUNNING; - } - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_LOAD_BUFFER; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_NO_ERROR; - VarsLowSpeed.ErrorCount[ChannelNumber] = 0; - dLowSpeedInitPins(ChannelNumber); } - else + break; + + case LOWSPEED_INIT: { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; + if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) + { + if (VarsLowSpeed.TimerState == TIMER_STOPPED) + { + dLowSpeedStartTimer(); + VarsLowSpeed.TimerState = TIMER_RUNNING; + } + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_LOAD_BUFFER; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_NO_ERROR; + VarsLowSpeed.ErrorCount[ChannelNumber] = 0; + dLowSpeedInitPins(ChannelNumber); + } + else + { + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; + } } - } - break; + break; - case LOWSPEED_LOAD_BUFFER: - { - if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) + case LOWSPEED_LOAD_BUFFER: { - VarsLowSpeed.OutputBuf[ChannelNumber].OutPtr = 0; - for (VarsLowSpeed.OutputBuf[ChannelNumber].InPtr = 0; VarsLowSpeed.OutputBuf[ChannelNumber].InPtr < IOMapLowSpeed.OutBuf[ChannelNumber].InPtr; VarsLowSpeed.OutputBuf[ChannelNumber].InPtr++) + if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) { - VarsLowSpeed.OutputBuf[ChannelNumber].Buf[VarsLowSpeed.OutputBuf[ChannelNumber].InPtr] = IOMapLowSpeed.OutBuf[ChannelNumber].Buf[IOMapLowSpeed.OutBuf[ChannelNumber].OutPtr]; - IOMapLowSpeed.OutBuf[ChannelNumber].OutPtr++; - } - if (dLowSpeedSendData(ChannelNumber, &VarsLowSpeed.OutputBuf[ChannelNumber].Buf[0], (VarsLowSpeed.OutputBuf[ChannelNumber].InPtr - VarsLowSpeed.OutputBuf[ChannelNumber].OutPtr))) - { - if (IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx != 0) + VarsLowSpeed.OutputBuf[ChannelNumber].OutPtr = 0; + for (VarsLowSpeed.OutputBuf[ChannelNumber].InPtr = 0; VarsLowSpeed.OutputBuf[ChannelNumber].InPtr < IOMapLowSpeed.OutBuf[ChannelNumber].InPtr; VarsLowSpeed.OutputBuf[ChannelNumber].InPtr++) + { + VarsLowSpeed.OutputBuf[ChannelNumber].Buf[VarsLowSpeed.OutputBuf[ChannelNumber].InPtr] = IOMapLowSpeed.OutBuf[ChannelNumber].Buf[IOMapLowSpeed.OutBuf[ChannelNumber].OutPtr]; + IOMapLowSpeed.OutBuf[ChannelNumber].OutPtr++; + } + if (dLowSpeedSendData(ChannelNumber, &VarsLowSpeed.OutputBuf[ChannelNumber].Buf[0], (VarsLowSpeed.OutputBuf[ChannelNumber].InPtr - VarsLowSpeed.OutputBuf[ChannelNumber].OutPtr))) { - dLowSpeedReceiveData(ChannelNumber, &VarsLowSpeed.InputBuf[ChannelNumber].Buf[0], IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx); - VarsLowSpeed.RxTimeCnt[ChannelNumber] = 0; + if (IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx != 0) + { + dLowSpeedReceiveData(ChannelNumber, &VarsLowSpeed.InputBuf[ChannelNumber].Buf[0], IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx, IOMapLowSpeed.NoRestartOnRead); + VarsLowSpeed.RxTimeCnt[ChannelNumber] = 0; + } + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_COMMUNICATING; + IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_TRANSMITTING; } - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_COMMUNICATING; - IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_TRANSMITTING; - } + else + { + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; + } + } else { IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; } } - else - { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; - } - } - break; + break; - case LOWSPEED_COMMUNICATING: - { - if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) + case LOWSPEED_COMMUNICATING: { - if (IOMapLowSpeed.Mode[ChannelNumber] == LOWSPEED_TRANSMITTING) - { - Temp = dLowSpeedComTxStatus(ChannelNumber); // Returns 0x00 if not done, 0x01 if success, 0xFF if error - - if (Temp == LOWSPEED_COMMUNICATION_SUCCESS) - { - if (IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx != 0) + if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) + { + if (IOMapLowSpeed.Mode[ChannelNumber] == LOWSPEED_TRANSMITTING) + { + Temp = dLowSpeedComTxStatus(ChannelNumber); // Returns 0x00 if not done, 0x01 if success, 0xFF if error + + if (Temp == LOWSPEED_COMMUNICATION_SUCCESS) + { + if (IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx != 0) + { + IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_RECEIVING; + } + else + { + IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_DATA_RECEIVED; + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_DONE; + } + } + if (Temp == LOWSPEED_COMMUNICATION_ERROR) + { + //ERROR in Communication, No ACK received from SLAVE, retry send data 3 times! + VarsLowSpeed.ErrorCount[ChannelNumber]++; + if (VarsLowSpeed.ErrorCount[ChannelNumber] > MAX_RETRY_TX_COUNT) + { + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_TX_ERROR; + } + else + { + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_LOAD_BUFFER; + } + } + } + if (IOMapLowSpeed.Mode[ChannelNumber] == LOWSPEED_RECEIVING) + { + VarsLowSpeed.RxTimeCnt[ChannelNumber]++; + if (VarsLowSpeed.RxTimeCnt[ChannelNumber] > LOWSPEED_RX_TIMEOUT) { - IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_RECEIVING; + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_RX_ERROR; } - else + Temp = dLowSpeedComRxStatus(ChannelNumber); + if (Temp == LOWSPEED_COMMUNICATION_SUCCESS) { + for (VarsLowSpeed.InputBuf[ChannelNumber].OutPtr = 0; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr < IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr++) + { + IOMapLowSpeed.InBuf[ChannelNumber].Buf[IOMapLowSpeed.InBuf[ChannelNumber].InPtr] = VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr]; + IOMapLowSpeed.InBuf[ChannelNumber].InPtr++; + if (IOMapLowSpeed.InBuf[ChannelNumber].InPtr >= SIZE_OF_LSBUF) + { + IOMapLowSpeed.InBuf[ChannelNumber].InPtr = 0; + } + VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr] = 0; + } IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_DATA_RECEIVED; IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_DONE; } - } - if (Temp == LOWSPEED_COMMUNICATION_ERROR) - { - //ERROR in Communication, No ACK received from SLAVE, retry send data 3 times! - VarsLowSpeed.ErrorCount[ChannelNumber]++; - if (VarsLowSpeed.ErrorCount[ChannelNumber] > MAX_RETRY_TX_COUNT) - { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_TX_ERROR; - } - else - { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_LOAD_BUFFER; - } - } - } - if (IOMapLowSpeed.Mode[ChannelNumber] == LOWSPEED_RECEIVING) - { - VarsLowSpeed.RxTimeCnt[ChannelNumber]++; - if (VarsLowSpeed.RxTimeCnt[ChannelNumber] > LOWSPEED_RX_TIMEOUT) - { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_RX_ERROR; - } - Temp = dLowSpeedComRxStatus(ChannelNumber); - if (Temp == LOWSPEED_COMMUNICATION_SUCCESS) - { - for (VarsLowSpeed.InputBuf[ChannelNumber].OutPtr = 0; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr < IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr++) + if (Temp == LOWSPEED_COMMUNICATION_ERROR) { - IOMapLowSpeed.InBuf[ChannelNumber].Buf[IOMapLowSpeed.InBuf[ChannelNumber].InPtr] = VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr]; - IOMapLowSpeed.InBuf[ChannelNumber].InPtr++; - if (IOMapLowSpeed.InBuf[ChannelNumber].InPtr >= SIZE_OF_LSBUF) + //There was and error in receiving data from the device + for (VarsLowSpeed.InputBuf[ChannelNumber].OutPtr = 0; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr < IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr++) { - IOMapLowSpeed.InBuf[ChannelNumber].InPtr = 0; + VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr] = 0; } - VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr] = 0; - } - IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_DATA_RECEIVED; - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_DONE; + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_RX_ERROR; + } } - if (Temp == LOWSPEED_COMMUNICATION_ERROR) - { - //There was and error in receiving data from the device - for (VarsLowSpeed.InputBuf[ChannelNumber].OutPtr = 0; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr < IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr++) - { - VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr] = 0; - } - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_RX_ERROR; - } - } - } - else - { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; - } - } - break; + } + else + { + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; + } + } + break; case LOWSPEED_ERROR: { -- cgit v1.2.3 From 87bea887659a34dec0fa01f87ed5b59e5ba701d7 Mon Sep 17 00:00:00 2001 From: tcsadmin Date: Wed, 25 Aug 2010 14:55:58 +0000 Subject: whitespace changes git-svn-id: https://mindboards.svn.sourceforge.net/svnroot/mindboards/lms_nbcnxc_128/trunk@16 c9361245-7fe8-9947-84e8-057757c4e366 --- AT91SAM7S256/Source/c_cmd.c | 249 +++++++++++++++++----------------- AT91SAM7S256/Source/c_cmd.h | 1 + AT91SAM7S256/Source/c_cmd_drawing.inc | 5 + AT91SAM7S256/Source/c_comm.c | 9 +- AT91SAM7S256/Source/c_lowspeed.c | 228 +++++++++++++++---------------- AT91SAM7S256/Source/c_output.c | 110 +++++++-------- AT91SAM7S256/Source/d_hispeed.r | 12 +- AT91SAM7S256/Source/d_input.h | 2 - AT91SAM7S256/Source/d_input.r | 2 +- AT91SAM7S256/Source/d_lowspeed.r | 233 +++++++++++++++---------------- AT91SAM7S256/Source/d_output.c | 13 +- AT91SAM7S256/Source/d_output.r | 76 +++++------ 12 files changed, 474 insertions(+), 466 deletions(-) (limited to 'AT91SAM7S256/Source/c_lowspeed.c') diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c index f4b1a31..41136b0 100644 --- a/AT91SAM7S256/Source/c_cmd.c +++ b/AT91SAM7S256/Source/c_cmd.c @@ -1091,7 +1091,7 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) } } break; - case RC_UPDATE_RESET_COUNT: + case RC_UPDATE_RESET_COUNT: { i = pInBuf[1]; @@ -1531,7 +1531,7 @@ void cCmdCtrl(void) cCmdWriteBenchmarkFile(); #endif - + //Re-initialize program state data (contents of memory pool preserved) //!!! Skip this step in simulator builds so helper access methods still work #ifndef SIM_NXT @@ -1579,7 +1579,7 @@ void cCmdCtrl(void) } while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time } - break; + break; case VM_RUN_PAUSE: { @@ -2079,7 +2079,7 @@ NXT_STATUS cCmdActivateProgram(UBYTE * pFileName) { VarsCmd.DatalogBuffer.Datalogs[j] = NOT_A_DS_ID; } - + // now that we've loaded program, prime memmgr dopevectors based upon number of handles in ds. ULONG numHandles= DV_ARRAY[0].Count/2; if(numHandles > 200) @@ -3535,13 +3535,14 @@ NXT_STATUS cCmdDatalogRead(UBYTE * pBuffer, UWORD Length, UBYTE Remove) return Status; } + // // Color Sensor Functions // 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 INPUTSTRUCT * pIn = &(pMapInput->Inputs[Port]); UBYTE sType = pIn->SensorType; @@ -4559,8 +4560,8 @@ afterCompaction: else if (Status == CLUMP_SUSPEND || Status == BREAKOUT_REQ || Status == ROTATE_QUEUE) // already requeued { pClumpRec->PC = pInstr + gPCDelta; - //Throw error if we ever advance beyond the clump's codespace - if (pInstr > lastClumpInstr) + //Throw error if we ever advance beyond the clump's codespace + if (pInstr > lastClumpInstr) { NXT_BREAK; Status = ERR_INSTR; @@ -5153,7 +5154,7 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) } break; - default: + default: { //Fatal error: Unrecognized instruction NXT_BREAK; @@ -5205,8 +5206,8 @@ NXT_STATUS cCmdInterpPolyUnop2(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset else { ArgVal2= cCmdGetScalarValFromDataArg(Arg2, Offset2); - if (opCode == OP_MOV) - ArgVal1 = ArgVal2; + if(opCode == OP_MOV) + ArgVal1= ArgVal2; else ArgVal1 = cCmdUnop2(Code, ArgVal2, TypeCode2); cCmdSetVal(pArg1, TypeCode1, ArgVal1); @@ -5215,10 +5216,11 @@ NXT_STATUS cCmdInterpPolyUnop2(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset } //At least one of the args is an aggregate type + if(TypeCode1 == TC_ARRAY && TypeCode2 == TC_ARRAY && opCode == OP_MOV) { TYPE_CODE tc1, tc2; - tc1 = cCmdDSType(INC_ID(Arg1)); - tc2 = cCmdDSType(INC_ID(Arg2)); + tc1= cCmdDSType(INC_ID(Arg1)); + tc2= cCmdDSType(INC_ID(Arg2)); if((tc1 <= TC_LAST_INT_SCALAR || tc1 == TC_FLOAT) && tc1 == tc2) { void *pArg1, *pArg2; ULONG Count = cCmdArrayCount(Arg2, Offset2); @@ -5504,12 +5506,12 @@ NXT_STATUS cCmdInterpScalarBinop(CODE_WORD * const pCode) if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) { ArgVal2= GetProcArray[TypeCode2](VarsCmd.pDataspace + dsTOC2Ptr->DSOffset); ArgVal3= GetProcArray[TypeCode3](VarsCmd.pDataspace + dsTOC3Ptr->DSOffset); - ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3); + ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3); DS_TOC_ENTRY *dsTOC1Ptr= &VarsCmd.pDataspaceTOC[Arg1]; SetProcArray[dsTOC1Ptr->TypeCode](VarsCmd.pDataspace + dsTOC1Ptr->DSOffset, ArgVal1); - scalarCmp++; - Status = NO_ERR; - } + scalarCmp++; + Status = NO_ERR; + } else if (TypeCode2 == TC_ARRAY) // two strings { // memcmp(); here or in compareagg, could use memcmp to speed up string compares ??? @@ -5523,31 +5525,31 @@ NXT_STATUS cCmdInterpScalarBinop(CODE_WORD * const pCode) } } else if(opCode == OP_BRCMP) { // t2 and t3 guaranteed scalar - TYPE_CODE TypeCode2, TypeCode3; - ULONG ArgVal2, ArgVal3; + TYPE_CODE TypeCode2, TypeCode3; + ULONG ArgVal2, ArgVal3; - Arg1 = pCode[1]; - Arg2 = pCode[2]; - Arg3 = pCode[3]; - TypeCode2= cCmdDSType(Arg2); - TypeCode3= cCmdDSType(Arg3); - ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0); - ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0); - CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3); + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; + TypeCode2= cCmdDSType(Arg2); + TypeCode3= cCmdDSType(Arg3); + ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0); + ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0); + CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3); - if (CmpBool) - gPCDelta = (SWORD)Arg1; - else - gPCDelta= 4; - Status= NO_ERR; - } + if (CmpBool) + gPCDelta = (SWORD)Arg1; + else + gPCDelta= 4; + Status= NO_ERR; + } else if(opCode >= OP_SETIN && opCode <= OP_GETOUT) { - Arg1 = pCode[1]; - Arg2 = pCode[2]; - Arg3 = pCode[3]; + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; Status= cCmdIOGetSet(opCode, Arg1, Arg2, Arg3); gPCDelta= 4; - } + } else { scalarOther ++; Status= cCmdInterpBinop(pCode); @@ -5573,10 +5575,10 @@ NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode) gPCDelta= 4; NXT_ASSERT(pCode != NULL); - opCode = OP_CODE(pCode); - Arg1 = pCode[1]; - Arg2 = pCode[2]; - Arg3 = pCode[3]; + opCode = OP_CODE(pCode); + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; if ((opCode <= OP_XOR) || (opCode >= OP_LSL && opCode <= OP_ROTR) || @@ -5585,96 +5587,96 @@ NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode) else if(opCode >= OP_SETIN && opCode <= OP_GETOUT) Status= cCmdIOGetSet(opCode, Arg1, Arg2, Arg3); else { - switch (opCode) + switch (opCode) + { + case OP_CMP: { - case OP_CMP: - { TYPE_CODE TypeCode2= cCmdDSType(Arg2), TypeCode3= cCmdDSType(Arg3); - if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) { - ULONG ArgVal1, ArgVal2, ArgVal3; - ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0); - ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0); - ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3); - cCmdSetScalarValFromDataArg(Arg1, ArgVal1); - PolyScalarCmp++; - } - else if (IS_AGGREGATE_TYPE(TypeCode2) && IS_AGGREGATE_TYPE(TypeCode3) && !IS_AGGREGATE_TYPE(cCmdDSType(Arg1))) - { - //Compare Aggregates - Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0); - cCmdSetScalarValFromDataArg(Arg1, CmpBool); - recursiveCmp++; - } - else - { - //Compare Elements - Status = cCmdInterpPolyBinop(*pCode, Arg1, 0, Arg2, 0, Arg3, 0); - polyPolyCmp++; - } - } - break; - - case OP_BRCMP: - { - TYPE_CODE TypeCode2= cCmdDSType(Arg2), TypeCode3= cCmdDSType(Arg3); - if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) { - ULONG ArgVal2, ArgVal3; + if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) { + ULONG ArgVal1, ArgVal2, ArgVal3; ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0); ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0); - CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3); + ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3); + cCmdSetScalarValFromDataArg(Arg1, ArgVal1); + PolyScalarCmp++; } - else //Compare Aggregates + else if (IS_AGGREGATE_TYPE(TypeCode2) && IS_AGGREGATE_TYPE(TypeCode3) && !IS_AGGREGATE_TYPE(cCmdDSType(Arg1))) + { + //Compare Aggregates Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0); - - if (CmpBool) - gPCDelta = (SWORD)Arg1; + cCmdSetScalarValFromDataArg(Arg1, CmpBool); + recursiveCmp++; } - break; - - case OP_INDEX: + else { - ArgVal3 = (Arg3 != NOT_A_DS_ID) ? cCmdGetScalarValFromDataArg(Arg3, 0) : 0; - - DVIndex2 = cCmdGetDVIndex(Arg2, 0); - if (ArgVal3 >= DV_ARRAY[DVIndex2].Count) - return (ERR_ARG); - - Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, ArgVal3)); + //Compare Elements + Status = cCmdInterpPolyBinop(*pCode, Arg1, 0, Arg2, 0, Arg3, 0); + polyPolyCmp++; } - break; - - case OP_ARRINIT: - { - //Arg1 - Dst, Arg2 - element type/default val, Arg3 - length - + } + break; + + case OP_BRCMP: + { + TYPE_CODE TypeCode2= cCmdDSType(Arg2), TypeCode3= cCmdDSType(Arg3); + if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) { + ULONG ArgVal2, ArgVal3; + ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0); + ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0); + CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3); + } + else //Compare Aggregates + Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0); + + if (CmpBool) + gPCDelta = (SWORD)Arg1; + } + break; + + case OP_INDEX: + { + ArgVal3 = (Arg3 != NOT_A_DS_ID) ? cCmdGetScalarValFromDataArg(Arg3, 0) : 0; + + DVIndex2 = cCmdGetDVIndex(Arg2, 0); + if (ArgVal3 >= DV_ARRAY[DVIndex2].Count) + return (ERR_ARG); + + Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, ArgVal3)); + } + break; + + case OP_ARRINIT: + { + //Arg1 - Dst, Arg2 - element type/default val, Arg3 - length + NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY); - + // determine the type of the array destination arg TYPE_CODE TypeCode = cCmdDSType(INC_ID(Arg1)); - + // How many elements do we want? ArgVal3 = (Arg3 != NOT_A_DS_ID) ? cCmdGetScalarValFromDataArg(Arg3, 0) : 0; - - Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)ArgVal3); + + Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)ArgVal3); if (!IS_ERR(Status)) { - DVIndex1 = cCmdGetDVIndex(Arg1, 0); + DVIndex1 = cCmdGetDVIndex(Arg1, 0); if(cCmdDSType(Arg2) <= TC_LAST_INT_SCALAR && TypeCode <= TC_LAST_INT_SCALAR) { ULONG val= cCmdGetScalarValFromDataArg(Arg2, 0); for (i = 0; i < ArgVal3; i++) // could init ptr and incr by offset GM??? - { - //copy Arg2 into each element of Arg1 + { + //copy Arg2 into each element of Arg1 cCmdSetVal(VarsCmd.pDataspace + ARRAY_ELEM_OFFSET(DVIndex1, i), TypeCode, val); } } else for (i = 0; i < ArgVal3; i++) //copy Arg2 into each element of Arg1 - Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, i), Arg2, 0); - } + Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, i), Arg2, 0); } - break; - + } + break; + case OP_FMTNUM: { //Check that the destination is a string (array of bytes) @@ -5682,13 +5684,13 @@ NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode) Status = ERR_INSTR; return (Status); } - + //Check that the format is a string (array of bytes) if (cCmdDSType(Arg2) != TC_ARRAY || cCmdDSType(INC_ID(Arg2)) != TC_UBYTE) { Status = ERR_INSTR; return (Status); } - + pArg2 = cCmdResolveDataArg(Arg2, 0, NULL); TYPE_CODE TypeCode3 = cCmdDSType(Arg3); @@ -5697,7 +5699,7 @@ NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode) Status = ERR_INSTR; return (Status); } - + char fmtBuf[256]; // arbitrary limit!!! // handle floats separately from scalar types if (TypeCode3 == TC_FLOAT) { @@ -5717,17 +5719,17 @@ NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode) Count = sprintf(fmtBuf, pArg2, ArgVal3); } } - + //add room for NULL terminator Count++; - + //Allocate array Status = cCmdDSArrayAlloc(Arg1, 0, Count); if (IS_ERR(Status)) return Status; - + pArg1 = cCmdResolveDataArg(Arg1, 0, NULL); - + //Populate array memcpy(pArg1, fmtBuf, Count); } @@ -5748,14 +5750,14 @@ NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode) } break; - default: - { - //Fatal error: Unrecognized instruction - NXT_BREAK; - Status = ERR_INSTR; - } - break; + default: + { + //Fatal error: Unrecognized instruction + NXT_BREAK; + Status = ERR_INSTR; } + break; + } } return (Status); } @@ -5800,10 +5802,10 @@ NXT_STATUS cCmdInterpPolyBinop(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset } else { - ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, Offset2); - ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, Offset3); - ArgVal1 = cCmdBinop(Code, ArgVal2, ArgVal3, TypeCode2, TypeCode3); - cCmdSetVal(pArg1, TypeCode1, ArgVal1); + ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, Offset2); + ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, Offset3); + ArgVal1 = cCmdBinop(Code, ArgVal2, ArgVal3, TypeCode2, TypeCode3); + cCmdSetVal(pArg1, TypeCode1, ArgVal1); } return Status; } @@ -6219,7 +6221,7 @@ NXT_STATUS cCmdMove(DATA_ARG Arg1, DATA_ARG Arg2) *(UWORD*)pArg1= *(UWORD*)pArg2; Status= NO_ERR; } - else + else { moveDiffInt++; ULONG val= cCmdGetScalarValFromDataArg(Arg2, 0); @@ -8683,6 +8685,7 @@ void cCmdWriteBenchmarkFile() } #endif + ///////////////////////////////////////////////////////////// // Dymanic syscall implementations //////////////////////////////////////////////////////////// diff --git a/AT91SAM7S256/Source/c_cmd.h b/AT91SAM7S256/Source/c_cmd.h index 5f9caa6..3af20cd 100644 --- a/AT91SAM7S256/Source/c_cmd.h +++ b/AT91SAM7S256/Source/c_cmd.h @@ -362,6 +362,7 @@ typedef struct #define SET_WRITE_DTLG(DVIndex) (VarsCmd.DatalogBuffer.Datalogs[VarsCmd.DatalogBuffer.WriteIndex] = (DVIndex)) #define SET_READ_DTLG(DVIndex) (VarsCmd.DatalogBuffer.Datalogs[VarsCmd.DatalogBuffer.ReadIndex] = (DVIndex)) + // //Definitions related to dataflow scheduling // diff --git a/AT91SAM7S256/Source/c_cmd_drawing.inc b/AT91SAM7S256/Source/c_cmd_drawing.inc index 01d90fd..9a62c12 100644 --- a/AT91SAM7S256/Source/c_cmd_drawing.inc +++ b/AT91SAM7S256/Source/c_cmd_drawing.inc @@ -306,10 +306,12 @@ void cCmdDrawTextHelper(ULONG DrawOptions, UBYTE *pString, IMG_PT * pPt) NXT_STATUS cCmdWrapDrawText(UBYTE * ArgV[]) { IMG_PT * pPt = (IMG_PT*) ArgV[1]; + ArgV[2] = (UBYTE*)cCmdDVPtr(*(DV_INDEX *)(ArgV[2])); //Resolve array argument cCmdDrawTextHelper(*(ULONG*)ArgV[3], ArgV[2], pPt); // Set return value *((SBYTE*)(ArgV[0])) = NO_ERR; + return NO_ERR; } @@ -382,8 +384,10 @@ NXT_STATUS cCmdWrapDrawCircle(UBYTE * ArgV[]) cCmdDrawCircle(pPt->X, pPt->Y, radius, pixelMode, fillMode); pMapDisplay->UpdateMask |= SCREEN_BIT(SCREEN_BACKGROUND); + // Set return value *((SBYTE*)(ArgV[0])) = NO_ERR; + return NO_ERR; } @@ -1533,6 +1537,7 @@ void cCmdSetPixel(SLONG X, SLONG Y, ULONG PixelMode) pMapDisplay->pFunc(DISPLAY_PIXEL, (UBYTE)PixelMode, (UBYTE)X, (UBYTE)Y, 0, 0); } + //----------------------------------------------------------------- //cCmdWrapSetScreenMode //ArgV[0]: (Function return) Status code, SBYTE diff --git a/AT91SAM7S256/Source/c_comm.c b/AT91SAM7S256/Source/c_comm.c index 610d369..f550ca3 100644 --- a/AT91SAM7S256/Source/c_comm.c +++ b/AT91SAM7S256/Source/c_comm.c @@ -219,6 +219,7 @@ void cCommCtrl(void) } } break; + case BT_ARM_DATA_MODE: case BT_ARM_GPS_MODE: case BT_ARM_RAW_MODE: @@ -1229,16 +1230,16 @@ UWORD cCommReceivedBtData(void) { if (VarsComm.BtState == BT_ARM_DATA_MODE) { - + /* Move the inptr ahead */ IOMapComm.BtInBuf.InPtr = NumberOfBytes; - + /* using the outbuf inptr in order to get the number of bytes in the return answer at the right place*/ IOMapComm.BtOutBuf.InPtr = NumberOfBytes; - + /* 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 (IOMapComm.BtOutBuf.InPtr) { diff --git a/AT91SAM7S256/Source/c_lowspeed.c b/AT91SAM7S256/Source/c_lowspeed.c index de0cf77..f8baa92 100644 --- a/AT91SAM7S256/Source/c_lowspeed.c +++ b/AT91SAM7S256/Source/c_lowspeed.c @@ -58,147 +58,147 @@ void cLowSpeedCtrl(void) for (ChannelNumber = 0; ChannelNumber < NO_OF_LOWSPEED_COM_CHANNEL; ChannelNumber++) { //Lowspeed com is activated - switch (IOMapLowSpeed.ChannelState[ChannelNumber]) - { - case LOWSPEED_IDLE: + switch (IOMapLowSpeed.ChannelState[ChannelNumber]) + { + case LOWSPEED_IDLE: + { + } + break; + + case LOWSPEED_INIT: + { + if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) { + if (VarsLowSpeed.TimerState == TIMER_STOPPED) + { + dLowSpeedStartTimer(); + VarsLowSpeed.TimerState = TIMER_RUNNING; + } + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_LOAD_BUFFER; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_NO_ERROR; + VarsLowSpeed.ErrorCount[ChannelNumber] = 0; + dLowSpeedInitPins(ChannelNumber); } - break; - - case LOWSPEED_INIT: + else { - if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) - { - if (VarsLowSpeed.TimerState == TIMER_STOPPED) - { - dLowSpeedStartTimer(); - VarsLowSpeed.TimerState = TIMER_RUNNING; - } - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_LOAD_BUFFER; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_NO_ERROR; - VarsLowSpeed.ErrorCount[ChannelNumber] = 0; - dLowSpeedInitPins(ChannelNumber); - } - else - { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; - } + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; } - break; + } + break; - case LOWSPEED_LOAD_BUFFER: + case LOWSPEED_LOAD_BUFFER: + { + if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) { - if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) + VarsLowSpeed.OutputBuf[ChannelNumber].OutPtr = 0; + for (VarsLowSpeed.OutputBuf[ChannelNumber].InPtr = 0; VarsLowSpeed.OutputBuf[ChannelNumber].InPtr < IOMapLowSpeed.OutBuf[ChannelNumber].InPtr; VarsLowSpeed.OutputBuf[ChannelNumber].InPtr++) { - VarsLowSpeed.OutputBuf[ChannelNumber].OutPtr = 0; - for (VarsLowSpeed.OutputBuf[ChannelNumber].InPtr = 0; VarsLowSpeed.OutputBuf[ChannelNumber].InPtr < IOMapLowSpeed.OutBuf[ChannelNumber].InPtr; VarsLowSpeed.OutputBuf[ChannelNumber].InPtr++) - { - VarsLowSpeed.OutputBuf[ChannelNumber].Buf[VarsLowSpeed.OutputBuf[ChannelNumber].InPtr] = IOMapLowSpeed.OutBuf[ChannelNumber].Buf[IOMapLowSpeed.OutBuf[ChannelNumber].OutPtr]; - IOMapLowSpeed.OutBuf[ChannelNumber].OutPtr++; - } - if (dLowSpeedSendData(ChannelNumber, &VarsLowSpeed.OutputBuf[ChannelNumber].Buf[0], (VarsLowSpeed.OutputBuf[ChannelNumber].InPtr - VarsLowSpeed.OutputBuf[ChannelNumber].OutPtr))) - { - if (IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx != 0) - { - dLowSpeedReceiveData(ChannelNumber, &VarsLowSpeed.InputBuf[ChannelNumber].Buf[0], IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx, IOMapLowSpeed.NoRestartOnRead); - VarsLowSpeed.RxTimeCnt[ChannelNumber] = 0; - } - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_COMMUNICATING; - IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_TRANSMITTING; - } - else + VarsLowSpeed.OutputBuf[ChannelNumber].Buf[VarsLowSpeed.OutputBuf[ChannelNumber].InPtr] = IOMapLowSpeed.OutBuf[ChannelNumber].Buf[IOMapLowSpeed.OutBuf[ChannelNumber].OutPtr]; + IOMapLowSpeed.OutBuf[ChannelNumber].OutPtr++; + } + if (dLowSpeedSendData(ChannelNumber, &VarsLowSpeed.OutputBuf[ChannelNumber].Buf[0], (VarsLowSpeed.OutputBuf[ChannelNumber].InPtr - VarsLowSpeed.OutputBuf[ChannelNumber].OutPtr))) + { + if (IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx != 0) { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; + dLowSpeedReceiveData(ChannelNumber, &VarsLowSpeed.InputBuf[ChannelNumber].Buf[0], IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx, IOMapLowSpeed.NoRestartOnRead); + VarsLowSpeed.RxTimeCnt[ChannelNumber] = 0; } - } + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_COMMUNICATING; + IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_TRANSMITTING; + } else { IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; } } - break; + else + { + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; + } + } + break; - case LOWSPEED_COMMUNICATING: + case LOWSPEED_COMMUNICATING: + { + if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) { - if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) - { - if (IOMapLowSpeed.Mode[ChannelNumber] == LOWSPEED_TRANSMITTING) - { - Temp = dLowSpeedComTxStatus(ChannelNumber); // Returns 0x00 if not done, 0x01 if success, 0xFF if error - - if (Temp == LOWSPEED_COMMUNICATION_SUCCESS) - { - if (IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx != 0) - { - IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_RECEIVING; - } - else - { - IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_DATA_RECEIVED; - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_DONE; - } - } - if (Temp == LOWSPEED_COMMUNICATION_ERROR) - { - //ERROR in Communication, No ACK received from SLAVE, retry send data 3 times! - VarsLowSpeed.ErrorCount[ChannelNumber]++; - if (VarsLowSpeed.ErrorCount[ChannelNumber] > MAX_RETRY_TX_COUNT) - { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_TX_ERROR; - } - else - { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_LOAD_BUFFER; - } - } - } - if (IOMapLowSpeed.Mode[ChannelNumber] == LOWSPEED_RECEIVING) - { - VarsLowSpeed.RxTimeCnt[ChannelNumber]++; - if (VarsLowSpeed.RxTimeCnt[ChannelNumber] > LOWSPEED_RX_TIMEOUT) + if (IOMapLowSpeed.Mode[ChannelNumber] == LOWSPEED_TRANSMITTING) + { + Temp = dLowSpeedComTxStatus(ChannelNumber); // Returns 0x00 if not done, 0x01 if success, 0xFF if error + + if (Temp == LOWSPEED_COMMUNICATION_SUCCESS) + { + if (IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx != 0) { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_RX_ERROR; + IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_RECEIVING; } - Temp = dLowSpeedComRxStatus(ChannelNumber); - if (Temp == LOWSPEED_COMMUNICATION_SUCCESS) + else { - for (VarsLowSpeed.InputBuf[ChannelNumber].OutPtr = 0; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr < IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr++) - { - IOMapLowSpeed.InBuf[ChannelNumber].Buf[IOMapLowSpeed.InBuf[ChannelNumber].InPtr] = VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr]; - IOMapLowSpeed.InBuf[ChannelNumber].InPtr++; - if (IOMapLowSpeed.InBuf[ChannelNumber].InPtr >= SIZE_OF_LSBUF) - { - IOMapLowSpeed.InBuf[ChannelNumber].InPtr = 0; - } - VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr] = 0; - } IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_DATA_RECEIVED; IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_DONE; } - if (Temp == LOWSPEED_COMMUNICATION_ERROR) + } + if (Temp == LOWSPEED_COMMUNICATION_ERROR) + { + //ERROR in Communication, No ACK received from SLAVE, retry send data 3 times! + VarsLowSpeed.ErrorCount[ChannelNumber]++; + if (VarsLowSpeed.ErrorCount[ChannelNumber] > MAX_RETRY_TX_COUNT) + { + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_TX_ERROR; + } + else + { + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_LOAD_BUFFER; + } + } + } + if (IOMapLowSpeed.Mode[ChannelNumber] == LOWSPEED_RECEIVING) + { + VarsLowSpeed.RxTimeCnt[ChannelNumber]++; + if (VarsLowSpeed.RxTimeCnt[ChannelNumber] > LOWSPEED_RX_TIMEOUT) + { + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_RX_ERROR; + } + Temp = dLowSpeedComRxStatus(ChannelNumber); + if (Temp == LOWSPEED_COMMUNICATION_SUCCESS) + { + for (VarsLowSpeed.InputBuf[ChannelNumber].OutPtr = 0; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr < IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr++) { - //There was and error in receiving data from the device - for (VarsLowSpeed.InputBuf[ChannelNumber].OutPtr = 0; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr < IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr++) + IOMapLowSpeed.InBuf[ChannelNumber].Buf[IOMapLowSpeed.InBuf[ChannelNumber].InPtr] = VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr]; + IOMapLowSpeed.InBuf[ChannelNumber].InPtr++; + if (IOMapLowSpeed.InBuf[ChannelNumber].InPtr >= SIZE_OF_LSBUF) { - VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr] = 0; + IOMapLowSpeed.InBuf[ChannelNumber].InPtr = 0; } - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_RX_ERROR; - } + VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr] = 0; + } + IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_DATA_RECEIVED; + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_DONE; } - } - else - { - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; - } - } - break; + if (Temp == LOWSPEED_COMMUNICATION_ERROR) + { + //There was and error in receiving data from the device + for (VarsLowSpeed.InputBuf[ChannelNumber].OutPtr = 0; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr < IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr++) + { + VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr] = 0; + } + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_RX_ERROR; + } + } + } + else + { + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_CH_NOT_READY; + } + } + break; case LOWSPEED_ERROR: { diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c index 23e0e28..ca3c494 100644 --- a/AT91SAM7S256/Source/c_output.c +++ b/AT91SAM7S256/Source/c_output.c @@ -48,10 +48,10 @@ void cOutputInit(void* pHeader) { OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]); pOut->Mode = 0x00; - pOut->Speed = 0x00; - pOut->ActualSpeed = 0x00; - pOut->TachoCnt = 0x00; - pOut->RunState = 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; @@ -71,71 +71,71 @@ void cOutputCtrl(void) { OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]); if (pOut->Flags != 0) - { - if (pOut->Flags & UPDATE_RESET_ROTATION_COUNT) + { + if (pOut->Flags & UPDATE_RESET_ROTATION_COUNT) { pOut->Flags &= ~UPDATE_RESET_ROTATION_COUNT; dOutputResetRotationCaptureCount(Tmp); } if (pOut->Flags & UPDATE_RESET_COUNT) - { - pOut->Flags &= ~UPDATE_RESET_COUNT; - dOutputResetTachoLimit(Tmp); - } + { + pOut->Flags &= ~UPDATE_RESET_COUNT; + dOutputResetTachoLimit(Tmp); + } if (pOut->Flags & UPDATE_RESET_BLOCK_COUNT) { pOut->Flags &= ~UPDATE_RESET_BLOCK_COUNT; - dOutputResetBlockTachoLimit(Tmp); + dOutputResetBlockTachoLimit(Tmp); } if (pOut->Flags & UPDATE_SPEED) - { - pOut->Flags &= ~UPDATE_SPEED; - if (pOut->Mode & MOTORON) - { - dOutputSetSpeed(Tmp, pOut->RunState, pOut->Speed, pOut->SyncTurnParameter); - } - } + { + pOut->Flags &= ~UPDATE_SPEED; + if (pOut->Mode & MOTORON) + { + dOutputSetSpeed(Tmp, pOut->RunState, pOut->Speed, pOut->SyncTurnParameter); + } + } if (pOut->Flags & UPDATE_TACHO_LIMIT) - { - pOut->Flags &= ~UPDATE_TACHO_LIMIT; - dOutputSetTachoLimit(Tmp, pOut->TachoLimit, pOut->Options); - } - if (pOut->Flags & UPDATE_MODE) - { + { + pOut->Flags &= ~UPDATE_TACHO_LIMIT; + dOutputSetTachoLimit(Tmp, pOut->TachoLimit, pOut->Options); + } + if (pOut->Flags & UPDATE_MODE) + { pOut->Flags &= ~UPDATE_MODE; - if (pOut->Mode & BRAKE) + if (pOut->Mode & BRAKE) { // Motor is Braked dOutputSetMode(Tmp, BRAKE); - } + } else { // Motor is floated dOutputSetMode(Tmp, 0x00); } - if (pOut->Mode & MOTORON) + if (pOut->Mode & MOTORON) { - if (pOut->Mode & REGULATED) - { - dOutputEnableRegulation(Tmp, pOut->RegMode); - } - else - { - dOutputDisableRegulation(Tmp); - } - } - else - { - dOutputSetSpeed(Tmp, 0x00, 0x00, 0x00); + 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); - } - } + { + pOut->Flags &= ~UPDATE_PID_VALUES; + dOutputSetPIDParameters(Tmp, pOut->RegPParameter, pOut->RegIParameter, pOut->RegDParameter); + } + } } dOutputCtrl(); cOutputUpdateIomap(); @@ -143,30 +143,30 @@ void cOutputCtrl(void) void cOutputUpdateIomap(void) { - UBYTE TempCurrentMotorSpeed[NO_OF_OUTPUTS]; - UBYTE TempRunState[NO_OF_OUTPUTS]; + UBYTE TempCurrentMotorSpeed[NO_OF_OUTPUTS]; + UBYTE TempRunState[NO_OF_OUTPUTS]; UBYTE TempMotorOverloaded[NO_OF_OUTPUTS]; - SLONG TempTachoCount[NO_OF_OUTPUTS]; + SLONG TempTachoCount[NO_OF_OUTPUTS]; SLONG TempBlockTachoCount[NO_OF_OUTPUTS]; SLONG TempRotationCount[NO_OF_OUTPUTS]; UBYTE Tmp; - dOutputGetMotorParameters(TempCurrentMotorSpeed, TempTachoCount, TempBlockTachoCount, TempRunState, TempMotorOverloaded,TempRotationCount); + dOutputGetMotorParameters(TempCurrentMotorSpeed, TempTachoCount, TempBlockTachoCount, TempRunState, TempMotorOverloaded,TempRotationCount); - for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++) + for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++) { - OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]); - pOut->ActualSpeed = TempCurrentMotorSpeed[Tmp]; + OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]); + pOut->ActualSpeed = TempCurrentMotorSpeed[Tmp]; pOut->TachoCnt = TempTachoCount[Tmp]; pOut->BlockTachoCount = TempBlockTachoCount[Tmp]; - pOut->RotationCount = TempRotationCount[Tmp]; + pOut->RotationCount = TempRotationCount[Tmp]; pOut->Overloaded = TempMotorOverloaded[Tmp]; if (!(pOut->Flags & PENDING_UPDATES)) { pOut->RunState = TempRunState[Tmp]; } - } + } } void cOutputExit(void) diff --git a/AT91SAM7S256/Source/d_hispeed.r b/AT91SAM7S256/Source/d_hispeed.r index 371b76d..949f0f5 100644 --- a/AT91SAM7S256/Source/d_hispeed.r +++ b/AT91SAM7S256/Source/d_hispeed.r @@ -27,7 +27,7 @@ #define PER_ID6_UART_0 0x40 #define UART0_INQ 0x40 -#define BAUD_RATE 921600L +#define BAUD_RATE 921600L #define SIZE_OF_INBUF 128 #define NO_OF_INBUFFERS 2 @@ -46,10 +46,10 @@ static UBYTE MsgIn; static UBYTE InBufOutCnt; #define HIGHSPEEDInit {\ - *AT91C_PIOA_PER = HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN | HIGHSPEED_RX_PIN; /* Enable PIO on PA07, PA06 & PA05 */\ - *AT91C_PIOA_PPUDR = HIGHSPEED_RX_PIN | HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN; /* Disable Pull-up resistor */\ + *AT91C_PIOA_PER = HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN | HIGHSPEED_RX_PIN; /* Enable PIO on PA07, PA06 & PA05 */\ + *AT91C_PIOA_PPUDR = HIGHSPEED_RX_PIN | HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN; /* Disable Pull-up resistor */\ *AT91C_PIOA_OER = HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN | HIGHSPEED_RX_PIN; /* PA07 & PA06 set to Output */\ - *AT91C_PIOA_CODR = HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN | HIGHSPEED_RX_PIN; /* Set output low */\ + *AT91C_PIOA_CODR = HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN | HIGHSPEED_RX_PIN; /* Set output low */\ } #define HIGHSPEEDSetupUart(_baud, _mode) {\ @@ -94,7 +94,7 @@ static UBYTE InBufOutCnt; #define HIGHSPEEDInitReceiver(InputBuffer)\ {\ - UBYTE Tmp;\ + UBYTE Tmp;\ pBuffer = InputBuffer;\ *AT91C_US0_PTCR = (AT91C_PDC_RXTDIS | AT91C_PDC_TXTDIS); /* Disable of TX & RX with DMA */\ *AT91C_US0_RCR = 0; /* Receive Counter Register */\ @@ -174,7 +174,7 @@ static UBYTE InBufOutCnt; #define HIGHSPEEDExit {\ *AT91C_PMC_PCDR = PER_ID6_UART_0; /* Disable PMC clock for UART 0*/\ *AT91C_PIOA_PER = HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN | HIGHSPEED_RX_PIN; /* Enable PIO on PA07, PA06 & PA05 */\ - *AT91C_PIOA_PPUDR = HIGHSPEED_RX_PIN | HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN; /* Disable Pull-up resistor */\ + *AT91C_PIOA_PPUDR = HIGHSPEED_RX_PIN | HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN; /* Disable Pull-up resistor */\ *AT91C_PIOA_OER = HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN | HIGHSPEED_RX_PIN; /* PA07 & PA06 set to Output */\ *AT91C_PIOA_CODR = HIGHSPEED_TX_PIN | HIGHSPEED_RTS_PIN | HIGHSPEED_RX_PIN; /* Set output low */\ } diff --git a/AT91SAM7S256/Source/d_input.h b/AT91SAM7S256/Source/d_input.h index 3805103..d365dd1 100644 --- a/AT91SAM7S256/Source/d_input.h +++ b/AT91SAM7S256/Source/d_input.h @@ -45,6 +45,4 @@ void dInputClearColor100msTimer(UBYTE No); UBYTE dInputChkColor100msTimer(UBYTE No); - - #endif diff --git a/AT91SAM7S256/Source/d_input.r b/AT91SAM7S256/Source/d_input.r index ffcac78..3dc567e 100644 --- a/AT91SAM7S256/Source/d_input.r +++ b/AT91SAM7S256/Source/d_input.r @@ -247,7 +247,7 @@ static ULONG ColorTimer[NO_OF_INPUTS]; {\ V = TRUE;\ } - + void rInputSingleADC(UBYTE Port, UWORD *Val) diff --git a/AT91SAM7S256/Source/d_lowspeed.r b/AT91SAM7S256/Source/d_lowspeed.r index e215b3c..3ca660d 100644 --- a/AT91SAM7S256/Source/d_lowspeed.r +++ b/AT91SAM7S256/Source/d_lowspeed.r @@ -74,17 +74,17 @@ const ULONG CLK_OR_DATA_PINS[4] = {CHANNEL_ONE_CLK | CHANNEL_ONE_DATA, #define PIO_INQ 0x04 //Used for variable ChannelState -#define LOWSPEED_IDLE 0x00 -#define LOWSPEED_TX_STOP_BIT 0x01 +#define LOWSPEED_IDLE 0x00 +#define LOWSPEED_TX_STOP_BIT 0x01 #define LOWSPEED_TRANSMITTING 0x02 -#define LOWSPEED_RECEIVING 0x04 +#define LOWSPEED_RECEIVING 0x04 #define LOWSPEED_TEST_WAIT_STATE 0x08 #define LOWSPEED_RESTART_CONDITION 0x10 #define LOWSPEED_WAIT_BEFORE_RX 0x20 //Used for variable TxState -#define TX_IDLE 0x00 -#define TX_DATA_MORE_DATA 0x01 +#define TX_IDLE 0x00 +#define TX_DATA_MORE_DATA 0x01 #define TX_DATA_CLK_HIGH 0x02 #define TX_EVALUATE_ACK_CLK_HIGH 0x03 #define TX_DATA_READ_ACK_CLK_LOW 0x04 @@ -92,56 +92,57 @@ const ULONG CLK_OR_DATA_PINS[4] = {CHANNEL_ONE_CLK | CHANNEL_ONE_DATA, #define TX_ACK_EVALUATED_CLK_LOW 0x06 //Used for variable RxState -#define RX_IDLE 0x00 -#define RX_START_BIT_CLK_HIGH 0x01 +#define RX_IDLE 0x00 +#define RX_START_BIT_CLK_HIGH 0x01 #define RX_DATA_CLK_HIGH 0x02 #define RX_ACK_TX_CLK_HIGH 0x03 -#define RX_DATA_CLK_LOW 0x04 +#define RX_DATA_CLK_LOW 0x04 #define RX_DONE_OR_NOT_CLK_LOW 0x05 //Used for variable ReStart #define RESTART_STATE_IDLE 0x00 -#define RESTART_STATE_ONE 0x01 -#define RESTART_STATE_TWO 0x02 +#define RESTART_STATE_ONE 0x01 +#define RESTART_STATE_TWO 0x02 #define RESTART_STATE_THREE 0x03 #define RESTART_STATE_FOUR 0x04 #define RESTART_STATE_FIVE 0x05 #define RESTART_STATE_SIX 0x06 #define RESTART_STATE_SEVEN 0x07 -#define LOWSpeedTxInit {\ - LowSpeedData[LOWSPEED_CHANNEL1].ChannelState = 0;\ - LowSpeedData[LOWSPEED_CHANNEL2].ChannelState = 0;\ - LowSpeedData[LOWSPEED_CHANNEL3].ChannelState = 0;\ - LowSpeedData[LOWSPEED_CHANNEL4].ChannelState = 0;\ -} - -#define LOWSpeedTimerInit {\ - *AT91C_PMC_PCER = 0x400; /* Enable clock for PWM, PID10*/\ - *AT91C_PWMC_MR = 0x01; /* CLKA is output from prescaler */\ - *AT91C_PWMC_MR |= 0x600; /* Prescaler MCK divided with 64 */\ - *AT91C_PWMC_CH0_CMR = 0x06; /* Channel 0 uses MCK divided by 64 */\ - *AT91C_PWMC_CH0_CMR &= 0xFFFFFEFF; /* Left alignment on periode */\ - *AT91C_PWMC_CH0_CPRDR = 0x20; /* Set to 39 => 52uSecondes interrupt */\ - *AT91C_PWMC_IDR = AT91C_PWMC_CHID0; /* Disable interrupt for PWM output channel 0 */\ - *AT91C_AIC_IDCR = 0x400; /* Disable AIC intterupt on ID10 PWM */\ - AT91C_AIC_SVR[10] = (unsigned int)LowSpeedPwmIrqHandler;\ - AT91C_AIC_SMR[10] = 0x01; /* Enable trigger on level */\ - *AT91C_AIC_ICCR = 0x400; /* Clear interrupt register PID10*/\ - *AT91C_PWMC_IER = AT91C_PWMC_CHID0; /* Enable interrupt for PWM output channel 0 */\ - *AT91C_AIC_IECR = 0x400; /* Enable interrupt from PWM */\ -} +#define LOWSpeedTxInit {\ + LowSpeedData[LOWSPEED_CHANNEL1].ChannelState = 0;\ + LowSpeedData[LOWSPEED_CHANNEL2].ChannelState = 0;\ + LowSpeedData[LOWSPEED_CHANNEL3].ChannelState = 0;\ + LowSpeedData[LOWSPEED_CHANNEL4].ChannelState = 0;\ + } + +#define LOWSpeedTimerInit {\ + *AT91C_PMC_PCER = 0x400; /* Enable clock for PWM, PID10*/\ + *AT91C_PWMC_MR = 0x01; /* CLKA is output from prescaler */\ + *AT91C_PWMC_MR |= 0x600; /* Prescaler MCK divided with 64 */\ + *AT91C_PWMC_CH0_CMR = 0x06; /* Channel 0 uses MCK divided by 64 */\ + *AT91C_PWMC_CH0_CMR &= 0xFFFFFEFF; /* Left alignment on periode */\ + *AT91C_PWMC_CH0_CPRDR = 0x20; /* Set to 39 => 52uSecondes interrupt */\ + *AT91C_PWMC_IDR = AT91C_PWMC_CHID0; /* Disable interrupt for PWM output channel 0 */\ + *AT91C_AIC_IDCR = 0x400; /* Disable AIC intterupt on ID10 PWM */\ + AT91C_AIC_SVR[10] = (unsigned int)LowSpeedPwmIrqHandler;\ + AT91C_AIC_SMR[10] = 0x01; /* Enable trigger on level */\ + *AT91C_AIC_ICCR = 0x400; /* Clear interrupt register PID10*/\ + *AT91C_PWMC_IER = AT91C_PWMC_CHID0; /* Enable interrupt for PWM output channel 0 */\ + *AT91C_AIC_IECR = 0x400; /* Enable interrupt from PWM */\ + } #define LOWSpeedExit -#define ENABLEDebugOutput {\ - *AT91C_PIOA_PER = AT91C_PIO_PA29; /* Enable PIO on PA029 */\ - *AT91C_PIOA_OER = AT91C_PIO_PA29; /* PA029 set to Output */\ - *AT91C_PIOA_CODR = 0x20000000;\ -} +#define ENABLEDebugOutput {\ + *AT91C_PIOA_PER = AT91C_PIO_PA29; /* Enable PIO on PA029 */\ + *AT91C_PIOA_OER = AT91C_PIO_PA29; /* PA029 set to Output */\ + *AT91C_PIOA_CODR = 0x20000000;\ + } + +#define SETDebugOutputHigh *AT91C_PIOA_SODR = 0x20000000 -#define SETDebugOutputHigh *AT91C_PIOA_SODR = 0x20000000 -#define SETDebugOutputLow *AT91C_PIOA_CODR = 0x20000000 +#define SETDebugOutputLow *AT91C_PIOA_CODR = 0x20000000 #define SETClkComOneHigh *AT91C_PIOA_SODR = CHANNEL_ONE_CLK #define SETClkComOneLow *AT91C_PIOA_CODR = CHANNEL_ONE_CLK @@ -253,13 +254,13 @@ const ULONG CLK_OR_DATA_PINS[4] = {CHANNEL_ONE_CLK | CHANNEL_ONE_DATA, *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 */\ - *AT91C_PWMC_IDR = AT91C_PWMC_CHID0; /* Disable interrupt from PWM output channel 0 */\ - *AT91C_AIC_IDCR = 0x400; /* Disable Irq from PID10 */\ - *AT91C_AIC_ICCR = 0x400; /* Clear interrupt register PID10*/\ - *AT91C_PMC_PCDR = 0x400; /* Disable clock for PWM, PID10*/\ -} +#define OLD_DISABLEPWMTimerForLowCom {\ + *AT91C_PWMC_DIS = AT91C_PWMC_CHID0; /* Disable PWM output channel 0 */\ + *AT91C_PWMC_IDR = AT91C_PWMC_CHID0; /* Disable interrupt from PWM output channel 0 */\ + *AT91C_AIC_IDCR = 0x400; /* Disable Irq from PID10 */\ + *AT91C_AIC_ICCR = 0x400; /* Clear interrupt register PID10*/\ + *AT91C_PMC_PCDR = 0x400; /* Disable clock for PWM, PID10*/\ + } __ramfunc void LowSpeedPwmIrqHandler(void) { @@ -452,11 +453,11 @@ __ramfunc void LowSpeedPwmIrqHandler(void) LowSpeedData[ChannelNr].ReStartBit = 0; LowSpeedData[ChannelNr].pComOutBuffer = &LowSpeedData[ChannelNr].ComDeviceAddress; *LowSpeedData[ChannelNr].pComOutBuffer += 0x01; - LowSpeedData[ChannelNr].ChannelState = LOWSPEED_TRANSMITTING; - LowSpeedData[ChannelNr].MaskBit = MASK_BIT_8; - LowSpeedData[ChannelNr].TxByteCnt = 0x01; - LowSpeedData[ChannelNr].TxState = TX_DATA_CLK_HIGH; - LowSpeedData[ChannelNr].AckStatus = 0; + LowSpeedData[ChannelNr].ChannelState = LOWSPEED_TRANSMITTING; + LowSpeedData[ChannelNr].MaskBit = MASK_BIT_8; + LowSpeedData[ChannelNr].TxByteCnt = 0x01; + LowSpeedData[ChannelNr].TxState = TX_DATA_CLK_HIGH; + LowSpeedData[ChannelNr].AckStatus = 0; } break; } @@ -514,9 +515,9 @@ __ramfunc void LowSpeedPwmIrqHandler(void) SETClkLow(ChannelNr); SETDataToInput(ChannelNr); LowSpeedData[ChannelNr].pComInBuffer++; - LowSpeedData[ChannelNr].RxByteCnt--; - LowSpeedData[ChannelNr].RxBitCnt = 0; - LowSpeedData[ChannelNr].RxState = RX_DONE_OR_NOT_CLK_LOW; + LowSpeedData[ChannelNr].RxByteCnt--; + LowSpeedData[ChannelNr].RxBitCnt = 0; + LowSpeedData[ChannelNr].RxState = RX_DONE_OR_NOT_CLK_LOW; } break; @@ -611,69 +612,69 @@ __ramfunc void LowSpeedPwmIrqHandler(void) } -#define STATUSTxCom(ChannelNumber, Status) {\ - if (LowSpeedData[ChannelNumber].ChannelState != 0)\ - {\ - if ((LowSpeedData[ChannelNumber].TxByteCnt == 0) && (LowSpeedData[ChannelNumber].ChannelState != LOWSPEED_RESTART_CONDITION))\ - {\ - if (LowSpeedData[ChannelNumber].MaskBit == 0)\ - {\ - if (LowSpeedData[ChannelNumber].AckStatus == 1)\ - {\ - Status = 0x01; /* TX SUCCESS */\ - }\ - else\ - {\ - Status = 0xFF; /* TX ERROR */\ - }\ - }\ - else\ - {\ - Status = 0;\ - }\ - }\ - else\ - {\ - Status = 0;\ - }\ - }\ - else\ - {\ - if (LowSpeedData[ChannelNumber].RxByteCnt == 0)\ - {\ - if (LowSpeedData[ChannelNumber].AckStatus == 1)\ - {\ - Status = 0x01; /* TX SUCCESS */\ - }\ - else\ - {\ - Status = 0xFF; /* TX ERROR */\ - }\ - }\ - else\ - {\ - Status = 0xFF; /* TX ERROR */\ - }\ - }\ -} +#define STATUSTxCom(ChannelNumber, Status) {\ + if (LowSpeedData[ChannelNumber].ChannelState != 0)\ + {\ + if ((LowSpeedData[ChannelNumber].TxByteCnt == 0) && (LowSpeedData[ChannelNumber].ChannelState != LOWSPEED_RESTART_CONDITION))\ + {\ + if (LowSpeedData[ChannelNumber].MaskBit == 0)\ + {\ + if (LowSpeedData[ChannelNumber].AckStatus == 1)\ + {\ + Status = 0x01; /* TX SUCCESS */\ + }\ + else\ + {\ + Status = 0xFF; /* TX ERROR */\ + }\ + }\ + else\ + {\ + Status = 0;\ + }\ + }\ + else\ + {\ + Status = 0;\ + }\ + }\ + else\ + {\ + if (LowSpeedData[ChannelNumber].RxByteCnt == 0)\ + {\ + if (LowSpeedData[ChannelNumber].AckStatus == 1)\ + {\ + Status = 0x01; /* TX SUCCESS */\ + }\ + else\ + {\ + Status = 0xFF; /* TX ERROR */\ + }\ + }\ + else\ + {\ + Status = 0xFF; /* TX ERROR */\ + }\ + }\ + } -#define STATUSRxCom(ChannelNumber, Status) {\ - if (LowSpeedData[ChannelNumber].ChannelState == LOWSPEED_IDLE)\ - {\ - if (LowSpeedData[ChannelNumber].RxByteCnt == 0)\ - {\ - Status = 0x01; /* RX SUCCESS */\ - }\ - else\ - {\ - Status = 0xFF; /* RX ERROR */\ - }\ - }\ - else\ - {\ - Status = 0;\ - }\ -} +#define STATUSRxCom(ChannelNumber, Status) {\ + if (LowSpeedData[ChannelNumber].ChannelState == LOWSPEED_IDLE)\ + {\ + if (LowSpeedData[ChannelNumber].RxByteCnt == 0)\ + {\ + Status = 0x01; /* RX SUCCESS */\ + }\ + else\ + {\ + Status = 0xFF; /* RX ERROR */\ + }\ + }\ + else\ + {\ + Status = 0;\ + }\ + } #endif diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c index 64f5e56..843db57 100644 --- a/AT91SAM7S256/Source/d_output.c +++ b/AT91SAM7S256/Source/d_output.c @@ -1125,13 +1125,12 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) { //Synchronise motor A & B MotorData[MotorOne].MotorSetSpeed = 0; - MotorData[MotorOne].MotorTargetSpeed = 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].MotorTargetSpeed = 0; MotorData[MotorTwo].MotorActualSpeed = 0; MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit; MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE; @@ -1143,12 +1142,12 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) { //Synchronise motor A & C MotorData[MotorOne].MotorSetSpeed = 0; - MotorData[MotorOne].MotorTargetSpeed = 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].MotorTargetSpeed = 0; MotorData[MotorTwo].MotorActualSpeed = 0; MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit; MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE; @@ -1157,7 +1156,7 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) { //Only Motor A has Sync setting => Stop normal MotorData[MotorNr].MotorSetSpeed = 0; - MotorData[MotorNr].MotorTargetSpeed = 0; + MotorData[MotorNr].MotorTargetSpeed = 0; MotorData[MotorNr].MotorActualSpeed = 0; MotorData[MotorNr].MotorRunState = pOne->RunStateAtLimit; MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; @@ -1244,7 +1243,7 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) { //Only Motor C has Sync settings => Stop normal MotorData[MotorNr].MotorSetSpeed = 0; - MotorData[MotorNr].MotorTargetSpeed = 0; + MotorData[MotorNr].MotorTargetSpeed = 0; MotorData[MotorNr].MotorActualSpeed = 0; MotorData[MotorNr].MotorRunState = pOne->RunStateAtLimit; MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; diff --git a/AT91SAM7S256/Source/d_output.r b/AT91SAM7S256/Source/d_output.r index a856920..1a30c5f 100644 --- a/AT91SAM7S256/Source/d_output.r +++ b/AT91SAM7S256/Source/d_output.r @@ -91,11 +91,11 @@ static TACHOPARAMETERS MotorTachoValue[3]; *AT91C_PIOA_PER = MOTOR_A_DIR; /* Enable PIO on PA1 */\ *AT91C_PIOA_ODR = MOTOR_A_DIR; /* PA1 set to input */\ *AT91C_PIOA_IFER = MOTOR_A_INT | MOTOR_A_DIR; /* Enable filter on PA15 & PA1 */\ - *AT91C_PMC_PCER = TIMER_1_ID13; /* Enable clock for TC1*/\ - *AT91C_TCB_BMR = AT91C_TCB_TC1XC1S_NONE; /* No external clock signal XC2 */\ - *AT91C_TCB_BCR = 0x0; /* Clear SYNC */\ - *AT91C_TC1_CMR = *AT91C_TC1_CMR & 0X00000000; /* Clear all bits in TC1_CMR */\ - *AT91C_TC1_CMR = *AT91C_TC1_CMR & 0xFFFF7FFF; /* Enable capture mode */\ + *AT91C_PMC_PCER = TIMER_1_ID13; /* Enable clock for TC1*/\ + *AT91C_TCB_BMR = AT91C_TCB_TC1XC1S_NONE; /* No external clock signal XC2 */\ + *AT91C_TCB_BCR = 0x0; /* Clear SYNC */\ + *AT91C_TC1_CMR = *AT91C_TC1_CMR & 0X00000000; /* Clear all bits in TC1_CMR */\ + *AT91C_TC1_CMR = *AT91C_TC1_CMR & 0xFFFF7FFF; /* Enable capture mode */\ *AT91C_TC1_CMR = *AT91C_TC1_CMR | AT91C_TC_CLKS_TIMER_DIV5_CLOCK; /* Set clock for timer to Clock5 = div 1024*/\ *AT91C_TC1_CMR = *AT91C_TC1_CMR | AT91C_TC_ABETRG; /* Use external trigger for TIO1*/\ *AT91C_TC1_CMR = *AT91C_TC1_CMR | AT91C_TC_EEVTEDG_BOTH; /* Trigger on both edges */\ @@ -118,14 +118,14 @@ static TACHOPARAMETERS MotorTachoValue[3]; *AT91C_PIOA_PPUDR = MOTOR_B_INT | MOTOR_B_DIR; /* Disable Pull Up resistor on PA26 & PA09 */\ *AT91C_PIOA_ODR = MOTOR_B_DIR; /* PA09 set to input */\ *AT91C_PIOA_IFER = MOTOR_B_INT | MOTOR_B_DIR; /* Enable filter on PA26 & PA09 */\ - *AT91C_PMC_PCER = TIMER_2_ID14; /* Enable clock for TC2*/\ - *AT91C_TCB_BMR = AT91C_TCB_TC2XC2S_NONE; /* No external clock signal */\ - *AT91C_TCB_BCR = 0x0; /* Clear SYNC */\ - *AT91C_TC2_CMR = *AT91C_TC2_CMR & 0X00000000; /* Clear all bits in TC1_CMR */\ - *AT91C_TC2_CMR = *AT91C_TC2_CMR & 0xFFFF7FFF; /* Enable capture mode */\ - *AT91C_TC2_CMR = *AT91C_TC2_CMR | AT91C_TC_CLKS_TIMER_DIV5_CLOCK; /* Set clock for timer to Clock5 = div 1024*/\ + *AT91C_PMC_PCER = TIMER_2_ID14; /* Enable clock for TC2*/\ + *AT91C_TCB_BMR = AT91C_TCB_TC2XC2S_NONE; /* No external clock signal */\ + *AT91C_TCB_BCR = 0x0; /* Clear SYNC */\ + *AT91C_TC2_CMR = *AT91C_TC2_CMR & 0X00000000; /* Clear all bits in TC1_CMR */\ + *AT91C_TC2_CMR = *AT91C_TC2_CMR & 0xFFFF7FFF; /* Enable capture mode */\ + *AT91C_TC2_CMR = *AT91C_TC2_CMR | AT91C_TC_CLKS_TIMER_DIV5_CLOCK; /* Set clock for timer to Clock5 = div 1024*/\ *AT91C_TC2_CMR = *AT91C_TC2_CMR | AT91C_TC_ABETRG; /* Use external trigger for TIO2*/\ - *AT91C_TC2_CMR = *AT91C_TC2_CMR | AT91C_TC_EEVTEDG_BOTH; /* Trigger on both edges */\ + *AT91C_TC2_CMR = *AT91C_TC2_CMR | AT91C_TC_EEVTEDG_BOTH; /* Trigger on both edges */\ *AT91C_TC2_CMR = *AT91C_TC2_CMR | AT91C_TC_LDRA_RISING; /* RA loading register set */\ *AT91C_AIC_IDCR = TIMER_2_ID14; /* Irq controller setup */\ AT91C_AIC_SVR[14] = (unsigned int)CaptureBInt; \ @@ -145,13 +145,13 @@ static TACHOPARAMETERS MotorTachoValue[3]; *AT91C_PIOA_PPUDR = MOTOR_C_INT | MOTOR_C_DIR; /* Disable Pull Up resistor on PA0 & PA08 */\ *AT91C_PIOA_ODR = MOTOR_C_DIR; /* PA08 set to input */\ *AT91C_PIOA_IFER = MOTOR_C_INT | MOTOR_C_DIR; /* Enable filter on PA26 & PA09 */\ - *AT91C_PMC_PCER = TIMER_0_ID12; /* Enable clock for TC0*/\ - *AT91C_TCB_BMR = AT91C_TCB_TC0XC0S_NONE; /* No external clock signal */\ - *AT91C_TC0_CMR = *AT91C_TC0_CMR & 0X00000000; /* Clear all bits in TC0_CMR */\ - *AT91C_TC0_CMR = *AT91C_TC0_CMR & 0xFFFF7FFF; /* Enable capture mode */\ - *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_CLKS_TIMER_DIV5_CLOCK; /* Set clock for timer to Clock5 = div 1024*/\ - *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_ABETRG; /* Use external trigger for TI0*/\ - *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_EEVTEDG_BOTH; /* Trigger on both edges */\ + *AT91C_PMC_PCER = TIMER_0_ID12; /* Enable clock for TC0*/\ + *AT91C_TCB_BMR = AT91C_TCB_TC0XC0S_NONE; /* No external clock signal */\ + *AT91C_TC0_CMR = *AT91C_TC0_CMR & 0X00000000; /* Clear all bits in TC0_CMR */\ + *AT91C_TC0_CMR = *AT91C_TC0_CMR & 0xFFFF7FFF; /* Enable capture mode */\ + *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_CLKS_TIMER_DIV5_CLOCK; /* Set clock for timer to Clock5 = div 1024*/\ + *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_ABETRG; /* Use external trigger for TI0*/\ + *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_EEVTEDG_BOTH; /* Trigger on both edges */\ *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_LDRA_RISING; /* RA loading register set */\ *AT91C_AIC_IDCR = TIMER_0_ID12; /* Disable interrupt */\ AT91C_AIC_SVR[12] = (unsigned int)CaptureCInt; \ @@ -270,29 +270,29 @@ __ramfunc void CaptureCInt(void) } } -#define OUTPUTExit {\ - *AT91C_AIC_IDCR = TIMER_0_ID12 | TIMER_1_ID13 | TIMER_2_ID14; /* Disable interrupts for the timers */\ - *AT91C_AIC_ICCR = TIMER_0_ID12 | TIMER_1_ID13 | TIMER_2_ID14; /* Clear penting interrupt register for timers*/\ - *AT91C_PMC_PCDR = TIMER_0_ID12 | TIMER_1_ID13 | TIMER_2_ID14; /* Disable the clock for each of the timers*/\ - *AT91C_PIOA_PER = MOTOR_A_DIR | MOTOR_A_INT | MOTOR_B_DIR | MOTOR_B_INT | MOTOR_C_DIR | MOTOR_C_INT; /* Enable PIO on PA15, PA11, PA26, PA09, PA27 & PA08 */\ - *AT91C_PIOA_ODR = MOTOR_A_DIR | MOTOR_A_INT | MOTOR_B_DIR | MOTOR_B_INT | MOTOR_C_DIR | MOTOR_C_INT; /* Set to input PA15, PA11, PA26, PA09, PA27 & PA08 */\ - *AT91C_PIOA_PPUDR = MOTOR_A_DIR | MOTOR_A_INT | MOTOR_B_DIR | MOTOR_B_INT | MOTOR_C_DIR | MOTOR_C_INT; /* Enable Pullup on PA15, PA11, PA26, PA09, PA27 & PA08 */\ - } +#define OUTPUTExit {\ + *AT91C_AIC_IDCR = TIMER_0_ID12 | TIMER_1_ID13 | TIMER_2_ID14; /* Disable interrupts for the timers */\ + *AT91C_AIC_ICCR = TIMER_0_ID12 | TIMER_1_ID13 | TIMER_2_ID14; /* Clear penting interrupt register for timers*/\ + *AT91C_PMC_PCDR = TIMER_0_ID12 | TIMER_1_ID13 | TIMER_2_ID14; /* Disable the clock for each of the timers*/\ + *AT91C_PIOA_PER = MOTOR_A_DIR | MOTOR_A_INT | MOTOR_B_DIR | MOTOR_B_INT | MOTOR_C_DIR | MOTOR_C_INT; /* Enable PIO on PA15, PA11, PA26, PA09, PA27 & PA08 */\ + *AT91C_PIOA_ODR = MOTOR_A_DIR | MOTOR_A_INT | MOTOR_B_DIR | MOTOR_B_INT | MOTOR_C_DIR | MOTOR_C_INT; /* Set to input PA15, PA11, PA26, PA09, PA27 & PA08 */\ + *AT91C_PIOA_PPUDR = MOTOR_A_DIR | MOTOR_A_INT | MOTOR_B_DIR | MOTOR_B_INT | MOTOR_C_DIR | MOTOR_C_INT; /* Enable Pullup on PA15, PA11, PA26, PA09, PA27 & PA08 */\ + } -#define TACHOCountReset(MotorNr) {\ - MotorTachoValue[MotorNr].TachoCountTable = 0;\ - MotorTachoValue[MotorNr].TachoCountTableOld = 0;\ - } +#define TACHOCountReset(MotorNr) {\ + MotorTachoValue[MotorNr].TachoCountTable = 0;\ + MotorTachoValue[MotorNr].TachoCountTableOld = 0;\ + } #define TACHOCaptureReadResetAll(MotorDataA,MotorDataB,MotorDataC){\ - MotorDataA = (MotorTachoValue[MOTOR_A].TachoCountTable - MotorTachoValue[MOTOR_A].TachoCountTableOld);\ - MotorTachoValue[MOTOR_A].TachoCountTableOld = MotorTachoValue[MOTOR_A].TachoCountTable;\ - MotorDataB = (MotorTachoValue[MOTOR_B].TachoCountTable - MotorTachoValue[MOTOR_B].TachoCountTableOld);\ - MotorTachoValue[MOTOR_B].TachoCountTableOld = MotorTachoValue[MOTOR_B].TachoCountTable;\ - MotorDataC = (MotorTachoValue[MOTOR_C].TachoCountTable - MotorTachoValue[MOTOR_C].TachoCountTableOld);\ - MotorTachoValue[MOTOR_C].TachoCountTableOld = MotorTachoValue[MOTOR_C].TachoCountTable;\ - } + MotorDataA = (MotorTachoValue[MOTOR_A].TachoCountTable - MotorTachoValue[MOTOR_A].TachoCountTableOld);\ + MotorTachoValue[MOTOR_A].TachoCountTableOld = MotorTachoValue[MOTOR_A].TachoCountTable;\ + MotorDataB = (MotorTachoValue[MOTOR_B].TachoCountTable - MotorTachoValue[MOTOR_B].TachoCountTableOld);\ + MotorTachoValue[MOTOR_B].TachoCountTableOld = MotorTachoValue[MOTOR_B].TachoCountTable;\ + MotorDataC = (MotorTachoValue[MOTOR_C].TachoCountTable - MotorTachoValue[MOTOR_C].TachoCountTableOld);\ + MotorTachoValue[MOTOR_C].TachoCountTableOld = MotorTachoValue[MOTOR_C].TachoCountTable;\ + } -- cgit v1.2.3