c_cmd.c

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

          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoCnt)), 4);
          ResponseLen += 4;

          //BlockTachoCount SLONG
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].BlockTachoCount)), 4);
          ResponseLen += 4;

          //RotationCount SLONG
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].RotationCount)), 4);
          ResponseLen += 4;

          NXT_ASSERT(ResponseLen == 23);
        }
      }
      break;

      case RC_GET_IN_VALS:
      {
        if (SendResponse == TRUE)
        {
          i = pInBuf[1];

          //Return error and all zeros if illegal port specification is made
          if (i >= NO_OF_INPUTS)
          {
            RCStatus = ERR_RC_ILLEGAL_VAL;
            memset(&(pOutBuf[ResponseLen]), 0, 13);
            ResponseLen += 13;
            break;
          }

          //Echo port
          pOutBuf[ResponseLen] = i;
          ResponseLen++;

          //Set "Valid?" boolean
          if (pMapInput->Inputs[i].InvalidData)
            pOutBuf[ResponseLen] = FALSE;
          else
            pOutBuf[ResponseLen] = TRUE;

          ResponseLen++;

          //Set "Calibrated?" boolean
          //!!! "Calibrated?" is a placeholder in the protocol.  Always FALSE for now.
          pOutBuf[ResponseLen] = FALSE;
          ResponseLen++;

          pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorType;
          ResponseLen++;

          pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorMode;
          ResponseLen++;

          //Set Raw, Normalized, and Scaled values
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].ADRaw)), 2);
          ResponseLen += 2;

          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorRaw)), 2);
          ResponseLen += 2;

          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].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);
          ResponseLen += 2;

          NXT_ASSERT(ResponseLen == 14);
        }
      }
      break;

      case RC_RESET_IN_VAL:
      {
        i = pInBuf[1];

        //Don't do anything if illegal port specification is made
        if (i >= NO_OF_INPUTS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        //Clear SensorValue to zero.  Leave Raw and Normalized as-is, since they never accumulate running values.
        pMapInput->Inputs[i].SensorValue = 0;
      }
      break;

      case RC_MESSAGE_WRITE:
      {
        QueueID = pInBuf[1];
        Count = pInBuf[2];
        pData = &(pInBuf[3]);

        //If Count is illegal or MsgData is not null-terminated,
        // we can't accept it as a valid string
        if (Count == 0 || Count > MAX_MESSAGE_SIZE || pData[Count - 1] != 0x00)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        RCStatus = cCmdMessageWrite(QueueID, pData, Count);

        //ERR_MEM here means we must compact the dataspace and retry message write
        if (RCStatus == ERR_MEM)
        {
          cCmdDSCompact();
          RCStatus = cCmdMessageWrite(QueueID, pData, Count);
        }
      }
      break;

      case RC_RESET_POSITION:
      {
        i = pInBuf[1];

        //Don't do anything if illegal port specification is made
        if (i >= NO_OF_OUTPUTS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        //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;
        }
      }
      break;

      case RC_GET_BATT_LVL:
      {
        if (SendResponse == TRUE)
        {
          //Return BatteryVoltage directly from IOMapUI, in mV
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&(pMapUi->BatteryVoltage), 2);
          ResponseLen += 2;
        }
      }
      break;

      case RC_STOP_SOUND:
      {
        //Tell sound module to stop playback, no questions asked
        pMapSound->State = SOUND_STOP;
      }
      break;

      case RC_KEEP_ALIVE:
      {
        pMapUi->Flags |= UI_RESET_SLEEP_TIMER;

        if (SendResponse == TRUE)
        {
          //Convert to milliseconds to match external conventions
          i = (pMapUi->SleepTimeout * 60 * 1000);
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&i, 4);
          ResponseLen += 4;
        }
      }
      break;

      case RC_LS_GET_STATUS:
      {
        if (SendResponse == TRUE)
        {
          i = pInBuf[1];

          //Don't do anything if illegal port specification is made
          if (i >= NO_OF_LOWSPEED_COM_CHANNEL)
          {
            RCStatus = ERR_RC_ILLEGAL_VAL;
            break;
          }

          RCStatus = cCmdLSCheckStatus(i);

          pOutBuf[ResponseLen] = cCmdLSCalcBytesReady(i);
          ResponseLen++;
        }
      }
      break;

      case RC_LS_WRITE:
      {
        i = pInBuf[1];
        Count = pInBuf[2];

        //Don't do anything if illegal port specification is made
        if (i >= NO_OF_LOWSPEED_COM_CHANNEL)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        RCStatus = cCmdLSWrite(i, Count, &(pInBuf[4]), pInBuf[3]);
      }
      break;

      case RC_LS_READ:
      {
        if (SendResponse == TRUE)
        {
          i = pInBuf[1];

          //Don't do anything if illegal port specification is made
          if (i >= NO_OF_LOWSPEED_COM_CHANNEL)
          {
            RCStatus = ERR_RC_ILLEGAL_VAL;
            break;
          }

          //Get channel status and number of bytes available to read
          RCStatus = cCmdLSCheckStatus(i);
          Count = cCmdLSCalcBytesReady(i);

          pOutBuf[ResponseLen] = (UBYTE)Count;
          ResponseLen++;

          //If channel is ready and has data ready for us, put the data into outgoing buffer
          if (!IS_ERR(RCStatus) && Count > 0)
          {
            RCStatus = cCmdLSRead(i, (UBYTE)Count, &(pOutBuf[ResponseLen]));
            ResponseLen += Count;
          }

          //Pad remaining data bytes with zeroes
          Count = 16 - Count;
          memset(&(pOutBuf[ResponseLen]), 0, Count);
          ResponseLen += Count;
        }
      }
      break;

      case RC_GET_CURR_PROGRAM:
      {
        if (SendResponse == TRUE)
        {
          //If there's no active program, return error and empty name buffer
          if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
          {
            RCStatus = ERR_NO_PROG;
            memset(&(pOutBuf[ResponseLen]), 0, FILENAME_LENGTH + 1);
          }
          //Else, copy out stashed program name
          else
          {
            strncpy((PSZ)(&(pOutBuf[ResponseLen])), (PSZ)(VarsCmd.ActiveProgName), FILENAME_LENGTH + 1);
          }

          //Regardless, we've copied out a filename's worth of bytes...
          ResponseLen += FILENAME_LENGTH + 1;
        }
      }
      break;

      case RC_MESSAGE_READ:
      {
        if (SendResponse == TRUE)
        {
          QueueID = pInBuf[1];

          //Fill in response with remote mailbox number so remote device knows where to store this message.
          pOutBuf[ResponseLen] = pInBuf[2];
          ResponseLen++;

          RCStatus = cCmdMessageGetSize(QueueID, &Count);
          pOutBuf[ResponseLen] = Count;
          ResponseLen++;

          if (!IS_ERR(RCStatus) && Count > 0)
          {
            pData = &(pOutBuf[ResponseLen]);
            RCStatus = cCmdMessageRead(QueueID, pData, Count, (pInBuf[3]));
            //If cCmdMessageRead encountered an error, there is no real data in the buffer, so clear it out (below)
            if (IS_ERR(RCStatus))
              Count = 0;
            else
              ResponseLen += Count;
          }

          //Pad remaining data bytes with zeroes
          Count = MAX_MESSAGE_SIZE - Count;
          memset(&(pOutBuf[ResponseLen]), 0, Count);
          ResponseLen += Count;
        }
      }
      break;

      default:
      {
        //Unknown remote command -- still inform client to not expect any response bytes
        NXT_BREAK;
        RCStatus = ERR_RC_UNKNOWN_CMD;
      }
      break;
    };
  }
  //Handle reply telegrams
  else
  {
    switch(pInBuf[0])
    {
      case RC_MESSAGE_READ:
      {
        QueueID = pInBuf[2];
        Count   = pInBuf[3];
        pData = &(pInBuf[4]);

        //This is a response to our request to read a message from a remote mailbox.
        //If telegram looks valid, write the resulting message into our local mailbox.
        //(If MsgData is not null-terminated, we can't accept it as a valid string.)
        if (!IS_ERR((SBYTE)(pInBuf[1]))
         && Count > 0
         && Count <= MAX_MESSAGE_SIZE
         && pData[Count - 1] == 0x00)
        {
          RCStatus = cCmdMessageWrite(QueueID, pData, Count);

          //ERR_MEM here means we must compact the dataspace
          if (RCStatus == ERR_MEM)
          {
            cCmdDSCompact();
            RCStatus = cCmdMessageWrite(QueueID, pData, Count);
          }
        }

        //If telegram doesn't check out, do nothing.  No errors are ever returned for reply telegrams.
      }
      break;

      default:
      {
        //Unhandled reply telegram.  Do nothing.
        //!!! Could/should stash unhandled/all replies somewhere so a syscall could read them
      }
      break;
    };
  }

  if (SendResponse == TRUE)
  {
    //Return response length (pointer checked above)
    *pLen = (UBYTE)ResponseLen;
    //Fill in status byte
    pOutBuf[0] = (UBYTE)(RCStatus);
  }
  else
    *pLen = 0;

  return (0);
}


//
// Standard interface functions
//

void      cCmdInit(void* pHeader)
{
  ULONG i;

  pHeaders        = pHeader;

  IOMapCmd.pRCHandler = &cCmdHandleRemoteCommands;

#if defined(ARM_DEBUG)
  //Init run-time assert tracking variables
  VarsCmd.AssertFlag = FALSE;
  VarsCmd.AssertLine = 0;
#endif

  //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);
  }

  //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);
  }

  //Clear memory pool and initialize VarsCmd (cCmdDeactivateProgram effectively re-inits VarsCmd)
  cCmdInitPool();
  cCmdDeactivateProgram();

  //Global state variables for BlueTooth communication.
  VarsCmd.CommStat      = (SWORD)SUCCESS;
  VarsCmd.CommStatReset = (SWORD)BTBUSY;
  VarsCmd.CommCurrConnection = 1;

  //Global flags for various reset and bookkeeping scenarios
  VarsCmd.DirtyComm = FALSE;
  VarsCmd.DirtyDisplay = FALSE;

  VarsCmd.VMState = VM_IDLE;

#if defined (ARM_NXT)
  //Make sure Pool is long-aligned
  NXT_ASSERT(!((ULONG)(POOL_START) % SIZE_SLONG));
#endif

  IOMapCmd.ProgStatus = PROG_IDLE;
  IOMapCmd.ActivateFlag = FALSE;
  IOMapCmd.Awake  = TRUE;

  //Default offsets explicitly chosen to cause an error if used with IOMAPREAD/IOMAPWRITE
  //Real values will be set when programs run and/or the DS is re-arranged.
  IOMapCmd.OffsetDVA = 0xFFFF;