/*---------------------------------------------------------------------------
TITLE : Hw_I2C_IMU_GetErrStatus
WORK :
ARG : void
RET : void
---------------------------------------------------------------------------*/
u16 Hw_I2C_IMU_GetErrStatus( void )
{
u16 Err = 0;
u16 SR;
if( IS_SET_BIT(REG_I2C1_SR1, 11) ) Err = 11;
if( IS_SET_BIT(REG_I2C1_SR1, 10) ) Err = 10;
if( IS_SET_BIT(REG_I2C1_SR1, 9) ) Err = 9;
if( IS_SET_BIT(REG_I2C1_SR1, 8) ) Err = 8;
//-- 에러가 난경우 복구
//
if( Err != 0 )
{
CLR_BIT( REG_I2C1_CR1, 0 ); // Peripheral disable
SR = REG_I2C1_SR1;
SR = REG_I2C1_SR2;
SET_BIT( REG_I2C1_CR1, 0 ); // Peripheral enable
SR = REG_I2C1_SR1;
SR = REG_I2C1_SR2;
}
return Err;
}
Std_ReturnType Serial_BackgroundTask()
{
Std_ReturnType retval = Status_OK;
if (transmitStateMachine == Serial_Writing)
{
// Check for empty transmit buffer
if (IS_SET_BIT(UCSR0A, UDRE0))
{
if (transmitLength > 0)
{
UDR0 = *(transmitBuffer++);
transmitLength--;
}
else
{
transmitStateMachine = Serial_Ready;
}
}
// More processing needed
retval = Status_Pending;
}
return retval;
}
/*---------------------------------------------------------------------------
TITLE : Hw_I2C_IMU_WaitForBitSet
WORK :
ARG : void
RET : void
---------------------------------------------------------------------------*/
u16 Hw_I2C_IMU_WaitForBitSet( u32 *reg_addr, u32 reg_bit, u32 time_out )
{
u32 Ret = 0;
volatile u32 Reg;
while( time_out )
{
Reg = *reg_addr;
if( IS_SET_BIT( Reg, reg_bit ) ) break;
time_out--;
}
if( time_out == 0 )
{
Ret = HW_I2C_ERR_TIMEOUT;
}
return Ret;
}
void EdgeDetector::search_for_closest_edge_point_with_pole( ClosestEdge& result, std::vector<TooN::Vector<3> > & vv3EdgeStrs,
const EdgeFeature & edgeFeature, const CVD::Image<REAL_TYPE>& image, const TooN::Vector<2> & v2ImagePoint, const TooN::Vector<2>& v2Normal )
{
TooN::Vector<3> edgeStr;
TooN::Vector<2> v2Xn;
register int i, j, k, chkpt, nSearch, nSearch_2nw, nSearch_nw, TwoiMaxSearchRange;
// apply mask to find edgeStr in both magnitude and direction for a whole range of search path.
TwoiMaxSearchRange = 2*miMaxSearchRange;
for( nSearch = 0; nSearch <= TwoiMaxSearchRange; ++nSearch )
{
v2Xn = v2ImagePoint + ( nSearch - miMaxSearchRange ) * v2Normal;
if( !image.in_image_with_border( CVD::ir( v2Xn ), 10 ) ) break;
edgeStrength( edgeStr, image, int( v2Xn[0] ), int( v2Xn[1] ) );
vv3EdgeStrs[nSearch] = edgeStr;
}
// 1D NMS for (2nw+1)-Neighborhood
// Efficient Non-Maximum Suppression, Alexander Neubeck and Luc Van Gool, ICPR, 2006
result.bNotFound = true;
REAL_TYPE subpixel, distance, test;
result.rDistance = 2 * miMaxSearchRange + 1.f;
const int nw = 2; // test nw = 2, must be more than or equal to 2;
i = nw;
CompPartialMax( vv3EdgeStrs, 0, i - 1 );
chkpt = -1;
nSearch_2nw = nSearch - 2*nw;
nSearch_nw = nSearch - nw;
while( i <= nSearch_2nw )
{
j = CompPartialMax( vv3EdgeStrs, i, i + nw );
k = CompPartialMax( vv3EdgeStrs, i + nw + 1, j + nw );
if( i == j || vv3EdgeStrs[j][0] > vv3EdgeStrs[k][0] )
{
if( ( chkpt <= j - nw || vv3EdgeStrs[j][0] >= mvrPMAX[chkpt] )
&& ( j - nw == i || vv3EdgeStrs[j][0] >= mvrPMAX[j - nw] ) )
{
if( vv3EdgeStrs[j][0] > mfEdgeThreshold ) // found edge
{
test = v2Normal*vv3EdgeStrs[j].slice(1,2);
if( ( IS_SET_BIT( edgeFeature.ucBits, EDGE_POLE ) && test > 0 ) || ( !IS_SET_BIT( edgeFeature.ucBits, EDGE_POLE ) && test <= 0 ) )
{
if( fabs(test) > COSANGLE ) // check angle
{
distance = j - miMaxSearchRange;
if( fabs( result.rDistance ) >= fabs( distance ) )
{
result.rDistance = distance;
result.iEdgeIndex = j;
result.bNotFound = false;
}
else
goto exit_label;
}
}
}
}
if( i < j ) chkpt = i + nw + 1;
i = j + nw + 1;
}
else
{
i = k;
chkpt = j + nw + 1;
while( i <= nSearch_nw )
{
j = CompPartialMax( vv3EdgeStrs, chkpt, i + nw );
if( vv3EdgeStrs[i][0] > vv3EdgeStrs[j][0] )
{
if( vv3EdgeStrs[i][0] > mfEdgeThreshold ) // found edge
{
test = v2Normal*vv3EdgeStrs[i].slice(1,2);
if( ( IS_SET_BIT( edgeFeature.ucBits, EDGE_POLE ) && test > 0 ) || ( !IS_SET_BIT( edgeFeature.ucBits, EDGE_POLE ) && test <= 0 ) )
{
if( fabs(test) > COSANGLE ) // check angle
{
distance = i - miMaxSearchRange;
if( fabs( result.rDistance ) >= fabs( distance) )
{
result.rDistance = distance;
result.iEdgeIndex = i;
result.bNotFound = false;
}
else
goto exit_label;
}
}
}
i = i + nw + 1;
break;
}
else
{
chkpt = i + nw - 1;
i = j;
}
}
}
}
exit_label:
if( !result.bNotFound )
{
// subpixel m = a-c/(2*(a-2b-c) where the parabola passing though (-1,a), (0,b), (1,c)
// A Non-Maxima Suppression Method for Edge Detection with Sub-Pixel Accuracy.
REAL_TYPE den = ( 2.0 * ( vv3EdgeStrs[result.iEdgeIndex - 1][0] - 2.0 * vv3EdgeStrs[result.iEdgeIndex][0] + vv3EdgeStrs[result.iEdgeIndex + 1][0] ) );
if( result.iEdgeIndex >= 1 && den != 0 )
subpixel = ( vv3EdgeStrs[result.iEdgeIndex - 1][0] - vv3EdgeStrs[result.iEdgeIndex + 1][0] ) / den;
else subpixel = 0.0;
result.rDistance += subpixel;
}
}
