marvelmind.c File Reference

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <pthread.h>
#include <sys/poll.h>
#include "marvelmind.h"

Include dependency graph for marvelmind.c:

Go to the source code of this file.

Macros
#define	SERIAL_PORT_HANDLE   int
#define	PORT_NOT_OPENED   -1

Enumerations
enum	{ RECV_HDR, RECV_DGRAM }

Functions
uint16_t	CalcCrcModbus_ (uint8_t *buf, int len)
uint32_t	_GetBaudCode (uint32_t baudrate, bool verbose)
int	OpenSerialPort_ (const char *portFileName, uint32_t baudrate, bool verbose)
static int16_t	get_int16 (uint8_t *buffer)
static uint32_t	get_uint32 (uint8_t *buffer)
static int32_t	get_int32 (uint8_t *buffer)
static uint8_t	markPositionReady (struct MarvelmindHedge *hedge)
static struct PositionValue	process_position_datagram (struct MarvelmindHedge *hedge, uint8_t *buffer)
static struct PositionValue	process_position_highres_datagram (struct MarvelmindHedge *hedge, uint8_t *buffer)
static struct StationaryBeaconPosition *	getOrAllocBeacon (struct MarvelmindHedge *hedge, uint8_t address)
static void	process_beacons_positions_datagram (struct MarvelmindHedge *hedge, uint8_t *buffer)
static void	process_beacons_positions_highres_datagram (struct MarvelmindHedge *hedge, uint8_t *buffer)
static void	process_imu_raw_datagram (struct MarvelmindHedge *hedge, uint8_t *buffer)
static void	process_imu_fusion_datagram (struct MarvelmindHedge *hedge, uint8_t *buffer)
static void	process_raw_distances_datagram (struct MarvelmindHedge *hedge, uint8_t *buffer)
void *	Marvelmind_Thread_ (void *param)
struct MarvelmindHedge *	createMarvelmindHedge ()
void	startMarvelmindHedge (struct MarvelmindHedge *hedge)
static bool	getPositionFromMarvelmindHedgeByAddress (struct MarvelmindHedge *hedge, struct PositionValue *position, uint8_t address)
bool	getPositionFromMarvelmindHedge (struct MarvelmindHedge *hedge, struct PositionValue *position)
void	printPositionFromMarvelmindHedge (struct MarvelmindHedge *hedge, bool onlyNew)
bool	getStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge *hedge, struct StationaryBeaconsPositions *positions)
void	printStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge *hedge, bool onlyNew)
bool	getRawDistancesFromMarvelmindHedge (struct MarvelmindHedge *hedge, struct RawDistances *rawDistances)
void	printRawDistancesFromMarvelmindHedge (struct MarvelmindHedge *hedge, bool onlyNew)
bool	getRawIMUFromMarvelmindHedge (struct MarvelmindHedge *hedge, struct RawIMUValue *rawIMU)
void	printRawIMUFromMarvelmindHedge (struct MarvelmindHedge *hedge, bool onlyNew)
bool	getFusionIMUFromMarvelmindHedge (struct MarvelmindHedge *hedge, struct FusionIMUValue *fusionIMU)
void	printFusionIMUFromMarvelmindHedge (struct MarvelmindHedge *hedge, bool onlyNew)
void	stopMarvelmindHedge (struct MarvelmindHedge *hedge)
void	destroyMarvelmindHedge (struct MarvelmindHedge *hedge)

Macro Definition Documentation

PORT_NOT_OPENED

#define PORT_NOT_OPENED   -1

Definition at line 155 of file marvelmind.c.

SERIAL_PORT_HANDLE

#define SERIAL_PORT_HANDLE   int

Definition at line 154 of file marvelmind.c.

Enumeration Type Documentation

anonymous enum

anonymous enum

Enumerator
RECV_HDR
RECV_DGRAM

Definition at line 204 of file marvelmind.c.

     {
  RECV_HDR,
  RECV_DGRAM
};

Function Documentation

_GetBaudCode()

uint32_t _GetBaudCode	(	uint32_t	baudrate,
		bool	verbose
	)

Definition at line 96 of file marvelmind.c.

                                                       {
  switch(baudrate) {
    case 50:
      return B50;
    case 75:
      return B75;
    case 110:
      return B110;
    case 134:
      return B134;
    case 150:
      return B150;
    case 200:
      return B200;
    case 300:
      return B300;
    case 600:
      return B600;
    case 1200:
      return B1200;
    case 1800:
      return B1800;
    case 2400:
      return B2400;
    case 4800:
      return B4800;
    case 9600:
      return B9600;
    case 19200:
      return B19200;
    case 38400:
      return B38400;
    case 57600:
      return B57600;
    case 115200:
      return B115200;
    case 230400:
      return B230400;
    case 460800:
      return B460800;
    case 500000:
      return B500000;
    case 576000:
      return B576000;
    case 921600:
      return B921600;
    case 1000000:
      return B1000000;
    case 1152000:
      return B1152000;
    default:
      if(verbose)
        printf("Warning: unsupported baudrate %u. Using 9600.\n",
               baudrate);
      return B9600;
  }
}

Referenced by OpenSerialPort_().

Here is the caller graph for this function:

CalcCrcModbus_()

uint16_t CalcCrcModbus_	(	uint8_t *	buf,
		int	len
	)

Definition at line 22 of file marvelmind.c.

                                               {
  uint16_t crc = 0xFFFF;
  int pos;
  for(pos = 0; pos < len; pos++) {
    crc ^= (uint16_t)buf[pos]; // XOR byte into least sig. byte of crc
    int i;
    for(i = 8; i != 0; i--) // Loop over each bit
    {
      if((crc & 0x0001) != 0) // If the LSB is set
      {
        crc >>= 1; // Shift right and XOR 0xA001
        crc ^= 0xA001;
      } else // Else LSB is not set
        crc >>= 1; // Just shift right
    }
  }
  return crc;
}

Referenced by Marvelmind_Thread_().

Here is the caller graph for this function:

createMarvelmindHedge()

struct MarvelmindHedge* createMarvelmindHedge

(

)

Definition at line 688 of file marvelmind.c.

                                                {
  struct MarvelmindHedge* hedge = malloc(sizeof(struct MarvelmindHedge));
  if(hedge) {
    hedge->ttyFileName            = DEFAULT_TTY_FILENAME;
    hedge->baudRate               = 9600;
    hedge->positionBuffer         = NULL;
    hedge->verbose                = false;
    hedge->receiveDataCallback    = NULL;
    hedge->anyInputPacketCallback = NULL;
    hedge->lastValuesCount_       = 0;
    hedge->lastValues_next        = 0;
    hedge->haveNewValues_         = false;
    hedge->terminationRequired    = false;
 
    hedge->rawIMU.updated       = false;
    hedge->fusionIMU.updated    = false;
    hedge->rawDistances.updated = false;
#ifdef WIN32
    InitializeCriticalSection(&hedge->lock_);
#else
    pthread_mutex_init(&hedge->lock_, NULL);
#endif
  } else
    puts("Not enough memory");
  return hedge;
}

References MarvelmindHedge::anyInputPacketCallback, MarvelmindHedge::baudRate, DEFAULT_TTY_FILENAME, MarvelmindHedge::fusionIMU, MarvelmindHedge::haveNewValues_, MarvelmindHedge::lastValues_next, MarvelmindHedge::lastValuesCount_, MarvelmindHedge::lock_, MarvelmindHedge::positionBuffer, MarvelmindHedge::rawDistances, MarvelmindHedge::rawIMU, MarvelmindHedge::receiveDataCallback, MarvelmindHedge::terminationRequired, MarvelmindHedge::ttyFileName, RawIMUValue::updated, FusionIMUValue::updated, RawDistances::updated, and MarvelmindHedge::verbose.

Referenced by MMNode::MMNode().

Here is the caller graph for this function:

destroyMarvelmindHedge()

void destroyMarvelmindHedge

(

struct MarvelmindHedge *

hedge

)

Definition at line 1077 of file marvelmind.c.

                                                           {
  if(hedge->positionBuffer)
    free(hedge->positionBuffer);
  free(hedge);
}

References MarvelmindHedge::positionBuffer.

get_int16()

static int16_t get_int16 ( uint8_t *  buffer )

static

Definition at line 211 of file marvelmind.c.

                                          {
  int16_t res = buffer[0] |
      (((uint16_t)buffer[1]) << 8);
 
  return res;
}

Referenced by process_imu_fusion_datagram(), and process_imu_raw_datagram().

Here is the caller graph for this function:

get_int32()

static int32_t get_int32 ( uint8_t *  buffer )

static

Definition at line 227 of file marvelmind.c.

                                          {
  int32_t res = buffer[0] |
      (((uint32_t)buffer[1]) << 8) |
      (((uint32_t)buffer[2]) << 16) |
      (((uint32_t)buffer[3]) << 24);
 
  return res;
}

Referenced by process_imu_fusion_datagram().

Here is the caller graph for this function:

get_uint32()

static uint32_t get_uint32 ( uint8_t *  buffer )

static

Definition at line 218 of file marvelmind.c.

                                            {
  uint32_t res = buffer[0] |
      (((uint32_t)buffer[1]) << 8) |
      (((uint32_t)buffer[2]) << 16) |
      (((uint32_t)buffer[3]) << 24);
 
  return res;
}

Referenced by process_imu_fusion_datagram(), process_imu_raw_datagram(), and process_raw_distances_datagram().

Here is the caller graph for this function:

getFusionIMUFromMarvelmindHedge()

bool getFusionIMUFromMarvelmindHedge	(	struct MarvelmindHedge *	hedge,
		struct FusionIMUValue *	fusionIMU
	)

Definition at line 1002 of file marvelmind.c.

                                                                       {
#ifdef WIN32
  EnterCriticalSection(&hedge->lock_);
#else
  pthread_mutex_lock(&hedge->lock_);
#endif
 
  *fusionIMU = hedge->fusionIMU;
 
#ifdef WIN32
  LeaveCriticalSection(&hedge->lock_);
#else
  pthread_mutex_unlock(&hedge->lock_);
#endif
 
  return true;
}

References MarvelmindHedge::fusionIMU, and MarvelmindHedge::lock_.

Referenced by MMNode::on_mm_callback(), and printFusionIMUFromMarvelmindHedge().

Here is the caller graph for this function:

getOrAllocBeacon()

static struct StationaryBeaconPosition* getOrAllocBeacon ( struct MarvelmindHedge *  hedge, uint8_t  address  )

static

Definition at line 334 of file marvelmind.c.

                                                                                                         {
  uint8_t i;
  uint8_t n_used = hedge->positionsBeacons.numBeacons;
 
  if(n_used != 0)
    for(i = 0; i < n_used; i++) {
      if(hedge->positionsBeacons.beacons[i].address == address) {
        return &hedge->positionsBeacons.beacons[i];
      }
    }
 
  if(n_used >= (MAX_STATIONARY_BEACONS - 1))
    return NULL;
 
  hedge->positionsBeacons.numBeacons = (n_used + 1);
  return &hedge->positionsBeacons.beacons[n_used];
}

References StationaryBeaconPosition::address, StationaryBeaconsPositions::beacons, MAX_STATIONARY_BEACONS, StationaryBeaconsPositions::numBeacons, and MarvelmindHedge::positionsBeacons.

Referenced by process_beacons_positions_datagram(), and process_beacons_positions_highres_datagram().

Here is the caller graph for this function:

getPositionFromMarvelmindHedge()

bool getPositionFromMarvelmindHedge	(	struct MarvelmindHedge *	hedge,
		struct PositionValue *	position
	)

Definition at line 816 of file marvelmind.c.

                                                                    {
  return getPositionFromMarvelmindHedgeByAddress(hedge, position, 0);
}

References getPositionFromMarvelmindHedgeByAddress().

Referenced by MMNode::on_mm_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

getPositionFromMarvelmindHedgeByAddress()

static bool getPositionFromMarvelmindHedgeByAddress ( struct MarvelmindHedge *  hedge, struct PositionValue *  position, uint8_t  address  )

static

Definition at line 749 of file marvelmind.c.

                                                                     {
  uint8_t i;
  int32_t avg_x = 0, avg_y = 0, avg_z = 0;
  double avg_ang         = 0.0;
  uint32_t max_timestamp = 0;
  bool position_valid;
  bool highRes = false;
#ifdef WIN32
  EnterCriticalSection(&hedge->lock_);
#else
  pthread_mutex_lock(&hedge->lock_);
#endif
  if(hedge->lastValuesCount_) {
    uint8_t real_values_count = MAX_BUFFERED_POSITIONS;
    uint8_t nFound            = 0;
    if(hedge->lastValuesCount_ < real_values_count)
      real_values_count = hedge->lastValuesCount_;
    for(i = 0; i < real_values_count; i++) {
      if(address != 0)
        if(hedge->positionBuffer[i].address != address)
          continue;
      if(!hedge->positionBuffer[i].ready)
        continue;
      if(hedge->positionBuffer[i].processed)
        continue;
      if(address == 0)
        address = hedge->positionBuffer[i].address;
      nFound++;
      avg_x += hedge->positionBuffer[i].x;
      avg_y += hedge->positionBuffer[i].y;
      avg_z += hedge->positionBuffer[i].z;
      avg_ang += hedge->positionBuffer[i].angle;
      if(hedge->positionBuffer[i].highResolution)
        highRes = true;
      hedge->positionBuffer[i].processed = true;
      if(hedge->positionBuffer[i].timestamp > max_timestamp)
        max_timestamp = hedge->positionBuffer[i].timestamp;
    }
    if(nFound != 0) {
      avg_x /= nFound;
      avg_y /= nFound;
      avg_z /= nFound;
      avg_ang /= nFound;
      position_valid = true;
    } else {
      position_valid = false;
    }
  } else
    position_valid = false;
#ifdef WIN32
  LeaveCriticalSection(&hedge->lock_);
#else
  pthread_mutex_unlock(&hedge->lock_);
#endif
  position->address        = address;
  position->x              = avg_x;
  position->y              = avg_y;
  position->z              = avg_z;
  position->angle          = avg_ang;
  position->timestamp      = max_timestamp;
  position->ready          = position_valid;
  position->highResolution = highRes;
  return position_valid;
}

References PositionValue::address, PositionValue::angle, PositionValue::highResolution, MarvelmindHedge::lastValuesCount_, MarvelmindHedge::lock_, MAX_BUFFERED_POSITIONS, MarvelmindHedge::positionBuffer, PositionValue::processed, PositionValue::ready, PositionValue::timestamp, PositionValue::x, PositionValue::y, and PositionValue::z.

Referenced by getPositionFromMarvelmindHedge(), and printPositionFromMarvelmindHedge().

Here is the caller graph for this function:

getRawDistancesFromMarvelmindHedge()

bool getRawDistancesFromMarvelmindHedge	(	struct MarvelmindHedge *	hedge,
		struct RawDistances *	rawDistances
	)

Definition at line 920 of file marvelmind.c.

                                                                           {
#ifdef WIN32
  EnterCriticalSection(&hedge->lock_);
#else
  pthread_mutex_lock(&hedge->lock_);
#endif
 
  *rawDistances = hedge->rawDistances;
 
#ifdef WIN32
  LeaveCriticalSection(&hedge->lock_);
#else
  pthread_mutex_unlock(&hedge->lock_);
#endif
 
  return true;
}

References MarvelmindHedge::lock_, and MarvelmindHedge::rawDistances.

Referenced by printRawDistancesFromMarvelmindHedge().

Here is the caller graph for this function:

getRawIMUFromMarvelmindHedge()

bool getRawIMUFromMarvelmindHedge	(	struct MarvelmindHedge *	hedge,
		struct RawIMUValue *	rawIMU
	)

Definition at line 964 of file marvelmind.c.

                                                              {
#ifdef WIN32
  EnterCriticalSection(&hedge->lock_);
#else
  pthread_mutex_lock(&hedge->lock_);
#endif
 
  *rawIMU = hedge->rawIMU;
 
#ifdef WIN32
  LeaveCriticalSection(&hedge->lock_);
#else
  pthread_mutex_unlock(&hedge->lock_);
#endif
 
  return true;
}

References MarvelmindHedge::lock_, and MarvelmindHedge::rawIMU.

Referenced by printRawIMUFromMarvelmindHedge().

Here is the caller graph for this function:

getStationaryBeaconsPositionsFromMarvelmindHedge()

bool getStationaryBeaconsPositionsFromMarvelmindHedge	(	struct MarvelmindHedge *	hedge,
		struct StationaryBeaconsPositions *	positions
	)

Definition at line 869 of file marvelmind.c.

                                                                                                    {
#ifdef WIN32
  EnterCriticalSection(&hedge->lock_);
#else
  pthread_mutex_lock(&hedge->lock_);
#endif
 
  *positions = hedge->positionsBeacons;
 
#ifdef WIN32
  LeaveCriticalSection(&hedge->lock_);
#else
  pthread_mutex_unlock(&hedge->lock_);
#endif
 
  return true;
}

References MarvelmindHedge::lock_, and MarvelmindHedge::positionsBeacons.

Referenced by printStationaryBeaconsPositionsFromMarvelmindHedge().

Here is the caller graph for this function:

markPositionReady()

static uint8_t markPositionReady ( struct MarvelmindHedge *  hedge )

static

Definition at line 240 of file marvelmind.c.

                                                                {
  uint8_t ind    = hedge->lastValues_next;
  uint8_t indCur = ind;
 
  hedge->positionBuffer[ind].ready =
      true;
  hedge->positionBuffer[ind].processed =
      false;
  ind++;
  if(ind >= MAX_BUFFERED_POSITIONS)
    ind = 0;
  if(hedge->lastValuesCount_ < MAX_BUFFERED_POSITIONS)
    hedge->lastValuesCount_++;
  hedge->haveNewValues_ = true;
 
  hedge->lastValues_next = ind;
 
  return indCur;
}

References MarvelmindHedge::haveNewValues_, MarvelmindHedge::lastValues_next, MarvelmindHedge::lastValuesCount_, MAX_BUFFERED_POSITIONS, MarvelmindHedge::positionBuffer, PositionValue::processed, and PositionValue::ready.

Referenced by process_position_datagram(), and process_position_highres_datagram().

Here is the caller graph for this function:

Marvelmind_Thread_()

void* Marvelmind_Thread_

(

void *

param

)

Definition at line 497 of file marvelmind.c.

                                    {
  struct MarvelmindHedge* hedge = (struct MarvelmindHedge*)param;
  struct PositionValue curPosition;
  uint8_t input_buffer[256];
  uint8_t recvState             = RECV_HDR; // current state of receive data
  uint8_t nBytesInBlockReceived = 0; // bytes received
  uint16_t dataId;
#ifndef WIN32
  struct pollfd fds[1];
  int pollrc;
#endif
 
  SERIAL_PORT_HANDLE ttyHandle = OpenSerialPort_(hedge->ttyFileName,
                                                 hedge->baudRate, hedge->verbose);
  if(ttyHandle == PORT_NOT_OPENED)
    hedge->terminationRequired = true;
  else if(hedge->verbose)
    printf("Opened serial port %s with baudrate %u\n",
           hedge->ttyFileName, hedge->baudRate);
 
  while(hedge->terminationRequired == false) {
    uint8_t receivedChar;
    bool readSuccessed = true;
#ifdef WIN32
    DWORD nBytesRead;
    OVERLAPPED osReader = { 0 };
 
    osReader.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
    if(osReader.hEvent == NULL) {
      printf("Failed create event\n");
      hedge->terminationRequired = true;
      continue;
    }
 
    readSuccessed = ReadFile(ttyHandle, &receivedChar, 1, &nBytesRead, NULL); //&osReader);
    /*    if (!readSuccessed)
        {
            dwRes = WaitForSingleObject(osReader.hEvent, 1000);
            if (dwRes == WAIT_OBJECT_0)
            {
               if (GetOverlappedResult(ttyHandle, &osReader, &nBytesRead, FALSE))
               {
                  readSuccessed= true;
               }
            }
        }
*/
    CloseHandle(osReader.hEvent);
#else
    int32_t nBytesRead;
    fds[0].fd     = ttyHandle;
    fds[0].events = POLLIN;
    pollrc        = poll(fds, 1, 1000);
    if(pollrc <= 0)
      continue;
    if((fds[0].revents & POLLIN) == 0)
      continue;
 
    nBytesRead = read(ttyHandle, &receivedChar, 1);
    if(nBytesRead < 0)
      readSuccessed = false;
#endif
    if(nBytesRead && readSuccessed) {
      bool goodByte                       = false;
      input_buffer[nBytesInBlockReceived] = receivedChar;
      switch(recvState) {
        case RECV_HDR:
          switch(nBytesInBlockReceived) {
            case 0:
              goodByte = (receivedChar == 0xff);
              break;
            case 1:
              goodByte = (receivedChar == 0x47);
              break;
            case 2:
              goodByte = true;
              break;
            case 3:
              dataId   = (((uint16_t)receivedChar) << 8) + input_buffer[2];
              goodByte = (dataId == POSITION_DATAGRAM_ID) ||
                  (dataId == BEACONS_POSITIONS_DATAGRAM_ID) ||
                  (dataId == POSITION_DATAGRAM_HIGHRES_ID) ||
                  (dataId == BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID) ||
                  (dataId == IMU_RAW_DATAGRAM_ID) ||
                  (dataId == IMU_FUSION_DATAGRAM_ID) ||
                  (dataId == BEACON_RAW_DISTANCE_DATAGRAM_ID);
              break;
            case 4:
              switch(dataId) {
                case POSITION_DATAGRAM_ID:
                  goodByte = (receivedChar == 0x10);
                  break;
                case BEACONS_POSITIONS_DATAGRAM_ID:
                case BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID:
                  goodByte = true;
                  break;
                case POSITION_DATAGRAM_HIGHRES_ID:
                  goodByte = (receivedChar == 0x16);
                  break;
                case IMU_RAW_DATAGRAM_ID:
                  goodByte = (receivedChar == 0x20);
                  break;
                case IMU_FUSION_DATAGRAM_ID:
                  goodByte = (receivedChar == 0x2a);
                  break;
                case BEACON_RAW_DISTANCE_DATAGRAM_ID:
                  goodByte = (receivedChar == 0x20);
                  break;
              }
              if(goodByte)
                recvState = RECV_DGRAM;
              break;
          }
          if(goodByte) {
            // correct header byte
            nBytesInBlockReceived++;
          } else {
            // ...or incorrect
            recvState             = RECV_HDR;
            nBytesInBlockReceived = 0;
          }
          break;
        case RECV_DGRAM:
          nBytesInBlockReceived++;
          if(nBytesInBlockReceived >= 7 + input_buffer[4]) {
            // parse dgram
            uint16_t blockCrc =
                CalcCrcModbus_(input_buffer, nBytesInBlockReceived);
            if(blockCrc == 0) {
#ifdef WIN32
              EnterCriticalSection(&hedge->lock_);
#else
              pthread_mutex_lock(&hedge->lock_);
#endif
              switch(dataId) {
                case POSITION_DATAGRAM_ID:
                  // add to positionBuffer
                  curPosition = process_position_datagram(hedge, input_buffer);
                  break;
                case BEACONS_POSITIONS_DATAGRAM_ID:
                  process_beacons_positions_datagram(hedge, input_buffer);
                  break;
                case POSITION_DATAGRAM_HIGHRES_ID:
                  // add to positionBuffer
                  curPosition = process_position_highres_datagram(hedge, input_buffer);
                  break;
                case BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID:
                  process_beacons_positions_highres_datagram(hedge, input_buffer);
                  break;
                case IMU_RAW_DATAGRAM_ID:
                  process_imu_raw_datagram(hedge, input_buffer);
                  break;
                case IMU_FUSION_DATAGRAM_ID:
                  process_imu_fusion_datagram(hedge, input_buffer);
                  break;
                case BEACON_RAW_DISTANCE_DATAGRAM_ID:
                  process_raw_distances_datagram(hedge, input_buffer);
                  break;
              }
#ifdef WIN32
              LeaveCriticalSection(&hedge->lock_);
#else
              pthread_mutex_unlock(&hedge->lock_);
#endif
              // callback
              if(hedge->anyInputPacketCallback) {
                hedge->anyInputPacketCallback();
              }
 
              if(hedge->receiveDataCallback) {
                if(dataId == POSITION_DATAGRAM_ID) {
                  hedge->receiveDataCallback(curPosition);
                }
              }
            }
            // and repeat
            recvState             = RECV_HDR;
            nBytesInBlockReceived = 0;
          }
      }
    }
  }
#ifndef WIN32
  return NULL;
#endif
}

References MarvelmindHedge::anyInputPacketCallback, MarvelmindHedge::baudRate, BEACON_RAW_DISTANCE_DATAGRAM_ID, BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID, BEACONS_POSITIONS_DATAGRAM_ID, CalcCrcModbus_(), IMU_FUSION_DATAGRAM_ID, IMU_RAW_DATAGRAM_ID, MarvelmindHedge::lock_, OpenSerialPort_(), PORT_NOT_OPENED, POSITION_DATAGRAM_HIGHRES_ID, POSITION_DATAGRAM_ID, process_beacons_positions_datagram(), process_beacons_positions_highres_datagram(), process_imu_fusion_datagram(), process_imu_raw_datagram(), process_position_datagram(), process_position_highres_datagram(), process_raw_distances_datagram(), MarvelmindHedge::receiveDataCallback, RECV_DGRAM, RECV_HDR, SERIAL_PORT_HANDLE, MarvelmindHedge::terminationRequired, MarvelmindHedge::ttyFileName, and MarvelmindHedge::verbose.

Referenced by startMarvelmindHedge().

Here is the call graph for this function:

Here is the caller graph for this function:

OpenSerialPort_()

int OpenSerialPort_	(	const char *	portFileName,
		uint32_t	baudrate,
		bool	verbose
	)

Definition at line 163 of file marvelmind.c.

                                                                               {
  int ttyHandle = open(portFileName, O_RDWR | O_NONBLOCK | O_NDELAY);
  if(ttyHandle < 0) {
    if(verbose)
      puts("Error: unable to open serial connection "
           "(possibly serial port is not available)");
    return -1;
  }
  struct termios ttyCtrl;
  memset(&ttyCtrl, 0, sizeof ttyCtrl);
  if(tcgetattr(ttyHandle, &ttyCtrl) != 0) {
    if(verbose)
      puts("Error: unable to get serial port parameters");
    return -1;
  }
 
  uint32_t baudCode = _GetBaudCode(baudrate, verbose);
  cfsetospeed(&ttyCtrl, baudCode);
  cfsetispeed(&ttyCtrl, baudCode);
  // 8N1, no flow control
  ttyCtrl.c_cflag &= ~(PARENB | CSTOPB | CSIZE | CRTSCTS);
  ttyCtrl.c_cflag |= CS8;
  // no signaling chars, no echo, no canonical processing
  ttyCtrl.c_lflag     = 0;
  ttyCtrl.c_oflag     = 0; // no remapping, no delays
  ttyCtrl.c_cc[VMIN]  = 0; // read doesn't block
  ttyCtrl.c_cc[VTIME] = 30; // 3 seconds read timeout
  ttyCtrl.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines
  ttyCtrl.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl
  ttyCtrl.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
  ttyCtrl.c_oflag &= ~OPOST; // make raw
  tcflush(ttyHandle, TCIFLUSH); // Flush port
  if(tcsetattr(ttyHandle, TCSANOW, &ttyCtrl) != 0) {
    if(verbose)
      puts("Error: unable to set serial port parameters");
    return -1;
  }
  return ttyHandle;
}

References _GetBaudCode().

Referenced by Marvelmind_Thread_().

Here is the call graph for this function:

Here is the caller graph for this function:

printFusionIMUFromMarvelmindHedge()

void printFusionIMUFromMarvelmindHedge	(	struct MarvelmindHedge *	hedge,
		bool	onlyNew
	)

Definition at line 1021 of file marvelmind.c.

                                                     {
  struct FusionIMUValue fusionIMU;
  float x_m, y_m, z_m;
  float qw, qx, qy, qz;
  float vx, vy, vz, ax, ay, az;
 
  getFusionIMUFromMarvelmindHedge(hedge, &fusionIMU);
 
  if(fusionIMU.updated || (!onlyNew)) {
    x_m = fusionIMU.x / 1000.0;
    y_m = fusionIMU.y / 1000.0;
    z_m = fusionIMU.z / 1000.0;
 
    qw = fusionIMU.qw / 10000.0;
    qx = fusionIMU.qx / 10000.0;
    qy = fusionIMU.qy / 10000.0;
    qz = fusionIMU.qz / 10000.0;
 
    vx = fusionIMU.vx / 1000.0;
    vy = fusionIMU.vy / 1000.0;
    vz = fusionIMU.vz / 1000.0;
 
    ax = fusionIMU.ax / 1000.0;
    ay = fusionIMU.ay / 1000.0;
    az = fusionIMU.az / 1000.0;
 
    printf("IMU fusion: Timestamp: %08d, X=%.3f  Y= %.3f  Z=%.3f  q=%.3f,%.3f,%.3f,%.3f v=%.3f,%.3f,%.3f  a=%.3f,%.3f,%.3f \n",
           (int)fusionIMU.timestamp,
           (float)x_m, (float)y_m, (float)z_m,
           (float)qw, (float)qx, (float)qy, (float)qz,
           (float)vx, (float)vy, (float)vz,
           (float)ax, (float)ay, (float)az);
 
    hedge->fusionIMU.updated = false;
  }
}

References FusionIMUValue::ax, FusionIMUValue::ay, FusionIMUValue::az, MarvelmindHedge::fusionIMU, getFusionIMUFromMarvelmindHedge(), FusionIMUValue::qw, FusionIMUValue::qx, FusionIMUValue::qy, FusionIMUValue::qz, FusionIMUValue::timestamp, FusionIMUValue::updated, FusionIMUValue::vx, FusionIMUValue::vy, FusionIMUValue::vz, FusionIMUValue::x, FusionIMUValue::y, and FusionIMUValue::z.

Here is the call graph for this function:

printPositionFromMarvelmindHedge()

void printPositionFromMarvelmindHedge	(	struct MarvelmindHedge *	hedge,
		bool	onlyNew
	)

Definition at line 825 of file marvelmind.c.

                                                    {
  uint8_t i, j;
  double xm, ym, zm;
 
  if(hedge->haveNewValues_ || (!onlyNew)) {
    struct PositionValue position;
    uint8_t addresses[MAX_BUFFERED_POSITIONS];
    uint8_t addressesNum = 0;
 
    for(i = 0; i < MAX_BUFFERED_POSITIONS; i++) {
      uint8_t address       = hedge->positionBuffer[i].address;
      bool alreadyProcessed = false;
      if(addressesNum != 0)
        for(j = 0; j < addressesNum; j++) {
          if(address == addresses[j]) {
            alreadyProcessed = true;
            break;
          }
        }
      if(alreadyProcessed)
        continue;
      addresses[addressesNum++] = address;
 
      getPositionFromMarvelmindHedgeByAddress(hedge, &position, address);
      xm = ((double)position.x) / 1000.0;
      ym = ((double)position.y) / 1000.0;
      zm = ((double)position.z) / 1000.0;
      if(position.ready) {
        if(position.highResolution) {
          printf("Address: %d, X: %.3f, Y: %.3f, Z: %.3f, Angle: %.1f  at time T: %u\n",
                 position.address, xm, ym, zm, position.angle, position.timestamp);
        } else {
          printf("Address: %d, X: %.2f, Y: %.2f, Z: %.2f, Angle: %.1f at time T: %u\n",
                 position.address, xm, ym, zm, position.angle, position.timestamp);
        }
      }
      hedge->haveNewValues_ = false;
    }
  }
}

References PositionValue::address, PositionValue::angle, getPositionFromMarvelmindHedgeByAddress(), MarvelmindHedge::haveNewValues_, PositionValue::highResolution, MAX_BUFFERED_POSITIONS, MarvelmindHedge::positionBuffer, PositionValue::ready, PositionValue::timestamp, PositionValue::x, PositionValue::y, and PositionValue::z.

Here is the call graph for this function:

printRawDistancesFromMarvelmindHedge()

void printRawDistancesFromMarvelmindHedge	(	struct MarvelmindHedge *	hedge,
		bool	onlyNew
	)

Definition at line 939 of file marvelmind.c.

                                                        {
  struct RawDistances rawDistances;
  uint8_t i;
  float d_m;
 
  getRawDistancesFromMarvelmindHedge(hedge, &rawDistances);
 
  if(rawDistances.updated || (!onlyNew)) {
    for(i = 0; i < 4; i++) {
      if(rawDistances.distances[i].address_beacon != 0) {
        d_m = rawDistances.distances[i].distance / 1000.0;
        printf("Raw distance: %02d ==> %02d,  Distance= %.3f \n",
               (int)rawDistances.address_hedge,
               (int)rawDistances.distances[i].address_beacon,
               (float)d_m);
      }
    }
 
    hedge->rawDistances.updated = false;
  }
}

References RawDistanceItem::address_beacon, RawDistances::address_hedge, RawDistanceItem::distance, RawDistances::distances, getRawDistancesFromMarvelmindHedge(), MarvelmindHedge::rawDistances, and RawDistances::updated.

Here is the call graph for this function:

printRawIMUFromMarvelmindHedge()

void printRawIMUFromMarvelmindHedge	(	struct MarvelmindHedge *	hedge,
		bool	onlyNew
	)

Definition at line 983 of file marvelmind.c.

                                                  {
  struct RawIMUValue rawIMU;
 
  getRawIMUFromMarvelmindHedge(hedge, &rawIMU);
 
  if(rawIMU.updated || (!onlyNew)) {
    printf("Raw IMU: Timestamp: %08d, aX=%05d aY=%05d aZ=%05d  gX=%05d gY=%05d gZ=%05d  cX=%05d cY=%05d cZ=%05d \n",
           (int)rawIMU.timestamp,
           (int)rawIMU.acc_x, (int)rawIMU.acc_y, (int)rawIMU.acc_z,
           (int)rawIMU.gyro_x, (int)rawIMU.gyro_y, (int)rawIMU.gyro_z,
           (int)rawIMU.compass_x, (int)rawIMU.compass_y, (int)rawIMU.compass_z);
 
    hedge->rawIMU.updated = false;
  }
}

References RawIMUValue::acc_x, RawIMUValue::acc_y, RawIMUValue::acc_z, RawIMUValue::compass_x, RawIMUValue::compass_y, RawIMUValue::compass_z, getRawIMUFromMarvelmindHedge(), RawIMUValue::gyro_x, RawIMUValue::gyro_y, RawIMUValue::gyro_z, MarvelmindHedge::rawIMU, RawIMUValue::timestamp, and RawIMUValue::updated.

Here is the call graph for this function:

printStationaryBeaconsPositionsFromMarvelmindHedge()

void printStationaryBeaconsPositionsFromMarvelmindHedge	(	struct MarvelmindHedge *	hedge,
		bool	onlyNew
	)

Definition at line 888 of file marvelmind.c.

                                                                      {
  struct StationaryBeaconsPositions positions;
  double xm, ym, zm;
 
  getStationaryBeaconsPositionsFromMarvelmindHedge(hedge, &positions);
 
  if(positions.updated || (!onlyNew)) {
    uint8_t i;
    uint8_t n = hedge->positionsBeacons.numBeacons;
    struct StationaryBeaconPosition* b;
 
    for(i = 0; i < n; i++) {
      b  = &positions.beacons[i];
      xm = ((double)b->x) / 1000.0;
      ym = ((double)b->y) / 1000.0;
      zm = ((double)b->z) / 1000.0;
      if(positions.beacons[i].highResolution) {
        printf("Stationary beacon: address: %d, X: %.3f, Y: %.3f, Z: %.3f \n",
               b->address, xm, ym, zm);
      } else {
        printf("Stationary beacon: address: %d, X: %.2f, Y: %.2f, Z: %.2f \n",
               b->address, xm, ym, zm);
      }
    }
 
    hedge->positionsBeacons.updated = false;
  }
}

References StationaryBeaconPosition::address, StationaryBeaconsPositions::beacons, getStationaryBeaconsPositionsFromMarvelmindHedge(), StationaryBeaconPosition::highResolution, StationaryBeaconsPositions::numBeacons, MarvelmindHedge::positionsBeacons, StationaryBeaconsPositions::updated, StationaryBeaconPosition::x, StationaryBeaconPosition::y, and StationaryBeaconPosition::z.

Referenced by MMNode::MMNode().

Here is the call graph for this function:

Here is the caller graph for this function:

process_beacons_positions_datagram()

static void process_beacons_positions_datagram ( struct MarvelmindHedge *  hedge, uint8_t *  buffer  )

static

Definition at line 352 of file marvelmind.c.

                                                                                               {
  uint8_t n = buffer[5]; // number of beacons in packet
  uint8_t i, ofs;
  uint8_t address;
  int16_t x, y, z;
  struct StationaryBeaconPosition* b;
 
  if((1 + n * 8) != buffer[4])
    return; // incorrect size
 
  for(i = 0; i < n; i++) {
    ofs = 6 + i * 8;
 
    address = buffer[ofs + 0];
    x       = buffer[ofs + 1] |
        (((uint16_t)buffer[ofs + 2]) << 8);
    y = buffer[ofs + 3] |
        (((uint16_t)buffer[ofs + 4]) << 8);
    z = buffer[ofs + 5] |
        (((uint16_t)buffer[ofs + 6]) << 8);
 
    b = getOrAllocBeacon(hedge, address);
    if(b != NULL) {
      b->address = address;
      b->x       = x * 10; // millimeters
      b->y       = y * 10; // millimeters
      b->z       = z * 10; // millimeters
 
      b->highResolution = false;
 
      hedge->positionsBeacons.updated = true;
    }
  }
}

References StationaryBeaconPosition::address, getOrAllocBeacon(), StationaryBeaconPosition::highResolution, MarvelmindHedge::positionsBeacons, StationaryBeaconsPositions::updated, StationaryBeaconPosition::x, StationaryBeaconPosition::y, and StationaryBeaconPosition::z.

Referenced by Marvelmind_Thread_().

Here is the call graph for this function:

Here is the caller graph for this function:

process_beacons_positions_highres_datagram()

static void process_beacons_positions_highres_datagram ( struct MarvelmindHedge *  hedge, uint8_t *  buffer  )

static

Definition at line 387 of file marvelmind.c.

                                                                                                       {
  uint8_t n = buffer[5]; // number of beacons in packet
  uint8_t i, ofs;
  uint8_t address;
  int32_t x, y, z;
  struct StationaryBeaconPosition* b;
 
  if((1 + n * 14) != buffer[4])
    return; // incorrect size
 
  for(i = 0; i < n; i++) {
    ofs = 6 + i * 14;
 
    address = buffer[ofs + 0];
    x       = buffer[ofs + 1] |
        (((uint32_t)buffer[ofs + 2]) << 8) |
        (((uint32_t)buffer[ofs + 3]) << 16) |
        (((uint32_t)buffer[ofs + 4]) << 24);
    y = buffer[ofs + 5] |
        (((uint32_t)buffer[ofs + 6]) << 8) |
        (((uint32_t)buffer[ofs + 7]) << 16) |
        (((uint32_t)buffer[ofs + 8]) << 24);
    z = buffer[ofs + 9] |
        (((uint32_t)buffer[ofs + 10]) << 8) |
        (((uint32_t)buffer[ofs + 11]) << 16) |
        (((uint32_t)buffer[ofs + 12]) << 24);
 
    b = getOrAllocBeacon(hedge, address);
    if(b != NULL) {
      b->address = address;
      b->x       = x;
      b->y       = y;
      b->z       = z;
 
      b->highResolution = true;
 
      hedge->positionsBeacons.updated = true;
    }
  }
}

References StationaryBeaconPosition::address, getOrAllocBeacon(), StationaryBeaconPosition::highResolution, MarvelmindHedge::positionsBeacons, StationaryBeaconsPositions::updated, StationaryBeaconPosition::x, StationaryBeaconPosition::y, and StationaryBeaconPosition::z.

Referenced by Marvelmind_Thread_().

Here is the call graph for this function:

Here is the caller graph for this function:

process_imu_fusion_datagram()

static void process_imu_fusion_datagram ( struct MarvelmindHedge *  hedge, uint8_t *  buffer  )

static

Definition at line 450 of file marvelmind.c.

                                                                                        {
  uint8_t* dataBuf = &buffer[5];
 
  hedge->fusionIMU.x = get_int32(&dataBuf[0]);
  hedge->fusionIMU.y = get_int16(&dataBuf[4]);
  hedge->fusionIMU.z = get_int16(&dataBuf[8]);
 
  hedge->fusionIMU.qw = get_int16(&dataBuf[12]);
  hedge->fusionIMU.qx = get_int16(&dataBuf[14]);
  hedge->fusionIMU.qy = get_int16(&dataBuf[16]);
  hedge->fusionIMU.qz = get_int16(&dataBuf[18]);
 
  hedge->fusionIMU.vx = get_int16(&dataBuf[20]);
  hedge->fusionIMU.vy = get_int16(&dataBuf[22]);
  hedge->fusionIMU.vz = get_int16(&dataBuf[24]);
 
  hedge->fusionIMU.ax = get_int16(&dataBuf[26]);
  hedge->fusionIMU.ay = get_int16(&dataBuf[28]);
  hedge->fusionIMU.az = get_int16(&dataBuf[30]);
 
  hedge->fusionIMU.timestamp = get_uint32(&dataBuf[34]);
 
  hedge->fusionIMU.updated = true;
}

References FusionIMUValue::ax, FusionIMUValue::ay, FusionIMUValue::az, MarvelmindHedge::fusionIMU, get_int16(), get_int32(), get_uint32(), FusionIMUValue::qw, FusionIMUValue::qx, FusionIMUValue::qy, FusionIMUValue::qz, FusionIMUValue::timestamp, FusionIMUValue::updated, FusionIMUValue::vx, FusionIMUValue::vy, FusionIMUValue::vz, FusionIMUValue::x, FusionIMUValue::y, and FusionIMUValue::z.

Referenced by Marvelmind_Thread_().

Here is the call graph for this function:

Here is the caller graph for this function:

process_imu_raw_datagram()

static void process_imu_raw_datagram ( struct MarvelmindHedge *  hedge, uint8_t *  buffer  )

static

Definition at line 428 of file marvelmind.c.

                                                                                     {
  uint8_t* dataBuf = &buffer[5];
 
  hedge->rawIMU.acc_x = get_int16(&dataBuf[0]);
  hedge->rawIMU.acc_y = get_int16(&dataBuf[2]);
  hedge->rawIMU.acc_z = get_int16(&dataBuf[4]);
 
  //
  hedge->rawIMU.gyro_x = get_int16(&dataBuf[6]);
  hedge->rawIMU.gyro_y = get_int16(&dataBuf[8]);
  hedge->rawIMU.gyro_z = get_int16(&dataBuf[10]);
 
  //
  hedge->rawIMU.compass_x = get_int16(&dataBuf[12]);
  hedge->rawIMU.compass_y = get_int16(&dataBuf[14]);
  hedge->rawIMU.compass_z = get_int16(&dataBuf[16]);
 
  hedge->rawIMU.timestamp = get_uint32(&dataBuf[24]);
 
  hedge->rawIMU.updated = true;
}

References RawIMUValue::acc_x, RawIMUValue::acc_y, RawIMUValue::acc_z, RawIMUValue::compass_x, RawIMUValue::compass_y, RawIMUValue::compass_z, get_int16(), get_uint32(), RawIMUValue::gyro_x, RawIMUValue::gyro_y, RawIMUValue::gyro_z, MarvelmindHedge::rawIMU, RawIMUValue::timestamp, and RawIMUValue::updated.

Referenced by Marvelmind_Thread_().

Here is the call graph for this function:

Here is the caller graph for this function:

process_position_datagram()

static struct PositionValue process_position_datagram ( struct MarvelmindHedge *  hedge, uint8_t *  buffer  )

static

Definition at line 260 of file marvelmind.c.

                                                                                                      {
  uint8_t ind = hedge->lastValues_next;
 
  hedge->positionBuffer[ind].address =
      buffer[16];
  hedge->positionBuffer[ind].timestamp =
      buffer[5] |
      (((uint32_t)buffer[6]) << 8) |
      (((uint32_t)buffer[7]) << 16) |
      (((uint32_t)buffer[8]) << 24);
 
  int16_t vx = buffer[9] |
      (((uint16_t)buffer[10]) << 8);
  hedge->positionBuffer[ind].x = vx * 10; // millimeters
 
  int16_t vy = buffer[11] |
      (((uint16_t)buffer[12]) << 8);
  hedge->positionBuffer[ind].y = vy * 10; // millimeters
 
  int16_t vz = buffer[13] |
      (((uint16_t)buffer[14]) << 8);
  hedge->positionBuffer[ind].z = vz * 10; // millimeters
 
  uint16_t vang = buffer[17] |
      (((uint16_t)buffer[18]) << 8);
  hedge->positionBuffer[ind].angle = ((float)(vang & 0x0fff)) / 10.0f;
 
  hedge->positionBuffer[ind].highResolution = false;
 
  ind = markPositionReady(hedge);
 
  return hedge->positionBuffer[ind];
}

References PositionValue::address, PositionValue::angle, PositionValue::highResolution, markPositionReady(), PositionValue::timestamp, PositionValue::x, PositionValue::y, and PositionValue::z.

Referenced by Marvelmind_Thread_().

Here is the call graph for this function:

Here is the caller graph for this function:

process_position_highres_datagram()

static struct PositionValue process_position_highres_datagram ( struct MarvelmindHedge *  hedge, uint8_t *  buffer  )

static

Definition at line 294 of file marvelmind.c.

                                                                                                              {
  uint8_t ind = hedge->lastValues_next;
 
  hedge->positionBuffer[ind].address =
      buffer[22];
  hedge->positionBuffer[ind].timestamp =
      buffer[5] |
      (((uint32_t)buffer[6]) << 8) |
      (((uint32_t)buffer[7]) << 16) |
      (((uint32_t)buffer[8]) << 24);
 
  int32_t vx = buffer[9] |
      (((uint32_t)buffer[10]) << 8) |
      (((uint32_t)buffer[11]) << 16) |
      (((uint32_t)buffer[12]) << 24);
  hedge->positionBuffer[ind].x = vx;
 
  int32_t vy = buffer[13] |
      (((uint32_t)buffer[14]) << 8) |
      (((uint32_t)buffer[15]) << 16) |
      (((uint32_t)buffer[16]) << 24);
  hedge->positionBuffer[ind].y = vy;
 
  int32_t vz = buffer[17] |
      (((uint32_t)buffer[18]) << 8) |
      (((uint32_t)buffer[19]) << 16) |
      (((uint32_t)buffer[20]) << 24);
  hedge->positionBuffer[ind].z = vz;
 
  uint16_t vang = buffer[23] |
      (((uint16_t)buffer[24]) << 8);
  hedge->positionBuffer[ind].angle = ((float)(vang & 0x0fff)) / 10.0f;
 
  hedge->positionBuffer[ind].highResolution = true;
 
  ind = markPositionReady(hedge);
 
  return hedge->positionBuffer[ind];
}

References PositionValue::address, PositionValue::angle, PositionValue::highResolution, markPositionReady(), PositionValue::timestamp, PositionValue::x, PositionValue::y, and PositionValue::z.

Referenced by Marvelmind_Thread_().

Here is the call graph for this function:

Here is the caller graph for this function:

process_raw_distances_datagram()

static void process_raw_distances_datagram ( struct MarvelmindHedge *  hedge, uint8_t *  buffer  )

static

Definition at line 475 of file marvelmind.c.

                                                                                           {
  uint8_t* dataBuf = &buffer[5];
  uint8_t ofs, i;
 
  hedge->rawDistances.address_hedge = dataBuf[0];
 
  ofs = 1;
  for(i = 0; i < 4; i++) {
    hedge->rawDistances.distances[i].address_beacon = dataBuf[ofs + 0];
    hedge->rawDistances.distances[i].distance       = get_uint32(&dataBuf[ofs + 1]);
    ofs += 6;
  }
 
  hedge->rawDistances.updated = true;
}

References RawDistanceItem::address_beacon, RawDistances::address_hedge, RawDistanceItem::distance, RawDistances::distances, get_uint32(), MarvelmindHedge::rawDistances, and RawDistances::updated.

Referenced by Marvelmind_Thread_().

Here is the call graph for this function:

Here is the caller graph for this function:

startMarvelmindHedge()

void startMarvelmindHedge

(

struct MarvelmindHedge *

hedge

)

Definition at line 718 of file marvelmind.c.

                                                         {
  uint8_t i;
 
  hedge->positionBuffer =
      malloc(sizeof(struct PositionValue) * MAX_BUFFERED_POSITIONS);
  if(hedge->positionBuffer == NULL) {
    if(hedge->verbose)
      puts("Not enough memory");
    hedge->terminationRequired = true;
    return;
  }
  for(i = 0; i < MAX_BUFFERED_POSITIONS; i++) {
    hedge->positionBuffer[i].ready     = false;
    hedge->positionBuffer[i].processed = false;
  }
  hedge->positionsBeacons.numBeacons = 0;
  hedge->positionsBeacons.updated    = false;
 
#ifdef WIN32
  _beginthread(Marvelmind_Thread_, 0, hedge);
#else
  pthread_create(&hedge->thread_, NULL, Marvelmind_Thread_, hedge);
#endif
}

References Marvelmind_Thread_(), MAX_BUFFERED_POSITIONS, StationaryBeaconsPositions::numBeacons, MarvelmindHedge::positionBuffer, MarvelmindHedge::positionsBeacons, PositionValue::processed, PositionValue::ready, MarvelmindHedge::terminationRequired, MarvelmindHedge::thread_, StationaryBeaconsPositions::updated, and MarvelmindHedge::verbose.

Referenced by MMNode::MMNode().

Here is the call graph for this function:

Here is the caller graph for this function:

stopMarvelmindHedge()

void stopMarvelmindHedge

(

struct MarvelmindHedge *

hedge

)

Definition at line 1062 of file marvelmind.c.

                                                        {
  hedge->terminationRequired = true;
  if(hedge->verbose)
    puts("stopping");
#ifdef WIN32
  WaitForSingleObject(hedge->thread_, INFINITE);
#else
  pthread_join(hedge->thread_, NULL);
#endif
}

References MarvelmindHedge::terminationRequired, MarvelmindHedge::thread_, and MarvelmindHedge::verbose.