C++ (Cpp) crtpReset Examples

Example #1

File: log.c Project: jannson/crazypony

/* This function is usually called by the worker subsystem */
void logRunBlock(void * arg)
{
  struct log_block *blk = arg;
  struct log_ops *ops = blk->ops;
  static CRTPPacket pk;
  unsigned int timestamp;
  
  xSemaphoreTake(logLock, portMAX_DELAY);

  timestamp = ((long long)xTaskGetTickCount())/portTICK_RATE_MS;
  
  pk.crtp_header = CRTP_HEADER(CRTP_PORT_LOG, LOG_CH);
  pk.size = 4;
  pk.crtp_data[0] = blk->id;
  pk.crtp_data[1] = timestamp&0x0ff;
  pk.crtp_data[2] = (timestamp>>8)&0x0ff;
  pk.crtp_data[3] = (timestamp>>16)&0x0ff;

  while (ops)
  {
    int valuei = 0;
    float valuef = 0;
     
    switch(ops->storageType)
    {
      case LOG_UINT8:
        valuei = *(uint8_t *)ops->variable;
        break;
      case LOG_INT8:
        valuei = *(int8_t *)ops->variable;
        break;
      case LOG_UINT16:
        valuei = *(uint16_t *)ops->variable;
        break;
      case LOG_INT16:
        valuei = *(int16_t *)ops->variable;
        break;
      case LOG_UINT32:
        valuei = *(uint32_t *)ops->variable;
        break;
      case LOG_INT32:
        valuei = *(int32_t *)ops->variable;
        break;
      case LOG_FLOAT:
        valuei = *(float *)ops->variable;
        break;
    }
    
    if (ops->logType == LOG_FLOAT || ops->logType == LOG_FP16)
    {
      if (ops->storageType == LOG_FLOAT)
        valuef = *(float *)ops->variable;
      else
        valuef = valuei;
      
      if (ops->logType == LOG_FLOAT)
      {
        memcpy(&pk.crtp_data[pk.size], &valuef, 4);
        pk.size += 4;
      }
      else
      {
        valuei = single2half(valuef);
        memcpy(&pk.crtp_data[pk.size], &valuei, 2);
        pk.size += 2;
      }
    }
    else  //logType is an integer
    {
      memcpy(&pk.crtp_data[pk.size], &valuei, typeLength[ops->logType]);
      pk.size += typeLength[ops->logType];
    }
    
    ops = ops->next;
  }
  
  xSemaphoreGive(logLock);

  // Check if the connection is still up, oherwise disable
  // all the logging and flush all the CRTP queues.
  if (!crtpIsConnected())
  {
    logReset();
    crtpReset(); 
  }
  else
  {
    crtpSendPacket(&pk);
  }
}

Example #2

File: log.c Project: rtt-gimbal/crazyflie-firmware

/* This function is usually called by the worker subsystem */
void logRunBlock(void * arg)
{
  struct log_block *blk = arg;
  struct log_ops *ops = blk->ops;
  static CRTPPacket pk;
  unsigned int timestamp;

  xSemaphoreTake(logLock, portMAX_DELAY);

  timestamp = ((long long)xTaskGetTickCount())/portTICK_RATE_MS;

  pk.header = CRTP_HEADER(CRTP_PORT_LOG, LOG_CH);
  pk.size = 4;
  pk.data[0] = blk->id;
  pk.data[1] = timestamp&0x0ff;
  pk.data[2] = (timestamp>>8)&0x0ff;
  pk.data[3] = (timestamp>>16)&0x0ff;

  while (ops)
  {
    float variable;
    int valuei = 0;
    float valuef = 0;

    // FPU instructions must run on aligned data. Make sure it is.
    variable = *(float *)ops->variable;

    switch(ops->storageType)
    {
      case LOG_UINT8:
        valuei = *(uint8_t *)&variable;
        break;
      case LOG_INT8:
        valuei = *(int8_t *)&variable;
        break;
      case LOG_UINT16:
        valuei = *(uint16_t *)&variable;
        break;
      case LOG_INT16:
        valuei = *(int16_t *)&variable;
        break;
      case LOG_UINT32:
        valuei = *(uint32_t *)&variable;
        break;
      case LOG_INT32:
        valuei = *(int32_t *)&variable;
        break;
      case LOG_FLOAT:
        valuei = *(float *)&variable;
        break;
    }

    if (ops->logType == LOG_FLOAT || ops->logType == LOG_FP16)
    {
      if (ops->storageType == LOG_FLOAT)
        valuef = *(float *)&variable;
      else
        valuef = valuei;

      // Try to append the next item to the packet.  If we run out of space,
      // drop this and subsequent items.
      if (ops->logType == LOG_FLOAT)
      {
        if (!appendToPacket(&pk, &valuef, 4)) break;
      }
      else
      {
        valuei = single2half(valuef);
        if (!appendToPacket(&pk, &valuei, 2)) break;
      }
    }
    else  //logType is an integer
    {
      if (!appendToPacket(&pk, &valuei, typeLength[ops->logType])) break;
    }

    ops = ops->next;
  }

  xSemaphoreGive(logLock);

  // Check if the connection is still up, oherwise disable
  // all the logging and flush all the CRTP queues.
  if (!crtpIsConnected())
  {
    logReset();
    crtpReset();
  }
  else
  {
    crtpSendPacket(&pk);
  }
}