BaudRate BaudUtils::getBaudRate(const std::string & rate) const {
std::map<std::string, BaudRate>::const_iterator iter = baud_rates_.find(rate);
if (iter != baud_rates_.end()) {
return iter->second;
}
return BaudRate(0);
}
PortW32::PortAccessStatus PortW32::POpen_Second()
{
if (IsPortParallel()) {
if (m_ProtectedOS)
m_lastError = this->OpenPortTalk();
else
m_PortTalk_Handle = (HANDLE)0; // dummy for raw access init done
if (m_lastError!=IPAC_Success)
m_portAddress = 0;
}
else if (IsPortSerial()) {
m_lastError = this->OpenSerialPort();
if (m_lastError==IPAC_Success) {
PSetBaudrate(BaudRate());
PSetHandshake(Handshake());
}
}
else if (IsPortGPUSB()) {
// GPUSB Port - start access is OK but not more
CloseGPUSB();
m_pGpUsb = GPUsb::Create();
bool retVal = m_pGpUsb->Open();
m_lastError = (retVal) ? IPAC_Success : IPAC_NoInterface;
if (m_lastError==IPAC_Success) {
m_pGpUsb->SetValue(0);// all lines high i.e. there is no guiding then
}
else {
CloseGPUSB(); // just delete this port
}
}
else if (IsPortLXUSB()) {
// LXUSB Port - start access is OK but not more
CloseLXUSB();
m_pLxUsb = LXUsb::Create();
bool retVal = m_pLxUsb->Open();
m_lastError = (retVal) ? IPAC_Success : IPAC_NoInterface;
if (m_lastError==IPAC_Success) {
m_pLxUsb->SetValue(0); // all lines high i.e. cam is in regular mode then
}
else {
CloseLXUSB(); // just delete this port
}
}
else if (IsPortLXLED()) {
// TucLED Port - start access is OK but not more
// note this port does not exist in Windows
}
else if (IsPortDummy()) {
// Dummy Port - start access is OK but not more
}
else {
// other Port ?? - start access is OK but not more
}
return m_lastError;
}
void RbFirmataController::Load(StdUtils::CStdXml &oXml)
{
RobotIOControl::Load(oXml);
oXml.IntoElem();
ComPort(oXml.GetChildString("ComPort", m_strComPort));
BaudRate(oXml.GetChildInt("BaudRate", m_iBaudRate));
oXml.OutOfElem();
}
bool RbFirmataController::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
{
std::string strType = Std_CheckString(strDataType);
if(RobotIOControl::SetData(strDataType, strValue, false))
return true;
if(strType == "COMPORT")
{
ComPort(strValue);
return true;
}
else if(strType == "BAUDRATE")
{
BaudRate((int) atoi(strValue.c_str()));
return true;
}
//If it was not one of those above then we have a problem.
if(bThrowError)
THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);
return false;
}
int
G2CmosDecodeDM24 (G2CmosRaw * in, G2CmosDM24 * out)
{
int i;
memset(out,0,sizeof(G2CmosDM24));
switch(in->len) {
case 0x100:
case 0xcc:
break;
default:
return -1;
}
DecodeDM24Time (&in->cmos[0], &out->rtc);
out->sysid = G2uint32 (&in->cmos[16]);
out->serial = G2uint32 (&in->cmos[20]);
out->pwm2 = G2uint32 (&in->cmos[24]);
for (i=0;i<8;++i) {
out->channels[i]=in->cmos[28+i];
}
for (i = 0; i < 8; ++i)
{
out->tx_bauds[i] = BaudRate (in->cmos[36 + i] & 0xf, G2uint16(&in->cmos[(i & 3)+124]));
out->rx_bauds[i] = BaudRate (in->cmos[36 + i] >> 4, G2uint16(&in->cmos[(i & 3)+124]));
}
out->aux_channels = G2uint16 (&in->cmos[44]);
for (i = 0; i < 4; ++i)
{
out->masses[i] = in->cmos[48 + i];
}
out->orientation = G2uint32 (&in->cmos[52]);
out->reboots = G2uint16 (&in->cmos[56]);
DecodeDM24Time (&in->cmos[58], &out->lastboot);
for (i = 0; i < 4; ++i)
{
out->decimations[i] = decimation (in->cmos[68 + i]);
out->samplerates[i] = in->cmos[72 + i];
}
/* BUG! FIXME: - DM24MKIII doesn't set decimations */
{
int bad=0;
for (i = 1; i < 4; ++i)
{
if (out->decimations[i] != (out->samplerates[i-1] / out->samplerates[i])) bad++;
}
if (bad) {
int sr=2000;
for (i=0;i<4;++i) {
out->decimations[i]=sr/out->samplerates[i];
sr=out->samplerates[i];
}
}
}
out->instrument_type=G2uint16(&in->cmos[76]);
for (i = 0; i < 3; ++i)
{
out->stc[i] = in->cmos[78 + (i*2)];
out->ltc[i] = in->cmos[84 + (i*2)];
out->ratio[i] = in->cmos[90 + (i*2)];
out->stc[i+3] = in->cmos[78 + 1+(i*2)];
out->ltc[i+3] = in->cmos[84 + 1+(i*2)];
out->ratio[i+3] = in->cmos[90 + 1+(i*2)];
}
/*7+8 hide here...
out->stc[3 + i] = in->cmos[146 + i];
out->ltc[3 + i] = in->cmos[148 + i];
out->ratio[3 + i] = in->cmos[150 + i];
*/
out->trigger_filter = in->cmos[96];
out->filter_tap = in->cmos[98];
for (i = 0; i < 4; ++i)
{
out->triggered_channels[i] = in->cmos[100 + i];
out->triggers[i] = in->cmos[104 + i];
}
out->pretrig = in->cmos[108];
out->posttrig = in->cmos[110];
out->flash_file_size = in->cmos[114];
out->flash_mode = in->cmos[116];
out->sync_src = G2uint16 (&in->cmos[118]);
out->heartbeat = G2uint16 (&in->cmos[120]);
out->acknak_wait = in->cmos[122];
out->stopbits = in->cmos[123];
out->once = in->cmos[162];
out->split = in->cmos[164];
out->gps_duty = G2uint16 (&in->cmos[198]);
if (in->len<248) {
out->auto_center = 255;
} else {
i = in->cmos[248];
if (i == (0xff & (~in->cmos[249])))
{
out->auto_center = i;
}
else
{
out->auto_center = 255;
}
}
return 0;
}
PortUX::PortAccessStatus PortUX::POpen_Second()
{
if (IsPortParallel()) {
m_ParallelPort_Handle = ::open(PortPath().fn_str(), O_RDWR);
if (m_ParallelPort_Handle < 0) {
switch (errno) {
case EACCES: m_lastError=IPAC_AccessDenied; break;
case ENOENT: m_lastError=IPAC_NoInterface; break;
case EROFS: m_lastError=IPAC_AccessDenied; break;
case EWOULDBLOCK: m_lastError=IPAC_AccessDenied; break;
default: m_lastError=IPAC_OtherError;
}//switch
m_ParallelPort_Handle = -1;
}
else {
int rlck = ::flock(m_ParallelPort_Handle, LOCK_EX | LOCK_NB);
if (rlck) {
// cannot lock - drop handle
m_lastError = IPCA_PortInUse;
::close(m_ParallelPort_Handle);
m_ParallelPort_Handle = -1;
}
// PPDEV procedure
if ( xioctl(m_ParallelPort_Handle, PPCLAIM, NULL) <0 ) {
// cannot claim - drop handle
m_lastError = IPCA_PortInUse;
::flock(m_ParallelPort_Handle, LOCK_UN | LOCK_NB);
::close(m_ParallelPort_Handle);
m_ParallelPort_Handle = -1;
}
int mode = IEEE1284_MODE_COMPAT;
if ( xioctl (m_ParallelPort_Handle, PPSETMODE, &mode) <0 ){
// cannot set proper mode - drop handle
m_lastError = IPAC_NotSupported;
xioctl(m_ParallelPort_Handle, PPRELEASE, NULL);
::flock(m_ParallelPort_Handle, LOCK_UN | LOCK_NB);
::close(m_ParallelPort_Handle);
m_ParallelPort_Handle = -1;
}
}
}
else if (IsPortSerial()) {
m_SerialPort_Handle = ::open(PortPath().fn_str(), O_RDWR | O_NOCTTY );
if(m_SerialPort_Handle<0) {
// result conversion
switch (errno) {
case EACCES: m_lastError=IPAC_AccessDenied; break;
case ENOENT: m_lastError=IPAC_NoInterface; break;
case EROFS: m_lastError=IPAC_AccessDenied; break;
case EWOULDBLOCK: m_lastError=IPAC_AccessDenied; break;
default: m_lastError=IPAC_OtherError;
}//switch
m_SerialPort_Handle = -1;
}
else {
ConfigPortNow();
PSetBaudrate(BaudRate());
PSetHandshake(Handshake());
}
}
else if (IsPortGPUSB()) {
// GPUSB Port - start access is OK but not more
CloseGPUSB();
m_pGpUsb = GPUsb::Create();
bool retVal = m_pGpUsb->Open();
m_lastError = (retVal) ? IPAC_Success : IPAC_NoInterface;
if (m_lastError==IPAC_Success) {
m_pGpUsb->SetValue(0);// all lines high i.e. there is no guiding then
}
else {
CloseGPUSB(); // just delete this port
}
}
else if (IsPortLXUSB()) {
// LXUSB Port - start access is OK but not more
CloseLXUSB();
m_pLxUsb = LXUsb::Create();
bool retVal = m_pLxUsb->Open();
m_lastError = (retVal) ? IPAC_Success : IPAC_NoInterface;
if (m_lastError==IPAC_Success) {
m_pLxUsb->SetValue(0); // all lines high i.e. cam is in regular mode then
}
else {
CloseLXUSB(); // just delete this port
}
}
else if (IsPortLXLED()) {
// TucLED Port - start access is OK but not more
// note this port does not exist in Windows
}
else if (IsPortDummy()) {
// Dummy Port - start access is OK but not more
}
else {
// other Port ?? - start access is OK but not more
}
return m_lastError;
}
void BaudUtils::initBaudRates(void) {
baud_rates_.insert(
std::make_pair("50", BaudRate(50)));
baud_rates_.insert(
std::make_pair("75", BaudRate(75)));
baud_rates_.insert(
std::make_pair("110", BaudRate(110)));
baud_rates_.insert(
std::make_pair("134", BaudRate(134)));
baud_rates_.insert(
std::make_pair("150", BaudRate(150)));
baud_rates_.insert(
std::make_pair("200", BaudRate(200)));
baud_rates_.insert(
std::make_pair("300", BaudRate(300)));
baud_rates_.insert(
std::make_pair("600", BaudRate(600)));
baud_rates_.insert(
std::make_pair("1200", BaudRate(1200)));
baud_rates_.insert(
std::make_pair("1800", BaudRate(1800)));
baud_rates_.insert(
std::make_pair("2400", BaudRate(2400)));
baud_rates_.insert(
std::make_pair("4800", BaudRate(4800)));
baud_rates_.insert(
std::make_pair("9600", BaudRate(9600)));
baud_rates_.insert(
std::make_pair("19200", BaudRate(19200)));
baud_rates_.insert(
std::make_pair("38400", BaudRate(38400)));
baud_rates_.insert(
std::make_pair("57600", BaudRate(57600)));
baud_rates_.insert(
std::make_pair("115200", BaudRate(115200)));
std::map<std::string, BaudRate, BaudComparator>::const_iterator iter = baud_rates_.begin();
while (iter != baud_rates_.end()) {
baud_str_.insert(std::make_pair(iter->second, iter->first));
++iter;
}
}