void ProcessWriteRegCommand( int i2cDev, const char *regStr, const char *valStr )
{
RegMap_t regMap;
I2C_IO_Info_t info;
if ( !ParseReg( i2cDev, regStr, ®Map ))
{
return;
}
// Make sure that we're at version 2 on the robostix
if ( !I2C_IO_GetInfo( i2cDev, &info ))
{
LogError( "Unable to retrieve information from i2c address 0x%02x\n", gI2cAddr );
exit( 1 );
}
if ( info.version < 2 )
{
LogError( "i2c-io.hex on the robostix is too old and needs to be updated\n" );
return;
}
if ( regMap.is16Bit )
{
uint16_t regVal16;
if ( !ParseUInt16( valStr, ®Val16 ))
{
return;
}
if ( !I2C_IO_WriteReg16( i2cDev, regMap.reg, regVal16 ))
{
LogError( "Error writing to %s\n", regMap.regStr );
return;
}
}
else
{
uint8_t regVal8;
if ( !ParseByte( valStr, ®Val8 ))
{
return;
}
if ( !I2C_IO_WriteReg8( i2cDev, regMap.reg, regVal8 ))
{
LogError( "Error writing to %s\n", regMap.regStr );
return;
}
}
} // ProcessWriteRegCommand
novatel_gps_msgs::TrackstatPtr
novatel_gps_driver::TrackstatParser::ParseBinary(const novatel_gps_driver::BinaryMessage& bin_msg) throw(ParseException)
{
uint32_t num_chans = ParseUInt32(&bin_msg.data_[12]);
if (bin_msg.data_.size() != (BINARY_CHANNEL_LENGTH * num_chans) +
BINARY_BODY_LENGTH)
{
std::stringstream error;
error << "Unexpected trackstat message size: " << bin_msg.data_.size();
throw ParseException(error.str());
}
uint16_t solution_status = ParseUInt16(&bin_msg.data_[0]);
if (solution_status > MAX_SOLUTION_STATUS)
{
std::stringstream error;
error << "Unknown solution status: " << solution_status;
throw ParseException(error.str());
}
novatel_gps_msgs::TrackstatPtr ros_msg = boost::make_shared<novatel_gps_msgs::Trackstat>();
ros_msg->solution_status = SOLUTION_STATUSES[solution_status];
uint16_t pos_type = ParseUInt16(&bin_msg.data_[4]);
if (pos_type > MAX_DATUM)
{
std::stringstream error;
error << "Unknown position type: " << pos_type;
throw ParseException(error.str());
}
ros_msg->position_type = POSITION_TYPES[pos_type];
ros_msg->cutoff = ParseFloat(&bin_msg.data_[8]);
for (int i = 0; i < num_chans; i++)
{
size_t chan_offset = BINARY_BODY_LENGTH +
i * BINARY_CHANNEL_LENGTH;
novatel_gps_msgs::TrackstatChannel chan;
chan.prn = ParseInt16(&bin_msg.data_[chan_offset]);
chan.glofreq = ParseInt16(&bin_msg.data_[chan_offset+2]);
chan.ch_tr_status = ParseUInt32(&bin_msg.data_[chan_offset+4]);
chan.psr = ParseDouble(&bin_msg.data_[chan_offset+8]);
chan.doppler = ParseFloat(&bin_msg.data_[chan_offset+16]);
chan.c_no = ParseFloat(&bin_msg.data_[chan_offset+20]);
chan.locktime = ParseFloat(&bin_msg.data_[chan_offset+24]);
chan.psr_res = ParseFloat(&bin_msg.data_[chan_offset+28]);
uint32_t reject = ParseUInt32(&bin_msg.data_[chan_offset+32]);
switch (reject)
{
case 0:
chan.reject = "GOOD";
break;
case 1:
chan.reject = "BADHEALTH";
break;
case 2:
chan.reject = "OLDEPHEMERIS";
break;
case 6:
chan.reject = "ELEVATIONERROR";
break;
case 7:
chan.reject = "MISCLOSURE";
break;
case 8:
chan.reject = "NODIFFCORR";
break;
case 9:
chan.reject = "NOEPHEMERIS";
break;
case 10:
chan.reject = "INVALIDCODE";
break;
case 11:
chan.reject = "LOCKEDOUT";
break;
case 12:
chan.reject = "LOWPOWER";
break;
case 13:
chan.reject = "OBSL2";
break;
case 15:
chan.reject = "UNKNOWN";
break;
case 16:
chan.reject = "NOIONOCORR";
break;
case 17:
chan.reject = "NOTUSED";
break;
case 18:
chan.reject = "OBSL1";
break;
case 19:
chan.reject = "OBSE1";
break;
case 20:
chan.reject = "OBSL5";
break;
case 21:
chan.reject = "OBSE5";
break;
case 22:
chan.reject = "OBSB2";
break;
case 23:
chan.reject = "OBSB1";
break;
case 24:
chan.reject = "OBSB3";
break;
case 25:
chan.reject = "NOSIGNALMATCH";
break;
case 26:
chan.reject = "SUPPLEMENTARY";
break;
case 99:
chan.reject = "NA";
break;
case 100:
chan.reject = "BAD_INTEGRITY";
break;
case 101:
chan.reject = "LOSSOFLOCK";
break;
case 102:
chan.reject = "NOAMBIGUITY";
break;
default:
{
std::stringstream error;
error << "Unexpected channel status: " << reject;
throw ParseException(error.str());
}
}
chan.psr_weight = ParseFloat(&bin_msg.data_[chan_offset+36]);
ros_msg->channels.push_back(chan);
}
return ros_msg;
}
