我们在第二节就分析到了 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
