Player

• Frontpage
• Contents

User

• Installation
• Quick start
• Supported devices
• Tutorials
• Utilities
• Client libraries
• FAQ
• Help

Developer

• Architecture
• libplayercore
  • interfaces
• libplayerdrivers
  • drivers
• libplayererror
• libplayerutils
• libplayersd
• libplayertcp
• libplayerxdr
• TODO

Online

Homepage
Download
Project
Bugs
Help

sip.cc

// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:1; -*-

#include <stdio.h>
#include <limits.h>
#include <math.h>  /* rint(3) */
#include <sys/types.h>
#include <netinet/in.h>
#include "sip.h"
#include <stdlib.h> /* for abs() */
#include <unistd.h>

int erSIP::PositionChange( unsigned short from, unsigned short to ) 
{
  int diff1, diff2;

  /* find difference in two directions and pick shortest */
  if ( to > from ) {
    diff1 = to - from;
    diff2 = - ( from + 4096 - to );
  }
  else {
    diff1 = to - from;
    diff2 = 4096 - from + to;
  }

  if ( abs(diff1) < abs(diff2) ) 
    return(diff1);
  else
    return(diff2);

}

void erSIP::Print() 
{
  printf("lwstall:%d rwstall:%d\n", lwstall, rwstall);

  printf("status: 0x%x", status);
  printf("battery: %d\n", battery);
  printf("xpos: %d ypos:%d ptu:%hu timer:%hu\n", xpos, ypos, ptu, timer);
  printf("angle: %d lvel: %d rvel: %d control: %d\n", angle, lvel, rvel, control);
}

// Parses and absorbs a standard packet from the robot
bool erSIP::Parse( unsigned char *buffer, int length ) 
{
  int cnt = 0, change;
  unsigned short newxpos, newypos;

        // Check type and length
        if (length < 20) return false;

  status = buffer[cnt];
  cnt += sizeof(unsigned char);

  // This driver does its own integration, and only cares for the lower bits of the odometry updates
        // Integers from robot are little-endian
  newxpos = buffer[cnt] + 0x100*(buffer[cnt+1]&0x0F);

  if (xpos!=INT_MAX) {
    change = (int) rint(PositionChange( rawxpos, newxpos ) * RobotParams[param_idx]->DistConvFactor);
    //printf("xchange: %i ", change);
    if (abs(change)>100)
      ;//PLAYER_WARN1("invalid odometry change [%d]; odometry values are tainted", change);
    else
      xpos += change;
  }
  else {
    //printf("xpos was INT_MAX\n");
                xpos = 0;
        }
  rawxpos = newxpos;
  cnt += 3;

  
  newypos = buffer[cnt] + 0x100*(buffer[cnt+1]&0x0F);

  if (ypos!=INT_MAX) {
    change = (int) rint(PositionChange( rawypos, newypos ) * RobotParams[param_idx]->DistConvFactor);
    if (abs(change)>100)
      ;//PLAYER_WARN1("invalid odometry change [%d]; odometry values are tainted", change);
    else
      ypos += change;
  }
  else
    ypos = 0;
  rawypos = newypos;
  cnt += 3;

        //if (debug_mode)
        //      printf("Just parsed, new xpos: %i ypos: %i\n", xpos, ypos);

  angle = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
         M_PI/2048.0 * 180.0/M_PI);
  cnt += sizeof(short);

  lvel = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
         RobotParams[param_idx]->VelConvFactor);
  cnt += sizeof(short);

  rvel = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
         RobotParams[param_idx]->VelConvFactor);
  cnt += sizeof(short);

  battery = buffer[cnt];
  cnt += sizeof(unsigned char);
  
  lwstall = buffer[cnt] & 0x01;
  cnt += sizeof(unsigned char);
  
  rwstall = buffer[cnt] & 0x01;
  cnt += sizeof(unsigned char);

  control = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
         RobotParams[param_idx]->AngleConvFactor);
  cnt += sizeof(short);

        return true;
}

// Spits out information that was previously parsed
void erSIP::Fill(player_erratic_data_t* data)
{
        // Odometry data
        {
                // initialize position to current offset
                data->position.pos.px = this->x_offset / 1e3;
                data->position.pos.py = this->y_offset / 1e3;
                // now transform current position by rotation if there is one
                // and add to offset
                if(this->angle_offset != 0) 
                {
                        double rot = DTOR(this->angle_offset);    // convert rotation to radians
                        data->position.pos.px +=  ((this->xpos/1e3) * cos(rot) - 
                                (this->ypos/1e3) * sin(rot));
                        data->position.pos.py +=  ((this->xpos/1e3) * sin(rot) + 
                                (this->ypos/1e3) * cos(rot));
                        data->position.pos.pa = DTOR(this->angle_offset + angle);
                }
                else 
                {
                        data->position.pos.px += this->xpos / 1e3;
                        data->position.pos.py += this->ypos / 1e3;
                        data->position.pos.pa = DTOR(this->angle);
                }
                data->position.vel.px = (((this->lvel) + (this->rvel) ) / 2) / 1e3;
                data->position.vel.py = 0.0;
                data->position.vel.pa = (0.596*(double)(this->rvel - this->lvel) /
                        (2.0/RobotParams[param_idx]->DiffConvFactor));
                data->position.stall = (unsigned char)(this->lwstall || this->rwstall);
        }

  // Battery data
        {
          data->power.valid = PLAYER_POWER_MASK_VOLTS | PLAYER_POWER_MASK_PERCENT;
        data->power.volts = this->battery / 1e1;
        data->power.percent = 1e2 * (data->power.volts / VIDERE_NOMINAL_VOLTAGE);
        }
}

---

Last updated 12 September 2005 21:38:45