Beispiel #1
0
void ProcessWriteRegCommand( int i2cDev, const char *regStr, const char *valStr )
{
    RegMap_t        regMap;
    I2C_IO_Info_t   info;

    if ( !ParseReg( i2cDev, regStr, &regMap ))
    {
        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, &regVal16 ))
        {
            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, &regVal8 ))
        {
            return;
        }
        if ( !I2C_IO_WriteReg8( i2cDev, regMap.reg, regVal8 ))
        {
            LogError( "Error writing to %s\n", regMap.regStr );
            return;
        }
    }

} // ProcessWriteRegCommand
Beispiel #2
0
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;
}