int main(int argc, char **argv)
{
ros::init(argc, argv, "reset");
ros::NodeHandle nh("~");
// parameters
int node_id;
std::string dev_path;
nh.param("node_id", node_id, 100);
nh.param("dev_path", dev_path, std::string("/dev/ttyACM0"));
int r;
// initialize interface
r = rcmIfInit(rcmIfUsb, (char*) dev_path.c_str());
error_check(r, "Initialization Failed");
// put in idle mode during configuration
r = rcmSleepModeSet(RCM_SLEEP_MODE_IDLE);
error_check(r, "Time out waiting for sleep mode set");
// make sure correct internal opmode is set
r = rcmOpModeSet(RCM_OPMODE_RCM);
error_check(r, "Timeout waiting for opmode set");
// execute Built-In Test
int status;
r = rcmBit(&status);
error_check(r, "Timeout waiting for test");
error_check(status, "Built-In Test Failed");
// get status information
rcmMsg_GetStatusInfoConfirm statusInfo;
r = rcmStatusInfoGet(&statusInfo);
error_check(r, "Timeout waiting for status information");
print_status(&statusInfo);
// get configuration from RCM
rcmConfiguration config;
r = rcmConfigGet(&config);
error_check(r, "Timeout waiting for configuration");
print_configuration(&config);
// default configuration
config.nodeId = node_id;
config.integrationIndex = 7;
config.codeChannel = 0;
config.electricalDelayPsA = 0;
config.electricalDelayPsB = 0;
config.flags = 0;
config.txGain = RCM_TXGAIN_MAX;
config.persistFlag = RCM_PERSIST_ALL;
r = rcmConfigSet(&config);
error_check(r, "Time out waiting for rcmConfig confirm");
print_configuration(&config);
// cleanup
rcmIfFlush();
rcmIfClose();
}
void initial()
{
int status;
if (rcmIfInit(rcmIf, dev) != OK)
{
printf("Initialization failed.\n");
//exit(0);
}
// Make sure RCM is awake
if (rcmSleepModeSet(RCM_SLEEP_MODE_ACTIVE) != 0)
{
printf("Time out waiting for sleep mode set.\n");
//exit(0);
}
// Make sure opmode is RCM
if (rcmOpModeSet(RCM_OPMODE_RCM) != 0)
{
printf("Time out waiting for opmode set.\n");
//exit(0);
}
// execute Built-In Test - verify that radio is healthy
if (rcmBit(&status) != 0)
{
printf("Time out waiting for BIT.\n");
//exit(0);
}
if (status != OK)
{
printf("Built-in test failed - status %d.\n", status);
//exit(0);
}
else
{
printf("Radio passes built-in test.\n\n");
}
// retrieve config from RCM
if (rcmConfigGet(&config) != 0)
{
printf("Time out waiting for config confirm.\n");
//exit(0);
}
// print out configuration
printf("Configuration:\n");
printf("\tnodeId: %d\n", config.nodeId);
printf("\tintegrationIndex: %d\n", config.integrationIndex);
printf("\tantennaMode: %d\n", config.antennaMode);
printf("\tcodeChannel: %d\n", config.codeChannel);
printf("\telectricalDelayPsA: %d\n", config.electricalDelayPsA);
printf("\telectricalDelayPsB: %d\n", config.electricalDelayPsB);
printf("\tflags: 0x%X\n", config.flags);
printf("\ttxGain: %d\n", config.txGain);
// retrieve status/info from RCM
if (rcmStatusInfoGet(&statusInfo) != 0)
{
printf("Time out waiting for status info confirm.\n");
//exit(0);
}
// print out status/info
printf("\nStatus/Info:\n");
printf("\tRCM version: %d.%d build %d\n", statusInfo.appVersionMajor,
statusInfo.appVersionMinor, statusInfo.appVersionBuild);
printf("\tUWB Kernel version: %d.%d build %d\n", statusInfo.uwbKernelVersionMajor,
statusInfo.uwbKernelVersionMinor, statusInfo.uwbKernelVersionBuild);
printf("\tFirmware version: %x/%x/%x ver %X\n", statusInfo.firmwareMonth,
statusInfo.firmwareDay, statusInfo.firmwareYear,
statusInfo.firmwareVersion);
printf("\tSerial number: %08X\n", statusInfo.serialNum);
printf("\tBoard revision: %c\n", statusInfo.boardRev);
printf("\tTemperature: %.2f degC\n\n", statusInfo.temperature/4.0);
}