inline static int moveOV8825AF(unsigned long a_u4Position)
{
int ret = 0;
if ((a_u4Position > g_u4OV8825AF_MACRO) || (a_u4Position < g_u4OV8825AF_INF)) {
OV8825AFDB("[OV8825AF] out of range\n");
return -EINVAL;
}
if (g_s4OV8825AF_Opened == 1) {
unsigned short InitPos;
ret = s4OV8825AF_ReadReg(&InitPos);
if (ret == 0) {
OV8825AFDB("[OV8825AF] Init Pos %6d\n", InitPos);
spin_lock(&g_OV8825AF_SpinLock);
g_u4CurrPosition = (unsigned long)InitPos;
spin_unlock(&g_OV8825AF_SpinLock);
} else {
spin_lock(&g_OV8825AF_SpinLock);
g_u4CurrPosition = 0;
spin_unlock(&g_OV8825AF_SpinLock);
}
spin_lock(&g_OV8825AF_SpinLock);
g_s4OV8825AF_Opened = 2;
spin_unlock(&g_OV8825AF_SpinLock);
}
if (g_u4CurrPosition < a_u4Position) {
spin_lock(&g_OV8825AF_SpinLock);
g_i4Dir = 1;
spin_unlock(&g_OV8825AF_SpinLock);
} else if (g_u4CurrPosition > a_u4Position) {
spin_lock(&g_OV8825AF_SpinLock);
g_i4Dir = -1;
spin_unlock(&g_OV8825AF_SpinLock);
} else {
return 0;
}
spin_lock(&g_OV8825AF_SpinLock);
g_u4TargetPosition = a_u4Position;
spin_unlock(&g_OV8825AF_SpinLock);
/* OV8825AFDB("[OV8825AF] move [curr] %d [target] %d\n", g_u4CurrPosition, g_u4TargetPosition); */
spin_lock(&g_OV8825AF_SpinLock);
g_sr = 3;
g_i4MotorStatus = 0;
spin_unlock(&g_OV8825AF_SpinLock);
if (s4OV8825AF_WriteReg((unsigned short)g_u4TargetPosition) == 0) {
spin_lock(&g_OV8825AF_SpinLock);
g_u4CurrPosition = (unsigned long)g_u4TargetPosition;
spin_unlock(&g_OV8825AF_SpinLock);
} else {
OV8825AFDB("[OV8825AF] set I2C failed when moving the motor\n");
spin_lock(&g_OV8825AF_SpinLock);
g_i4MotorStatus = -1;
spin_unlock(&g_OV8825AF_SpinLock);
}
return 0;
}
inline static int moveOV8825AF(unsigned long a_u4Position)
{
if((a_u4Position > g_u4OV8825AF_MACRO) || (a_u4Position < g_u4OV8825AF_INF))
{
OV8825AFDB("[OV8825AF] out of range \n");
return -EINVAL;
}
if (g_s4OV8825AF_Opened == 1)
{
unsigned short InitPos;
if(s4OV8825AF_ReadReg(&InitPos) == 0)
{
OV8825AFDB("[OV8825AF] Init Pos %6d \n", InitPos);
g_u4CurrPosition = (unsigned long)InitPos;
}
else
{
g_u4CurrPosition = 0;
}
g_s4OV8825AF_Opened = 2;
}
if (g_u4CurrPosition < a_u4Position) {g_i4Dir = 1;}
else if (g_u4CurrPosition > a_u4Position) {g_i4Dir = -1;}
else {return 0;}
if (1)
{
g_i4Position = (long)g_u4CurrPosition;
g_u4TargetPosition = a_u4Position;
if (g_i4Dir == 1)
{
//if ((g_u4TargetPosition - g_u4CurrPosition)<60)
{
g_i4MotorStatus = 0;
if(s4OV8825AF_WriteReg((unsigned short)g_u4TargetPosition) == 0)
{
g_u4CurrPosition = (unsigned long)g_u4TargetPosition;
}
else
{
OV8825AFDB("[OV8825AF] set I2C failed when moving the motor \n");
g_i4MotorStatus = -1;
}
}
//else
//{
// g_i4MotorStatus = 1;
//}
}
else if (g_i4Dir == -1)
{
//if ((g_u4CurrPosition - g_u4TargetPosition)<60)
{
g_i4MotorStatus = 0;
if(s4OV8825AF_WriteReg((unsigned short)g_u4TargetPosition) == 0)
{
g_u4CurrPosition = (unsigned long)g_u4TargetPosition;
}
else
{
OV8825AFDB("[OV8825AF] set I2C failed when moving the motor \n");
g_i4MotorStatus = -1;
}
}
//else
//{
// g_i4MotorStatus = 1;
//}
}
}
else
{
g_i4Position = (long)g_u4CurrPosition;
g_u4TargetPosition = a_u4Position;
g_i4MotorStatus = 1;
}
return 0;
}
inline static int moveOV8825AF(unsigned long a_u4Position)
{
int ret = 0;
OV8825AFDB("a_u4Position = %d \n", a_u4Position);
if((a_u4Position > g_u4OV8825AF_MACRO) || (a_u4Position < g_u4OV8825AF_INF))
{
OV8825AFDB("[OV8825AF] out of range \n");
return -EINVAL;
}
if (g_s4OV8825AF_Opened == 1)
{
unsigned short InitPos;
ret = s4OV8825AF_ReadReg(&InitPos);
spin_lock(&g_OV8825AF_SpinLock);
if(ret == 0)
{
OV8825AFDB("[OV8825AF] Init Pos %6d \n", InitPos);
g_u4CurrPosition = (unsigned long)InitPos;
}
else
{
g_u4CurrPosition = 0;
}
g_s4OV8825AF_Opened = 2;
spin_unlock(&g_OV8825AF_SpinLock);
}
if (g_u4CurrPosition < a_u4Position)
{
spin_lock(&g_OV8825AF_SpinLock);
g_i4Dir = 1;
spin_unlock(&g_OV8825AF_SpinLock);
}
else if (g_u4CurrPosition > a_u4Position)
{
spin_lock(&g_OV8825AF_SpinLock);
g_i4Dir = -1;
spin_unlock(&g_OV8825AF_SpinLock);
}
else {return 0;}
spin_lock(&g_OV8825AF_SpinLock);
//g_i4Position = (long)g_u4CurrPosition;
g_u4TargetPosition = a_u4Position;
g_sr = 3;
g_i4MotorStatus = 0;
spin_unlock(&g_OV8825AF_SpinLock);
if(abs(g_u4CurrPosition - g_u4TargetPosition) > MAX_MOVE_STEP)
{
OV8825AFDB("[OV8825AF] in small stepLeave breakdown \n");
breakDownSteps(MAX_MOVE_STEP, MOVE_INTERVAL, g_u4CurrPosition, g_u4TargetPosition);
}
else
{
if(s4OV8825AF_WriteReg((unsigned short)g_u4TargetPosition) == 0)
{
spin_lock(&g_OV8825AF_SpinLock);
g_u4CurrPosition = (unsigned long)g_u4TargetPosition;
spin_unlock(&g_OV8825AF_SpinLock);
}
else
{
OV8825AFDB("[OV8825AF] set I2C failed when moving the motor \n");
spin_lock(&g_OV8825AF_SpinLock);
g_i4MotorStatus = -1;
spin_unlock(&g_OV8825AF_SpinLock);
}
}
return 0;
}
