void initLogs() {
INITLOG(TAG);
// Para desactivar niveles de logs
if (verboseLevel==0)
LOG_LEVEL(LOG_ERR);
else if (verboseLevel==1)
LOG_LEVEL(LOG_NOTICE);
else
LOG_LEVEL(LOG_DEBUG);
}
/*----------------------------------------------------------------------------*/
BOOL
DllEntry(HANDLE hDLL,
DWORD u4Reason,
LPVOID lpReserved)
{
DEBUGFUNC("DllEntry");
switch (u4Reason) {
case DLL_PROCESS_ATTACH:
CREATE_LOG_FILE();
INITLOG(("MT6620: DLL_PROCESS_ATTACH\n"));
DisableThreadLibraryCalls((HMODULE) hDLL);
break;
case DLL_PROCESS_DETACH:
INITLOG(("MT6620: DLL_PROCESS_DETTACH\n"));
break;
default:
INITLOG(("MT6620: Fail\n"));
break;
}
return TRUE;
} /* DllEntry */
/*----------------------------------------------------------------------------*/
NDIS_STATUS
windowsUnregisterIsrt (
IN P_GLUE_INFO_T prGlueInfo
)
{
P_GL_HIF_INFO_T prHifInfo;
DEBUGFUNC("windowsUnregisterIsrt");
ASSERT(prGlueInfo);
prHifInfo = &prGlueInfo->rHifInfo;
if (GLUE_TEST_FLAG(prGlueInfo, GLUE_FLAG_INTERRUPT_IN_USE)) {
NdisMDeregisterInterrupt(&prHifInfo->rInterrupt);
GLUE_CLEAR_FLAG(prGlueInfo, GLUE_FLAG_INTERRUPT_IN_USE);
INITLOG(("Interrupt deregistered\n"));
}
return NDIS_STATUS_SUCCESS;
} /* windowsUnregisterIsrt */
int main (int argc, char *argv[])
{
bool _headinitialized = false;
bool _arminitialized = false;
bool _headrunning = false;
bool _armrunning = false;
double *_headjointstore = NULL;
double *_armjointstore = NULL;
/// init robot.
INITLOG ();
PRINTLOG ("Log started\n");
FLUSHLOG();
/// head (bus 1).
ValueCanOpenParameters op_par;
memcpy (op_par._destinations, _destinations, sizeof(unsigned char) * CANBUS_MAXCARDS);
op_par._my_address = CANBUS_MY_ADDRESS; /// my address.
op_par._polling_interval = CANBUS_POLLING_INTERVAL; /// thread polling interval [ms].
op_par._timeout = CANBUS_TIMEOUT; /// approx this value times the polling interval [ms].
op_par._njoints = MAX_HEAD_JNTS;
op_par._p = NULL;
if (head.open ((void *)&op_par) < 0)
{
head.close();
ACE_OS::printf ("head driver open failed\n");
}
else
{
// filters out certain messages.
int msg = 20;
head.IOCtl(CMDSetDebugMessageFilter, (void *)&msg);
_headinitialized = true;
}
/// arm (bus 2).
ValueCanOpenParameters op_par2;
op_par2._njoints = MAX_ARM_JNTS;
op_par2._p = PRINTLOG;
memcpy (op_par2._destinations, _destinations, sizeof(unsigned char) * CANBUS_MAXCARDS);
op_par2._my_address = CANBUS_MY_ADDRESS; /// my address.
op_par2._polling_interval = CANBUS_POLLING_INTERVAL; /// thread polling interval [ms].
op_par2._timeout = CANBUS_TIMEOUT; /// approx this value times the polling interval [ms].
if (arm.open ((void *)&op_par2) < 0)
{
arm.close();
ACE_OS::printf ("arm driver open failed\n");
}
else
{
// filters out certain messages.
int msg = 20;
arm.IOCtl(CMDSetDebugMessageFilter, (void *)&msg);
_arminitialized = true;
}
//head.close();
//_headinitialized = false;
if (!_headinitialized && !_arminitialized)
{
ACE_OS::printf ("No device found\n");
return -1;
}
ACE_OS::printf ("The robot is initialized\n");
if (_headinitialized)
{
_headjointstore = new double[MAX_HEAD_JNTS];
ACE_ASSERT (_headjointstore != NULL);
}
if (_arminitialized)
{
_armjointstore = new double[MAX_ARM_JNTS];
ACE_ASSERT (_armjointstore != NULL);
}
ACE_OS::printf ("Running!\n");
int cycle;
for (cycle = 0; ; cycle++)
{
/// read from head.
int ret;
if (_headinitialized)
{
ret = head.IOCtl(CMDGetPositions, _headjointstore);
if (ret != YARP_OK)
ACE_OS::printf ("troubles reading head joint pos\n");
ACE_OS::printf ("head: ");
for (int i = 0; i < MAX_HEAD_JNTS; i++)
{
ACE_OS::printf ("%.2f ", _headjointstore[i]);
}
ACE_OS::printf ("\n");
}
if (_arminitialized)
{
ret = arm.IOCtl(CMDGetPositions, _armjointstore);
if (ret != YARP_OK)
ACE_OS::printf ("troubles reading arm joint pos\n");
ACE_OS::printf ("arm: ");
for (int i = 0; i < MAX_ARM_JNTS; i++)
{
ACE_OS::printf ("%.2f ", _armjointstore[i]);
}
ACE_OS::printf ("\n");
}
Sleep (150);
FLUSHLOG();
}
if (_headinitialized)
{
head.close();
if (_headjointstore != NULL) delete[] _headjointstore;
}
if (_arminitialized)
{
arm.close();
if (_armjointstore != NULL) delete[] _armjointstore;
}
CLOSELOG();
return 0;
}
/*----------------------------------------------------------------------------*/
NDIS_STATUS windowsFindAdapter(IN P_GLUE_INFO_T prGlueInfo, IN NDIS_HANDLE rWrapperConfigurationContext)
{
NDIS_HANDLE rMiniportAdapterHandle;
PDEVICE_EXTENSION prDevExt;
NDIS_STATUS rStatus;
INT i;
UCHAR ucTmp;
SDBUS_INTERFACE_PARAMETERS rInterfaceParameters = { 0 };
DEBUGFUNC("windowsFindAdapter");
DBGLOG1(INIT, TRACE, "\n");
ASSERT(prGlueInfo);
INITLOG(("windowsFindAdapter\n"));
rMiniportAdapterHandle = prGlueInfo->rMiniportAdapterHandle;
prDevExt = &prGlueInfo->rHifInfo.dx;
NdisMGetDeviceProperty(rMiniportAdapterHandle,
&prDevExt->PhysicalDeviceObject,
&prDevExt->FunctionalDeviceObject, &prDevExt->NextLowerDriverObject, NULL, NULL);
rStatus = SdBusOpenInterface(prDevExt->PhysicalDeviceObject,
&prDevExt->BusInterface,
sizeof(SDBUS_INTERFACE_STANDARD), SDBUS_INTERFACE_VERSION);
INITLOG(("SdBusOpenInterface: (status=0x%x)\n", rStatus));
INITLOG(("Size: (0x%x)\n", prDevExt->BusInterface.Size));
INITLOG(("Version: (0x%x)\n", prDevExt->BusInterface.Version));
INITLOG(("Context: (0x%x)\n", prDevExt->BusInterface.Context));
INITLOG(("InterfaceReference: (0x%x)\n", prDevExt->BusInterface.InterfaceReference));
INITLOG(("InterfaceDereference: (0x%x)\n", prDevExt->BusInterface.InterfaceDereference));
INITLOG(("InitializeInterface: (0x%x)\n", prDevExt->BusInterface.InitializeInterface));
INITLOG(("AcknowledgeInterrupt: (0x%x)\n", prDevExt->BusInterface.AcknowledgeInterrupt));
if (NT_SUCCESS(rStatus)) {
rInterfaceParameters.Size = sizeof(SDBUS_INTERFACE_PARAMETERS);
rInterfaceParameters.TargetObject = prDevExt->NextLowerDriverObject;
/* INTerrupt callback function */
rInterfaceParameters.DeviceGeneratesInterrupts = TRUE;
rInterfaceParameters.CallbackAtDpcLevel = FALSE; /* passive level for synchronous I/O */
rInterfaceParameters.CallbackRoutine = sdioINTerruptCallback;
rInterfaceParameters.CallbackRoutineContext = prGlueInfo;
rStatus = STATUS_UNSUCCESSFUL;
if (prDevExt->BusInterface.InitializeInterface) {
INITLOG(("pDevExt->BusINTerface.InitializeINTerface exists\n"));
rStatus = (prDevExt->BusInterface.InitializeInterface)
(prDevExt->BusInterface.Context, &rInterfaceParameters);
}
/* dump sdbus parameter */
/* dump sdbus INTerface standard after init */
if (NT_SUCCESS(rStatus)) {
INITLOG(("INTial SD-bus INTerface OK!!\n"));
prDevExt->busInited = TRUE;
} else {
ERRORLOG(("INTial SD-bus INTerface fail,status:%x\n", rStatus));
if (prDevExt->BusInterface.InterfaceDereference) {
(prDevExt->BusInterface.InterfaceDereference) (prDevExt->BusInterface.Context);
RtlZeroMemory(&prDevExt->BusInterface, sizeof(SDBUS_INTERFACE_STANDARD));
}
return rStatus;
}
} else { /* open SD-bus INTerface fail */
ERRORLOG(("open SD-bus INTerface fail, status:%x\n", rStatus));
return rStatus;
}
rStatus = sdioSetupCardFeature(prGlueInfo, prDevExt);
return rStatus;
} /* end of windowsFindAdapter() */
NDIS_STATUS sdioSetupCardFeature(IN P_GLUE_INFO_T prGlueInfo, IN PDEVICE_EXTENSION prDevExt)
{
NDIS_STATUS rStatus;
UINT_8 ucBusWidth = (UINT_8) prGlueInfo->rRegInfo.u4SdBusWidth;
/* NOTE(Kevin): in ntddsd.h - Length of SDP_FUNCTION_BLOCK_LENGTH == USHORT */
UINT_16 u2BlockLength = (UINT_16) prGlueInfo->rRegInfo.u4SdBlockSize;
UINT_16 u2HostBlockLength;
/* NOTE(Kevin): in ntddsd.h - Length of SDP_BUS_WIDTH == UCHAR */
UINT_32 u4BusDriverVer;
UINT_32 u4BusClock = prGlueInfo->rRegInfo.u4SdClockRate;
/* 4 <1> Check Function Number */
rStatus = sdioConfigProperty(prDevExt,
SDRF_GET_PROPERTY,
SDP_FUNCTION_NUMBER, &prDevExt->FunctionNumber, sizeof(prDevExt->FunctionNumber));
if (!NT_SUCCESS(rStatus)) {
ERRORLOG(("SdBusSubmitRequest fail, status:%x\n", rStatus));
return rStatus;
} else {
INITLOG(("[SDIO] get func. no is %d\n", prDevExt->FunctionNumber));
}
#if 1 /* 20091012 George */
/* */
/* SDIO bus driver version */
rStatus = sdioConfigProperty(prDevExt,
SDRF_GET_PROPERTY,
SDP_BUS_DRIVER_VERSION, (PUINT_8) & u4BusDriverVer, sizeof(u4BusDriverVer));
if (!NT_SUCCESS(rStatus)) {
ERRORLOG(("SdBusSubmitRequest fail, status:%x\n", rStatus));
return rStatus;
} else {
INITLOG(("[SDIO] bus driver version is %d\n", u4BusDriverVer));
}
/* */
/* SDIO HOST_BLOCK_LENGTH */
rStatus = sdioConfigProperty(prDevExt,
SDRF_GET_PROPERTY,
SDP_HOST_BLOCK_LENGTH, (PUINT_8) & u2HostBlockLength, sizeof(u2HostBlockLength));
if (!NT_SUCCESS(rStatus)) {
ERRORLOG(("SdBusSubmitRequest fail, status:%x\n", rStatus));
return rStatus;
} else {
INITLOG(("[SDIO] host block length is %d\n", u2HostBlockLength));
}
/* */
/* SDIO SDP_FN0_BLOCK_LENGTH */
rStatus = sdioConfigProperty(prDevExt,
SDRF_GET_PROPERTY,
SDP_FN0_BLOCK_LENGTH, (PUINT_8) & u2HostBlockLength, sizeof(u2HostBlockLength));
if (!NT_SUCCESS(rStatus)) {
ERRORLOG(("SdBusSubmitRequest fail, status:%x\n", rStatus));
return rStatus;
} else {
INITLOG(("[SDIO] function 0 block length is %d\n", u2HostBlockLength));
}
#endif
/* 4 <2> Setup Block Length */
#ifdef SDBUS_DRIVER_VERSION_2
rStatus = sdioConfigProperty(prDevExt,
SDRF_SET_PROPERTY,
SDP_FUNCTION_BLOCK_LENGTH, (PUINT_8) & u2BlockLength, sizeof(u2BlockLength));
if (!NT_SUCCESS(rStatus)) {
ERRORLOG(("SdBusSubmitRequest fail to set block size, status:%x\n", rStatus));
prGlueInfo->rHifInfo.dx.fgIsSdioBlockModeEnabled = FALSE;
} else {
INITLOG(("[SDIO] set Block size %d\n", u2BlockLength));
prGlueInfo->rHifInfo.dx.fgIsSdioBlockModeEnabled = TRUE;
}
rStatus = sdioConfigProperty(prDevExt,
SDRF_GET_PROPERTY,
SDP_FUNCTION_BLOCK_LENGTH, (PUINT_8) & u2BlockLength, sizeof(u2BlockLength));
if (!NT_SUCCESS(rStatus)) {
ERRORLOG(("SdBusSubmitRequest fail to set block size, status:%x\n", rStatus));
prGlueInfo->rHifInfo.dx.fgIsSdioBlockModeEnabled = FALSE;
prGlueInfo->rHifInfo.u4BlockLength = BLOCK_TRANSFER_LEN;
} else {
prGlueInfo->rHifInfo.u4BlockLength = (UINT_32) u2BlockLength;
INITLOG(("[SDIO] get Block size %d\n", u2BlockLength));
}
/* 4 <3> Setup Bus Width */
rStatus = sdioConfigProperty(prDevExt, SDRF_SET_PROPERTY, SDP_BUS_WIDTH, &ucBusWidth, sizeof(ucBusWidth));
if (!NT_SUCCESS(rStatus))
ERRORLOG(("SdBusSubmitRequest fail to set bus width, status:%x\n", rStatus));
else
INITLOG(("[SDIO] set Bus width %d\n", ucBusWidth));
rStatus = sdioConfigProperty(prDevExt, SDRF_GET_PROPERTY, SDP_BUS_WIDTH, &ucBusWidth, sizeof(ucBusWidth));
if (!NT_SUCCESS(rStatus)) {
ERRORLOG(("SdBusSubmitRequest fail to set bus width, status:%x\n", rStatus));
} else {
prGlueInfo->rHifInfo.u4BusWidth = (UINT_32) ucBusWidth;
INITLOG(("[SDIO] get Bus width %d\n", prGlueInfo->rHifInfo.u4BusWidth));
}
/* 4 <3> Setup Bus Clock */
rStatus = sdioConfigProperty(prDevExt,
SDRF_SET_PROPERTY, SDP_BUS_CLOCK, (PUINT_8) & u4BusClock, sizeof(u4BusClock));
if (!NT_SUCCESS(rStatus))
ERRORLOG(("SdBusSubmitRequest fail to set bus clock, status:%x\n", rStatus));
else
INITLOG(("[SDIO] set Bus clock %d\n", u4BusClock));
rStatus = sdioConfigProperty(prDevExt,
SDRF_GET_PROPERTY, SDP_BUS_CLOCK, (PUINT_8) & u4BusClock, sizeof(u4BusClock));
if (!NT_SUCCESS(rStatus)) {
ERRORLOG(("SdBusSubmitRequest fail to get bus clock, status:%x\n", rStatus));
} else {
prGlueInfo->rHifInfo.u4BusClock = u4BusClock;
INITLOG(("[SDIO] get Bus clock %d\n", prGlueInfo->rHifInfo.u4BusClock));
}
#else
prGlueInfo->rHifInfo.dx.fgIsSdioBlockModeEnabled = FALSE;
prGlueInfo->rHifInfo.u4BlockLength = BLOCK_TRANSFER_LEN;
#endif /* SDP_FUNCTION_BLOCK_LENGTH */
/* emuInitChkCis(prGlueInfo->prAdapter); */
/* emuChkIntEn(prGlueInfo->prAdapter); */
return STATUS_SUCCESS;
}
int main (int argc, char *argv[])
{
bool _headinitialized = false;
bool _arminitialized = false;
bool _headrunning = false;
bool _armrunning = false;
double *_headjointstore = NULL;
double *_armjointstore = NULL;
/// init robot.
INITLOG ();
PRINTLOG ("Log started\n");
FLUSHLOG();
char *root = GetYarpRoot();
char path[512];
ACE_OS::sprintf (path, "%s/%s\0", root, ConfigFilePath);
// initialize head controller on bus 1.
YARPRobotcubHeadParameters parameters;
parameters.load (YARPString(path), YARPString(HEAD_INI_FILE));
parameters._message_filter = 20;
parameters._p = NULL; //xprintf;
int ret = head.initialize(parameters);
if (ret != YARP_OK)
{
_headinitialized = false;
ACE_OS::printf ("Can't start the interface with the robot head, please make sure the hardware and control cards are on.\nThe ini file was: %s%s", path, HEAD_INI_FILE);
}
else
{
_headinitialized = true;
_headrunning = false;
ACE_ASSERT (head.nj() == MAX_HEAD_JNTS); // not other size is allowed.
}
// initialize arm controller on bus 2.
YARPRobotcubArmParameters aparameters;
aparameters.load (YARPString(path), YARPString(ARM_INI_FILE));
aparameters._message_filter = 20;
aparameters._p = (int (*) (char *, ...)) PRINTLOG;
ret = arm.initialize(aparameters);
if (ret != YARP_OK)
{
_arminitialized = false;
ACE_OS::printf ("Can't start the interface with the robot arm/hand, please make sure the hardware and control cards are on.\nThe ini file was: %s%s", path, ARM_INI_FILE);
}
else
{
_arminitialized = true;
_armrunning = false;
ACE_ASSERT (arm.nj() == MAX_ARM_JNTS);
}
if (!_headinitialized && !_arminitialized)
{
ACE_OS::printf ("No device found\n");
return -1;
}
ACE_OS::printf ("The robot is initialized\n");
if (_headinitialized)
{
_headjointstore = new double[head.nj()];
ACE_ASSERT (_headjointstore != NULL);
}
if (_arminitialized)
{
_armjointstore = new double[arm.nj()];
ACE_ASSERT (_armjointstore != NULL);
}
ACE_OS::printf ("Running!\n");
int cycle;
for (cycle = 0; ; cycle++)
{
if (_headinitialized)
{
head.getPositions (_headjointstore);
ACE_OS::printf ("head: ");
for (int i = 0; i < MAX_HEAD_JNTS; i++)
{
ACE_OS::printf ("%.2f ", _headjointstore[i]);
}
ACE_OS::printf ("\n");
}
if (_arminitialized)
{
arm.getPositions (_armjointstore);
ACE_OS::printf ("arm: ");
for (int i = 0; i < MAX_ARM_JNTS; i++)
{
ACE_OS::printf ("%.2f ", _armjointstore[i]);
}
ACE_OS::printf ("\n");
}
Sleep (150);
FLUSHLOG();
}
/// close devices.
///
if (_headinitialized)
{
head.idleMode ();
_headrunning = false;
head.uninitialize ();
_headinitialized = false;
if (_headjointstore != NULL) delete[] _headjointstore;
}
if (_arminitialized)
{
arm.idleMode ();
_armrunning = false;
arm.uninitialize ();
_arminitialized = false;
if (_armjointstore != NULL) delete[] _armjointstore;
}
CLOSELOG();
return 0;
}
/*----------------------------------------------------------------------------*/
NDIS_STATUS
windowsRegisterIsrt (
IN P_GLUE_INFO_T prGlueInfo
)
{
NDIS_STATUS rStatus;
P_GL_HIF_INFO_T prHifInfo;
DEBUGFUNC("windowsRegisterIsrt");
ASSERT(prGlueInfo);
prHifInfo = &prGlueInfo->rHifInfo;
/* Next we'll register our interrupt with the NDIS wrapper. */
/* Hook our interrupt vector. We used level-triggered, shared
interrupts with our PCI adapters. */
INITLOG(("Register IRQ: handle=0x%x, irq=0x%x, level-triggered\n",
prGlueInfo->rMiniportAdapterHandle, prHifInfo->u4InterruptLevel));
rStatus = NdisMRegisterInterrupt(&prHifInfo->rInterrupt,
prGlueInfo->rMiniportAdapterHandle,
(UINT) prHifInfo->u4InterruptVector,
(UINT) prHifInfo->u4InterruptLevel,
TRUE, /* RequestIsr */
FALSE, /* SharedInterrupt */
NIC_INTERRUPT_MODE);
if (rStatus != NDIS_STATUS_SUCCESS) {
ERRORLOG(("Interrupt conflict: status=0x%08x, IRQ=%d, level-sensitive\n",
rStatus, prHifInfo->u4InterruptLevel));
NdisWriteErrorLogEntry(prGlueInfo->rMiniportAdapterHandle,
NDIS_ERROR_CODE_INTERRUPT_CONNECT, 1,
(UINT_32) prHifInfo->u4InterruptLevel);
}
GLUE_SET_FLAG(prGlueInfo, GLUE_FLAG_INTERRUPT_IN_USE);
INITLOG(("Register interrupt -- OK\n"));
#if SC32442_SPI
{
UINT_32 g_SysIntr;
prHifInfo->gWaitEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
if (!KernelIoControl(IOCTL_HAL_REQUEST_SYSINTR,
&prHifInfo->u4InterruptLevel,
sizeof(UINT32), &g_SysIntr,
sizeof(UINT32), NULL)) {
INITLOG(("ERROR:Failed to request sysintr value for Timer1 inturrupt!/n"));
}
else {
INITLOG(("Request sysintr value %d!/r/n", g_SysIntr));
prHifInfo->u4sysIntr = g_SysIntr;
}
if (!(InterruptInitialize(g_SysIntr, prHifInfo->gWaitEvent, 0, 0)))
{
INITLOG(("ERROR: Interrupt initialize failed.\n"));
}
}
#endif
return rStatus;
} /* windowsRegisterIsrt */