CC2431 代码分析④-衣锦还乡的CC2431

我们在第二节就分析到了 finishCollection( void )，但是当我们分析完第三节后，整个系统才真正执行到这里，我们依然像第二节一样把这个函数全部贴出来

/*********************************************************************
 * @fn      finishCollection
 *
 * @brief   Sends the next Bind Node Response message
 *
 * @param   none
 *
 * @return  none
 */
static void finishCollection( void )
{
  if ((ZDO_Config_Node_Descriptor.CapabilityFlags & CAPINFO_RCVR_ON_IDLE) == 0)
  {
    uint8 x;

    // Turn the receiver back off while idle
    x = false;
    ZMacSetReq( ZMacRxOnIdle, &x );
  }

  // Send the Blind node response
  sendRsp();

  if ( config.mode == NODE_MODE_AUTO )
  {
    // set up next auto response
    osal_start_timerEx( BlindNode_TaskID, BLINDNODE_FIND_EVT, config.cycle );
    defAddr.addrMode = afAddr16Bit;
    defAddr.addr.shortAddr = config.dstAddr;
    defAddr.endPoint = config.dstEp;
  }

  state = eBnIdle;
}

　

其它不解释了，具体参见第二节，我们下面直接进入函数sendRsp

/*********************************************************************
 * @fn      sendRsp
 *
 * @brief   Build and send the response message.
 *
 * @param   None.
 *
 * @return  status from call to AF_DataRequest().
 */
static afStatus_t sendRsp( void )
{
  uint8 msg[BLINDNODE_RESPONSE_LEN];
  LocRefNode_t locNodes[BLINDNODE_MAX_REF_NODES];
  uint16 xOff, yOff;
  uint8 idx, cnt = 0;

  for ( idx = 0; idx < rspCnt; idx++ )
  {
    BLINDNODE_CONV_RSSI( refNodes[idx].rssi );
    if ( refNodes[idx].rssi != 0 )
    {
      cnt++;
    }
  }

  if ( cnt >= config.minRefNodes )
  {
    msg[BLINDNODE_RESPONSE_STATUS_IDX] = BLINDNODE_RSP_STATUS_SUCCESS;

    // Sort the ref nodes by RSSI in order to pass the best 16 to loc engine.
    rspCnt = sortNodes( refNodes );

    calcOffsets( refNodes, &xOff, &yOff );

    // Convert to logical coordinates.
    setLogicals( locNodes, refNodes, xOff, yOff );

    // Run the location calculation
    locationCalculatePosition( locNodes, &(config.loc) );

    // Convert results to real coordinates and average over several samples.
    xOff += config.loc.x;
    yOff += config.loc.y;

    if ( ((xOff > xOld) && ((xOff - xOld) > BLINDNODE_FLUSH))  ||
         ((xOff < xOld) && ((xOld - xOff) > BLINDNODE_FLUSH))  ||
         ((yOff > yOld) && ((yOff - yOld) > BLINDNODE_FLUSH))  ||
         ((yOff < yOld) && ((yOld - yOff) > BLINDNODE_FLUSH)) )
    {
      xOld = xOff;
      yOld = yOff;
    }
    else
    {
      xOld = ((xOld * (BLINDNODE_FILTER-1)) + xOff) / BLINDNODE_FILTER;
      yOld = ((yOld * (BLINDNODE_FILTER-1)) + yOff) / BLINDNODE_FILTER;
    }

    xOff = (uint16)xOld;
    yOff = (uint16)yOld;
  }
  else
  {
    msg[BLINDNODE_RESPONSE_STATUS_IDX] = BLINDNODE_RSP_STATUS_NOT_ENOUGH_REFNODES;

    xOff = (uint16)xOld;
    yOff = (uint16)yOld;
  }
  rspMsg[REFNODE_CONFIG_X_LO_IDX] =
  msg[BLINDNODE_RESPONSE_X_LO_IDX] = LO_UINT16( xOff );
  rspMsg[REFNODE_CONFIG_X_HI_IDX] =
  msg[BLINDNODE_RESPONSE_X_HI_IDX] = HI_UINT16( xOff );
  rspMsg[REFNODE_CONFIG_Y_LO_IDX] =
  msg[BLINDNODE_RESPONSE_Y_LO_IDX] = LO_UINT16( yOff );
  rspMsg[REFNODE_CONFIG_Y_HI_IDX] =
  msg[BLINDNODE_RESPONSE_Y_HI_IDX] = HI_UINT16( yOff );

  msg[BLINDNODE_RESPONSE_NUMREFNODES_IDX] = cnt;

  if ( rspCnt != 0 )
  {
    msg[BLINDNODE_RESPONSE_REF_SHORTADDR_LO_IDX] = LO_UINT16( refNodes->addr );
    msg[BLINDNODE_RESPONSE_REF_SHORTADDR_HI_IDX] = HI_UINT16( refNodes->addr );
    msg[BLINDNODE_RESPONSE_REF_X_LO_IDX] = LO_UINT16( refNodes->x );
    msg[BLINDNODE_RESPONSE_REF_X_HI_IDX] = HI_UINT16( refNodes->x );
    msg[BLINDNODE_RESPONSE_REF_Y_LO_IDX] = LO_UINT16( refNodes->y );
    msg[BLINDNODE_RESPONSE_REF_Y_HI_IDX] = HI_UINT16( refNodes->y );
    msg[BLINDNODE_RESPONSE_REF_RSSI] = refNodes->rssi;
  }
  else
  {
    msg[BLINDNODE_RESPONSE_REF_SHORTADDR_LO_IDX] = LO_UINT16( INVALID_NODE_ADDR );
    msg[BLINDNODE_RESPONSE_REF_SHORTADDR_HI_IDX] = HI_UINT16( INVALID_NODE_ADDR );
    msg[BLINDNODE_RESPONSE_REF_X_LO_IDX] = LO_UINT16( LOC_DEFAULT_X_Y );
    msg[BLINDNODE_RESPONSE_REF_X_HI_IDX] = HI_UINT16( LOC_DEFAULT_X_Y );
    msg[BLINDNODE_RESPONSE_REF_Y_LO_IDX] = LO_UINT16( LOC_DEFAULT_X_Y );
    msg[BLINDNODE_RESPONSE_REF_Y_HI_IDX] = HI_UINT16( LOC_DEFAULT_X_Y );
    msg[BLINDNODE_RESPONSE_REF_RSSI] = 0xFF;
  }

  osal_start_timerEx( BlindNode_TaskID, BLINDNODE_WAIT_EVT, 1000 );

  return ( AF_DataRequest( &defAddr, (endPointDesc_t *)&epDesc,
                           LOCATION_BLINDNODE_FIND_RESPONSE,
                           BLINDNODE_RESPONSE_LEN, msg,
                           &transId, 0, AF_DEFAULT_RADIUS ) );
}

这个函数code比较长，而且里面还调用了很多函数，如果我们分析完这个函数CC2431 盲节点的代码也就分析完了（除盲节点配置外）。

我们一点一点分析这个函数的内容，首先

for ( idx = 0; idx < rspCnt; idx++ )
  {
    BLINDNODE_CONV_RSSI( refNodes[idx].rssi );
    if ( refNodes[idx].rssi != 0 )
    {
      cnt++;
    }
  }

　　一个for 循环，循环的次数是rspCnt这个全局变量，它是值的大小在CC2431盲节点接收收据的时候被赋值，表示了有多少个CC2430参考节点回应数据，具体这个量随实际情况变换。 后面有个宏BLINDNODE_CONV_RSSI( refNodes[idx].rssi );

#define BLINDNODE_CONV_RSSI( lqi ) \
 st ( \
  if ( lqi <= BLINDNODE_MIN_RSSI ) \
    lqi = 0; \
  else if ( lqi >= BLINDNODE_MAX_RSSI ) \
    lqi = -LOC_ENGINE_MAX_DBM; \
  else \
    lqi = -BLINDNODE_MIN_DBM - ((uint8)(((uint16)(lqi) * _ED_RF_POWER_DELTA) / _MAC_SPEC_ED_MAX)); \
 )

　　这个宏的最大作用是 将RSSI做换成LQI，并且要考虑RSSI最大最小情况。 RSSI与LQI的关系参见博文http://lfwendula0.blog.163.com/blog/static/20513823620127282719319/，从这里我们可以看出，其实实际在计算距离的时候是用的LQI。

下面在接着分析代码

  if ( cnt >= config.minRefNodes )
  {
    msg[BLINDNODE_RESPONSE_STATUS_IDX] = BLINDNODE_RSP_STATUS_SUCCESS;

    // Sort the ref nodes by RSSI in order to pass the best 16 to loc engine.
    rspCnt = sortNodes( refNodes );

    calcOffsets( refNodes, &xOff, &yOff );

    // Convert to logical coordinates.
    setLogicals( locNodes, refNodes, xOff, yOff );

    // Run the location calculation
    locationCalculatePosition( locNodes, &(config.loc) );

    // Convert results to real coordinates and average over several samples.
    xOff += config.loc.x;
    yOff += config.loc.y;

    if ( ((xOff > xOld) && ((xOff - xOld) > BLINDNODE_FLUSH))  ||
         ((xOff < xOld) && ((xOld - xOff) > BLINDNODE_FLUSH))  ||
         ((yOff > yOld) && ((yOff - yOld) > BLINDNODE_FLUSH))  ||
         ((yOff < yOld) && ((yOld - yOff) > BLINDNODE_FLUSH)) )
    {
      xOld = xOff;
      yOld = yOff;
    }
    else
    {
      xOld = ((xOld * (BLINDNODE_FILTER -1)) + xOff) / BLINDNODE_FILTER;
      yOld = ((yOld * (BLINDNODE_FILTER-1)) + yOff) / BLINDNODE_FILTER;
    }

    xOff = (uint16)xOld;
    yOff = (uint16)yOld;
  }

　　上面贴出的代码是满足条件 cnt >= config.minRefNodes，也就是通过前面的code分析，并最终获得的可用参考节点数量大于config.minRefNodes，通常config.minRefNodes这个值定义为3，因为算法是三边定位，最小需要三个节点。当然可以设置为4 或者更大的值（如果系统中只有三个节点，设置成4则永远无法实现定位）。后面else 部分的code 没有贴出来，后面的代码就是没有足够多的参考节点使用上次的坐标信息。

　　如果这个if 条件里的code分析完，CC431的代码也就基本完事了。我们分别看里面的几个主要函数。

　　 rspCnt = sortNodes( refNodes );　　

/*********************************************************************
 * @fn      sortNodes
 *
 * @brief   Sorts the nodes into a list with the best to least RSSI
 *
 * INPUTS:  ref - Array of reference nodes w/ RSSI values converted for
 *          location engine from 40 (strong) to 95 (weak) & zero = too week.
 *
 * OUTPUTS: none
 *
 * @return  Count of non-zero RSSI entries.
 */
static uint8 sortNodes( RefNode_t *ref )
{
  RefNode_t *workNodes;
  uint8 idx;

  workNodes = osal_mem_alloc( sizeof( RefNode_t ) * rspCnt );

  if ( workNodes == NULL )
  {
    return 0;
  }

  osal_memcpy( workNodes, ref, sizeof( RefNode_t ) * rspCnt );

  for ( idx = 0; idx < rspCnt; idx++ )
  {
    RefNode_t *node = findBestRSSI( workNodes );

    if ( node == NULL )
    {
      break;
    }
    else
    {
      osal_memcpy( ref, node, sizeof( RefNode_t ) );
      node->addr = INVALID_NODE_ADDR;
    }

    ref++;
  }

  osal_mem_free( workNodes );

  return idx;
}

　　这个函数的意义和代码很简单，就是把保存参考节点信息的数组按照RSSI(其实现在是LQI)的大小重新排序。具体为何排序，我的理解是定位引擎只会利用数组前面若干个参考节点的信息，后面的一部分是不用的。 假如这样想，CC2431收到150个参考节点的回复，而定位引擎只需要使用100个，那么最好的办法是使用最好的100个参考节点。

calcOffsets( refNodes, &xOff, &yOff );

/*********************************************************************
 * @fn      calcOffsets
 *
 * @brief   Calculates the XY offsets.
 *
 * INPUTS:
 * @param   ref - Array of reference nodes, pre-sorted on RSSI, best to worst.
 *
 * OUTPUTS:
 * @param   xOff - pointer to X offset
 * param    yOff - pointer to Y offset
 *
 * @return  none
 */
static void calcOffsets( RefNode_t *ref, uint16 *xOff, uint16 *yOff )
{
  RefNode_t *rnP = ref;
  uint16 xMax = 0;
  uint16 yMax = 0;
  uint8 idx;

  for ( idx = 0; idx < rspCnt; idx++ )
  {
    if ( xMax < rnP->x )
    {
      xMax = rnP->x;
    }
    if ( yMax < rnP->y )
    {
      yMax = rnP->y;
    }

    rnP++;
  }

  // No need for conversion.
  if ( (xMax < 256) && (yMax < 256) )
  {
    *xOff = *yOff = 0;
  }
  else
  {
    // Force reference node with the best RSSI to sit at logical (32,32).
    *xOff = (ref->x & 0xFFFC) - 128;
    *yOff = (ref->y & 0xFFFC) - 128;
  }
}

　　这个函数就比较难以理解了，其实简单一说，就觉得哦原来这样。首先我们我们知道CC2431定位范围是64*64平米的一个范围，但这个是一个CC2431所能覆盖的范围，但并不是一个系统的范围，定位系统可以覆盖的范围几百几千米，具体要看参考节点的范围。为何CC2431只能覆盖64*64 平米，因为CC2431使用8 bit 作为传入参数，定位精度0.25米。 2^8 * 0.25 =64.

下面我们画几张图理解一下

第一张图： 常规应用，红色为2431，黑色为参考节点，所有的节点都在64 * 64 平米内

第二张图

下面这张图片是一个大系统的定位，不只是64*64平范围了，那么CC2431不能定位吗？ 可以，我们可以计算偏移实现。

相对上面的(0,0) 坐标，现在变成了(100,100). 计算偏移的方法其实很简单，就是强制把参考节点中最好的一个强制换成(32,32)

第三张图片

我们强制把下面的一个节点的坐标换成(32,32),(我们假定离CC2431节点越近RSSI越好)。那么这个这点的做换过程出来的值就是偏移。假定开始的坐标是(112,113).那么偏移量分别是(80,81).

好了，到这里看完三张图片， calcOffsets 这个函数也就理解完了。

setLogicals( locNodes, refNodes, xOff, yOff );

/*********************************************************************
 * @fn      setLogicals
 *
 * @brief   Sets the reference node's logical coordinates & RSSI for the
 *          required number of inputs to the location engine.
 *
 * INPUTS:
 * @param   ref - array of reference nodes
 * @param   offsetX - X offset used to make logical numbers
 * param    offsetY - Y offset used to make logical numbers
 *
 * @return  none
 */
static void setLogicals( LocRefNode_t *loc, RefNode_t *ref,
                                               uint16 xOff, uint16 yOff )
{
  // Rev-B Chip have LocEng Ver 1.0 w/ cap=8, Rev-C have LocEng Ver 2.0 w/ 16.
  const uint8 stop = ( ( CHVER == 0x01 ) ? LOC_ENGINE_NODE_CAPACITY_REVB :
                                          LOC_ENGINE_NODE_CAPACITY_REVC);
  uint16 xTmp, yTmp;
  uint8 idx;

  // Set the logical coordinates
  for ( idx = 0; idx < rspCnt; idx++ )
  {
    xTmp = ref->x - xOff;
    yTmp = ref->y - yOff;

    if ( (xTmp < 256) && (yTmp < 256) )
    {
      loc->x = (uint8)xTmp;
      loc->y = (uint8)yTmp;
      loc->rssi = ref->rssi;
    }
    else
    {
      // Out of bounds, so feed zero to location engine.
      loc->x = loc->y = loc->rssi = 0;
    }

    loc++;
    ref++;
  }

  for ( ; idx < stop; idx++ )
  {
    // Feed zero to location engine to meet the required number of inputs.
    loc->x = loc->y = 0;
    loc->rssi = 0;
    loc++;
  }
}

　　这个函数的意义也很简单， 上面不是我们已经计算出一个偏移了，但是其他节点都是用的实际坐标，我们需要将所有节点的坐标都转换一下。

　　

  // Set the logical coordinates
  for ( idx = 0; idx < rspCnt; idx++ )
  {
    xTmp = ref->x - xOff;
    yTmp = ref->y - yOff;

    if ( (xTmp < 256) && (yTmp < 256) )
    {
      loc->x = (uint8)xTmp;
      loc->y = (uint8)yTmp;
      loc->rssi = ref->rssi;
    }
    else
    {
      // Out of bounds, so feed zero to location engine.
      loc->x = loc->y = loc->rssi = 0;
    }

    loc++;
    ref++;
  }

　　后面有个判断，如果转换后的坐标信息大于(64,64),则把RSSI的值设置为0， 因为2431 只能利用64*64(相对来说)范围内的信息。

上面坐标信息做换完成了，下面就是定位了

locationCalculatePosition( locNodes, &(config.loc) );

这个函数具体内容不分析了，可以查看CC2431的数据手册。 CC2430与CC2431的区别就这个部分，如果这个函数用软件实现了，那么定位就无需用CC2431芯片了。

定位完成后，出来的坐标信息是相对的，我们需要加上之前计算的偏移量就是真正的坐标了

   // Convert results to real coordinates and average over several samples.
    xOff += config.loc.x;
    yOff += config.loc.y;

　　

滤波算法，TI知道CC2431使用RSSI定位有些精度上的问题，所以后面加了一个非常简答的滤波算法，有兴趣的同学可以实现更为高级的滤波算法， 我先简单分析一下Ti这个滤波算法

    if ( ((xOff > xOld) && ((xOff - xOld) > BLINDNODE_FLUSH))  ||
         ((xOff < xOld) && ((xOld - xOff) > BLINDNODE_FLUSH))  ||
         ((yOff > yOld) && ((yOff - yOld) > BLINDNODE_FLUSH))  ||
         ((yOff < yOld) && ((yOld - yOff) > BLINDNODE_FLUSH)) )
    {
      xOld = xOff;
      yOld = yOff;
    }
    else
    {
      xOld = ((xOld * (BLINDNODE_FILTER -1)) + xOff) / BLINDNODE_FILTER;
      yOld = ((yOld * (BLINDNODE_FILTER-1)) + yOff) / BLINDNODE_FILTER;
    }

    xOff = (uint16)xOld;
    yOff = (uint16)yOld;

　　

　　

CC2431定位套餐推荐:https://item.taobao.com/item.htm?id=527836022363