c_cmd.c

乐高机器人的源码,开发平台是IAR_for_AVR.

{
  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode;
  UWORD i, Count;
  UBYTE *pVal;

  NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));

  TypeCode = cCmdDSType(DSElementID);

  if (TypeCode == TC_CLUSTER)
  {
    Count = cCmdClusterCount(DSElementID);
    //Advance DSElementID to sub-type
    DSElementID = INC_ID(DSElementID);
    //Loop through sub-types, inflate recursively
    for (i = 0; i < Count; i++)
    {
      Status = cCmdInflateDSDefaults(pDSDefaults, pDefaultsOffset, DSElementID);
      if (IS_ERR(Status))
        return Status;
      DSElementID = cCmdNextDSElement(DSElementID);
    }
  }
  else
  {
    if (TypeCode == TC_ARRAY)
    {
      //Resolve pointer to DVIndex
      pVal = VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset;
    }
    else
    {
      pVal = cCmdResolveDataArg(DSElementID, 0, NULL);
    }

    //Check if the element has the "default default"
    if (VarsCmd.pDataspaceTOC[DSElementID].Flags & DS_DEFAULT_DEFAULT)
    {
      //Fill element with the "default default" of zero
      memset(pVal, 0, cCmdSizeOf(TypeCode));
    }
    else
    {
      //Get default from stream
      memmove(pVal, pDSDefaults + *pDefaultsOffset, cCmdSizeOf(TypeCode));
      *pDefaultsOffset += cCmdSizeOf(TypeCode);
    }
  }

  //!!! Currently will always return NO_ERR
  return Status;
}


NXT_STATUS cCmdActivateProgram(UBYTE * pFileName)
{
  UWORD i, j;
  UBYTE * pCursor;

  NXT_STATUS Status = NO_ERR;
  PROG_FILE_OFFSETS FileOffsets;

  LOADER_STATUS LStatus;
  ULONG DataSize;
  UBYTE * pData;
  ULONG pDataHolder;
  UWORD DefaultsOffset;

  LStatus = pMapLoader->pFunc(OPENREADLINEAR, pFileName, (UBYTE*)(&pDataHolder), &DataSize);
  pData = (UBYTE*)(pDataHolder);

  //If Loader returned error or bad file pointer, bail out
  if (LOADER_ERR(LStatus) != SUCCESS || pData == NULL || DataSize == 0)
    return (ERR_FILE);

  //Deactivate current program and re-initialize memory pool
  cCmdDeactivateProgram();
  cCmdInitPool();

  //Stash this program's handle since we hold it open while running
  VarsCmd.ActiveProgHandle = LOADER_HANDLE(LStatus);

  //Stash this program's name for easy reference later
  strncpy((PSZ)(VarsCmd.ActiveProgName), (PSZ)(pFileName), FILENAME_LENGTH + 1);

  //Consume activation record data stream.
  //See TargettingVIs/NXT.PackAR.vi for data stream packing details

  //Read header portion of the file, calculating offsets and initializing VarsCmd
  Status = cCmdReadFileHeader(pData, DataSize, &FileOffsets);
  if (IS_ERR(Status))
    return Status;

  //Do some spot checks to make sure bad file contents didn't leave us with obviously insane VarsCmd contents
  //!!! Should add alignment checks on these pointers to avoid data abort exceptions later
  if (((UBYTE*)(VarsCmd.pCodespace) < pData)
   || ((UBYTE*)(VarsCmd.pCodespace) >= (pData + DataSize))
   || ((UBYTE*)(VarsCmd.pAllClumps) < POOL_START)
   || ((UBYTE*)(VarsCmd.pAllClumps) >= POOL_SENTINEL)
   || ((UBYTE*)(VarsCmd.pDataspace) < POOL_START)
   || ((UBYTE*)(VarsCmd.pDataspace) >= POOL_SENTINEL)
   || (VarsCmd.DataspaceSize == 0) )
  {
    NXT_BREAK;
    return ERR_FILE;
  }

  //Initialize CLUMP_RECs as contiguous list in RAM
  pCursor = (pData + FileOffsets.Clumps);
  for (i = 0; i < VarsCmd.AllClumpsCount; i++)
  {
    VarsCmd.pAllClumps[i].InitFireCount  = *(UBYTE*)(pCursor + i * VM_FILE_CLUMP_REC_SIZE);
    VarsCmd.pAllClumps[i].DependentCount = *(UBYTE*)(pCursor + (i * VM_FILE_CLUMP_REC_SIZE) + 1);
    VarsCmd.pAllClumps[i].CodeStart      = *(UWORD*)(pCursor + (i * VM_FILE_CLUMP_REC_SIZE) + 2);

    //Initialize remaining CLUMP_REC fields
    VarsCmd.pAllClumps[i].PC = 0;
    VarsCmd.pAllClumps[i].Priority = 20;
    VarsCmd.pAllClumps[i].Link = NOT_A_CLUMP;

    //Activate any clumps with CurrFireCount of 0
    VarsCmd.pAllClumps[i].CurrFireCount = VarsCmd.pAllClumps[i].InitFireCount;
    if (VarsCmd.pAllClumps[i].CurrFireCount == 0)
      cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)i);
  }

  //Patch up dependents in separate pass (reuse of pCursor)
  pCursor += VarsCmd.AllClumpsCount * VM_FILE_CLUMP_REC_SIZE;
  for (i = 0; i < VarsCmd.AllClumpsCount; i++)
  {
    if (VarsCmd.pAllClumps[i].DependentCount > 0)
    {
      VarsCmd.pAllClumps[i].pDependents = (CLUMP_ID*)(pCursor);

      pCursor += (VarsCmd.pAllClumps[i].DependentCount * sizeof(CLUMP_ID));
    }
    else
      VarsCmd.pAllClumps[i].pDependents = NULL;

    //Patch up CodeEnd value based on CodeStart of next clump or last overall codeword
    if (i < (VarsCmd.AllClumpsCount - 1))
      VarsCmd.pAllClumps[i].CodeEnd = VarsCmd.pAllClumps[i+1].CodeStart - 1;
    else
      VarsCmd.pAllClumps[i].CodeEnd = VarsCmd.CodespaceCount - 1;

    //Test for empty/insane clump code definitions
    NXT_ASSERT(VarsCmd.pAllClumps[i].CodeStart < VarsCmd.pAllClumps[i].CodeEnd);
  }

  //Programs with no active clumps constitutes an activation error
  if (VarsCmd.RunQ.Head == NOT_A_CLUMP)
    return (ERR_FILE);

  //Initialize dataspace with default values from file
  //!!! This would be a good place to enforce check against potentially
  //    unsafe nested types (deeply nested types mean deep recursive calls)
  DefaultsOffset = 0;
  for (i = 0; i != NOT_A_DS_ID; i = cCmdNextDSElement(i))
  {

    Status = cCmdInflateDSDefaults(pData + FileOffsets.DSDefaults, &DefaultsOffset, i);
    if (IS_ERR(Status))
      return Status;
  }

  if ((DefaultsOffset != FileOffsets.DynamicDefaults)
   || (DefaultsOffset + FileOffsets.DynamicDefaultsSize != FileOffsets.DSDefaultsSize))
  {
    NXT_BREAK;
    return (ERR_FILE);
  }

  //Copy Dynamic defaults from file
  memmove(VarsCmd.pDataspace + VarsCmd.DSStaticSize, pData + FileOffsets.DSDefaults + FileOffsets.DynamicDefaults, FileOffsets.DynamicDefaultsSize);

  //Verify the MemMgr ended up where we said it would
  if ((UBYTE *)VarsCmd.MemMgr.pDopeVectorArray != VarsCmd.pDataspace + DV_ARRAY[0].Offset)
  {
    NXT_BREAK;
    return (ERR_FILE);
  }

  //Initialize message queues
  for (i = 0; i < MESSAGE_QUEUE_COUNT; i++)
  {
    VarsCmd.MessageQueues[i].ReadIndex = 0;
    VarsCmd.MessageQueues[i].WriteIndex = 0;

    for (j = 0; j < MESSAGES_PER_QUEUE; j++)
    {
      VarsCmd.MessageQueues[i].Messages[j] = NOT_A_DS_ID;
    }
  }

  if (cCmdVerifyMemMgr() != TRUE)
    return (ERR_FILE);

  return (Status);
}


void cCmdDeactivateProgram()
{
  UBYTE i, tmp;

  //Wipe away all references into the pool and clear all run-time data
  VarsCmd.pCodespace = NULL;
  VarsCmd.CodespaceCount = 0;

  VarsCmd.pAllClumps = NULL;
  VarsCmd.AllClumpsCount = 0;

  VarsCmd.DataspaceCount = 0;
  VarsCmd.pDataspaceTOC = NULL;
  VarsCmd.pDataspace = NULL;
  VarsCmd.DataspaceSize = 0;
  VarsCmd.DSStaticSize = 0;

  VarsCmd.MemMgr.Head = NOT_A_DS_ID;
  VarsCmd.MemMgr.Tail = NOT_A_DS_ID;
  VarsCmd.MemMgr.FreeHead = NOT_A_DS_ID;
  VarsCmd.MemMgr.pDopeVectorArray = NULL;

  VarsCmd.RunQ.Head = NOT_A_CLUMP;
  VarsCmd.RunQ.Tail = NOT_A_CLUMP;
  VarsCmd.ScratchPC = 0;
  VarsCmd.CallerClump = NOT_A_CLUMP;

  if (VarsCmd.ActiveProgHandle != NOT_A_HANDLE)
  {
    //Close handle that we've kept open for this program
    pMapLoader->pFunc(CLOSE, &(VarsCmd.ActiveProgHandle), NULL, NULL);
    VarsCmd.ActiveProgHandle = NOT_A_HANDLE;

    //Clear internal stashed name
    memset(VarsCmd.ActiveProgName, 0, FILENAME_LENGTH + 1);
  }

  //Close any files we had opened programatically
  for (i = 0; i < MAX_HANDLES; i++)
  {
    //Copy i to tmp, because we pass a pointer to it to pFunc
    tmp = i;
    //Close file
    if (*(VarsCmd.FileHandleTable[i]) != 0)
      pMapLoader->pFunc(CLOSE, &tmp, NULL, NULL);
  }

  //Clear FileHandleTable
  memset(VarsCmd.FileHandleTable, 0, sizeof(VarsCmd.FileHandleTable));

  return;
}


void cCmdResetDevices(void)
{
  UBYTE i;

  //Clear NXT button counts so 'bumped' will work on first run
  for (i = 0; i < NO_OF_BTNS; i++)
  {
    pMapButton->BtnCnt[i].RelCnt = 0;
    //Need to clear short and long counts too, because RelCnt depends on them.  No known side effects.
    pMapButton->BtnCnt[i].ShortRelCnt = 0;
    pMapButton->BtnCnt[i].LongRelCnt = 0;
  }

  for (i = 0; i < NO_OF_INPUTS; i++)
  {
    //Clear type and mode to defaults
    pMapInput->Inputs[i].SensorType = NO_SENSOR;
    pMapInput->Inputs[i].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;

    //Assert invalid data flag so future code is aware of these changes
    pMapInput->Inputs[i].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;
  }

  //Lowspeed init, INSERT CODE !!!
  for (i = 0; i < NO_OF_LOWSPEED_COM_CHANNEL; i++)
  {
    pMapLowSpeed->InBuf[i].InPtr = 0;
    pMapLowSpeed->InBuf[i].OutPtr = 0;
    pMapLowSpeed->InBuf[i].BytesToRx = 0;
    pMapLowSpeed->OutBuf[i].InPtr = 0;
    pMapLowSpeed->OutBuf[i].OutPtr = 0;
    if (pMapLowSpeed->ChannelState[i] != LOWSPEED_IDLE)
    {
      pMapLowSpeed->ChannelState[i] = LOWSPEED_DONE;
      pMapLowSpeed->State |= (0x01<<i);
    }
  }

}


//Add NewClump to end, updating Queue's head/tail as needed
void cCmdEnQClump(CLUMP_Q * Queue, CLUMP_ID NewClump)
{
  //Make sure NewClump's ID is valid and not already on Q
  NXT_ASSERT(cCmdIsClumpIDSane(NewClump));
  NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
  NXT_ASSERT(!cCmdIsClumpOnQ(Queue, NewClump));

  VarsCmd.pAllClumps[NewClump].Link = NOT_A_CLUMP;

  //If queue is empty, NewClump becomes both head and tail
  if (Queue->Head == NOT_A_CLUMP)
  {
    NXT_ASSERT(Queue->Tail == NOT_A_CLUMP);

    Queue->Head = NewClump;
    Queue->Tail = NewClump;
  }
  //Otherwise, tack onto the end
  else
  {
    VarsCmd.pAllClumps[Queue->Tail].Link = NewClump;
    Queue->Tail = NewClump;
  }

  return;
}

//Dequeue specified clump
//Normal usage is to dequeue only from the head (i.e. pass Queue.Head as arg)
void cCmdDeQClump(CLUMP_Q * Queue, CLUMP_ID Clump)
{
  CLUMP_ID CurrID, LinkID;

  //Make sure Clump's ID is valid and is already on Queue
  NXT_ASSERT(cCmdIsClumpIDSane(Clump));
  NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
  NXT_ASSERT(cCmdIsClumpOnQ(Queue, Clump));

  CurrID = Queue->Head;

  //If our clump is the head, move up the next and disconnect
  if (CurrID == Clump)
  {
    Queue->Head = VarsCmd.pAllClumps[Clump].Link;
    VarsCmd.pAllClumps[Clump].Link = NOT_A_CLUMP;

    //If we just removed the last clump, patch up the queue's tail
    if (Queue->Head == NOT_A_CLUMP)
      Queue->Tail = NOT_A_CLUMP;
  }
  //Else, look through rest of list looking for a link to our clump
  else
  {
    do
    {
      LinkID = VarsCmd.pAllClumps[CurrID].Link;

      //If we find a link to our clump, patch up predecessor's link
      if (VarsCmd.pAllClumps[CurrID].Link == Clump)
      {
        VarsCmd.pAllClumps[CurrID].Link = VarsCmd.pAllClumps[Clump].Link;
        VarsCmd.pAllClumps[Clump].Link = NOT_A_CLUMP;

        //If we just removed the tail, patch tail
        if (Clump == Queue->Tail)
          Queue->Tail = CurrID;
      }

      CurrID = LinkID;
    } while (CurrID != NOT_A_CLUMP);
  }

  return;
}


//Rotate head to tail and advance head for given Queue
void cCmdRotateQ(CLUMP_Q * Queue)
{
  CLUMP_ID CurrID;
  CLUMP_REC * pClumpRec;

  //Make sure Queue is sane
  NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);

  //If queue has at least two clumps
  if (Queue->Head != Queue->Tail)
  {
    CurrID = Queue->Head;
    pClumpRec = &(VarsCmd.pAllClumps[CurrID]);

    //Disconnect head
    Queue->Head = pClumpRec->Link;
    pClumpRec->Link = NOT_A_CLUMP;

    //Reconnect head as tail
    pClumpRec = &(VarsCmd.pAllClumps[Queue->Tail]);
    pClumpRec->Link = CurrID;
    Queue->Tail = CurrID;

    //Make sure we didn't make any really stupid mistakes
    NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
  }

  return;
}


UBYTE cCmdIsClumpOnQ(CLUMP_Q * Queue, CLUMP_ID Clump)
{
  CLUMP_ID CurrID;

  //Make sure Clump's ID is valid and is already on Queue
  NXT_ASSERT(cCmdIsClumpIDSane(Clump));
  NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);

  CurrID = Queue->Head;

  while (CurrID != NOT_A_CLUMP)
  {
    if (CurrID == Clump)
      return TRUE;

    CurrID = VarsCmd.pAllClumps[CurrID].Link;