从SOD到OOD（AutoFocus模块）

最新推荐文章于 2022-08-09 21:14:04 发布

Huskar_Liu

最新推荐文章于 2022-08-09 21:14:04 发布

阅读量170

点赞数

分类专栏： cpp

本文链接：https://blog.csdn.net/weixin_42418557/article/details/118729189

版权

cpp 专栏收录该内容

30 篇文章 0 订阅

订阅专栏

先来看看数据的封装

class AutoFocus
{
	...
public:
	SystemManager* sys_;
	
	const int max_steps_ = 16;
	
	int af_min_focus_;
	int af_max_focus_;
	int af_init_focus_;
	int af_dx_;
	u32 current_clarity_;
	u8 af_doing_;
};

首先定义了索引类的成员变量，这里定义了一个system manager的索引变量，用于实现源端引用。

然后定义了常量类的成员变量，

然后定义了特定的用途的成员变量。

再来看看操作集的封装。

class AutoFocus
{
public:
	AutoFocus(SystemManager* sys);

	void set_param(u32 min_focus, u32 max_focus, u32 init_focus)
	void set_af_min_focus(int value)
	void set_af_max_focus(int value)
	void set_af_dx(int value)
	
	int auto_focus_loop();
	
	int explore_hill_auto_focus_move(u32 min_focus_value, u32 max_focus_value, int timeout);

	...
};

定义：
1）构建函数

AutoFocus::AutoFocus(SystemManager* sys) : sys_(sys)
{
	set_param(30000, 60000, 45000);
	af_dx_ = 10000 / 128;//10000
	af_doing_ = 0;
}

对成员变量赋值。

2）SET系列

void AutoFocus::set_param(u32 min_focus, u32 max_focus, u32 init_focus) {
	af_min_focus_ = min_focus;
	af_max_focus_ = max_focus;
	af_init_focus_ = init_focus;
}

void AutoFocus::set_af_min_focus(int value) {
	af_min_focus_ = value;
}

void AutoFocus::set_af_max_focus(int value) {
	af_max_focus_ = value;
}

void AutoFocus::set_af_dx(int value) {
	af_dx_ = value;
}

根据传入的参数，对成员变量赋值。

4）explore_hill_auto_focus_move，功能级函数

int AutoFocus::explore_hill_auto_focus_move(u32 min_focus_value, u32 max_focus_value, int timeout)
{
	...
	MotorMcu* xmotor = &sys_->xmotor_;
	...
	sys_->xmotor_.set_focus_far(10);
	...
	usleep(1000 * 150);
	...
	while(1)
	{
		...
		if (sys_->sensor_frame_count_ != pre_frame_id) {	
			if ((down_count >= 10) || (now_focus <= min_focus_value)) {
				sys_->xmotor_.set_focus_near(10);
				break;
			}
			else if (up_count >= 10) {
				...
				up_flag = 1;
				break;
			}
		}
	}

	while(1)
	{
		if (sys_->sensor_frame_count_ != pre_frame_id) {
			if (down_count >= 10)
			{
				...
				break;
			}
		}
	}

	if (min_clarity >= max_clarity) {
		return -1;
	}

	while(1)
	{
		if (dlt_time > timeout) {
			...
			xmotor->set_motor_stop();
			return -1;
		}

		if (sys_->sensor_frame_count_ != pre_frame_id) {
			if (current_clarity > best_clarity) {
				...
				break;
			}
		}
	}

	return 0;
}

这里用了多个while(1)循环体，执行waitfor的工作。

4）auto_focus_loop，业务级函数

int AutoFocus::auto_focus_loop()
{
	int ret;

	af_doing_ = 1;
	ret = explore_hill_auto_focus_move(af_min_focus_, af_max_focus_, 5000);
	af_doing_ = 0;
	
	return ret;
}

这里使用了共享变量，和其他函数进行同步。
即af_doing，作为flag变量使用，这里起到的作用是mutex lock的作用。
在进入代码段之前，先上锁，执行完毕返回后，再解锁。
++++++++++++++++++++++++++++++++++++++++++++++++++++++++
详细解析功能级函数，explore_hill_auto_focus_move
找到system manager对象中的motormcu对象，然后为MotorMCU的对象赋予一个句柄，方便使用。

MotorMcu* xmotor = &sys_->xmotor_;

后续将使用这个句柄来获取motormcu的相关数据。

定义一个static变量，这个变量是函数内static变量，所以不会出现命令冲突问题。

static u32 pre_frame_id = 0;

首先获取zoom相关数据，计算zoom比例，

	int cur_zoom = xmotor->zoom_motor_value_;
	int zoom_scale = (cur_zoom - xmotor->min_zoom_value) * 1024 / (xmotor->max_zoom_value - xmotor->min_zoom_value);

然后获取dx相关数据，并根据zoom比例来计算dx。

	int min_dx = af_dx_;
	int max_dx = af_dx_ * 5;
	int dx = (max_dx - min_dx) * zoom_scale / 1024 + min_dx;

然后尝试性的向远焦点移动，并等待电机的迟滞，

sys_->xmotor_.set_focus_far(10);
usleep(1000 * 150);

然后记下时间戳，并试探回头，
（
在这个函数中，需要用一个数组作为window，记录下clarity。并用变量保存clarity的极值。

u32 clarity[max_steps_] = {0};
u32 min_clarity = 0xffffffff;
u32 max_clarity = 0;

）
本段代码如下：

u32 start_time = sys_->get_timestamp();
while(1)
	{
		//超时判断
		u32 end_time = sys_->get_timestamp();
		int dlt_time = (end_time > start_time) ? (end_time - start_time) : (start_time - end_time);
		if (dlt_time > timeout) {
			xmotor->set_motor_stop();
			return -1;
		}

		if (sys_->sensor_frame_count_ != pre_frame_id) {
			pre_frame_id = sys_->sensor_frame_count_;

			//记录当前，最大，最小清晰度
			u32 current_clarity = current_clarity_;
			for (int i = max_steps_ - 1; i >= 1; i--) {
				clarity[i] = clarity[i - 1];
			}
			clarity[0] = current_clarity;

			if (current_clarity > max_clarity) {
				max_clarity = current_clarity;
			}
			if (current_clarity < min_clarity) {
				min_clarity = current_clarity;
			}

			//判断一个区间内清晰度是否是变小
			down_count = 0;
			up_count = 0;
			for (int i = max_steps_ - 1; i >= 1; i--) {
				int diff = ABS_DEC(clarity[max_steps_ - 1], clarity[i - 1]);
				if ((clarity[max_steps_ - 1] != 0) && (diff > dx) && (clarity[max_steps_ - 1] > clarity[i - 1]))
				{
					down_count++;
				}
				if ((clarity[max_steps_ - 1] != 0) && (diff > dx) && (clarity[max_steps_ - 1] < clarity[i - 1]))
				{
					up_count++;
				}
			}

			//判断当前焦距位置是否是极限位置
			u32 now_focus = sys_->xmotor_.focus_motor_value_;

			//回头
			if (now_focus <= min_focus_value) {
				sys_->xmotor_.set_focus_near(10);
				down_flag = 1;
				break;
			}
			else if ((down_count >= 10) ) {
				sys_->xmotor_.set_focus_near(10);
				down_flag = 1;
				break;
			}
			else if (up_count >= 10) {
				up_flag = 1;
				break;
			}
		}
	}

sys_->sensor_frame_count_是在前台ISR中修改的，后台查询并使用这个共享变量。
这个变量被用作同步，即，如果没有被更新，则不执行代码块。如果被更新，则本地寄存变量跟随变化。

xaf->current_clarity_是在前台ISR中修改的，后台查询并使用这个共享变量。在前台ISR中，通过读取HLS模块中计算出的值，来更新xaf->current_clarity_。

使用一个for循环体来完成window shift moving，将新的clarity的值登记到窗口中，

更新max_clarity和min_clarity的值，

使用一个for循环体来完成统计，通过遍历window，统计窗口中出现的差分方向的次数。
窗口内的每个clarity都与oldest clarity进行比较，统计newer clarity的变化趋势。

获取motor mcu反馈的focus_motor_value_的值，用来判断下一步的运转方向，
自此，退出waitfor循环。

然后，
根据判断的运转方向，做出动作，并延时，以等待电机的迟滞。

if (down_flag) {
	sys_->xmotor_.set_focus_near(10);
}
usleep(1000 * 150);

然后，
使用一个for循环体来完成window init，

	for (int i = 0; i < max_steps_; i++)
	{
		clarity[i] = 0;
	}

然后，使用一个while(1)循环体实现waitfor作业流程，

	start_time = sys_->get_timestamp();
	while(1)
	{
		//超时判断
		u32 end_time = sys_->get_timestamp();
		int dlt_time = (end_time > start_time) ? (end_time - start_time) : (start_time - end_time);
		if (dlt_time > timeout) {
			//爬坡超时，停止
			xmotor->set_motor_stop();
			return -1;
		}

		if (sys_->sensor_frame_count_ != pre_frame_id) {
			pre_frame_id = sys_->sensor_frame_count_;

			//记录当前，最大，最小清晰度
			u32 current_clarity = current_clarity_;
			for (int i = max_steps_ - 1; i >= 1; i--) {
				clarity[i] = clarity[i - 1];
			}
			clarity[0] = current_clarity;

			if (current_clarity > max_clarity) {
				max_clarity = current_clarity;
			}
			if (current_clarity < min_clarity) {
				min_clarity = current_clarity;
			}

			//判断一个区间内清晰度是否是变小
			down_count = 0;
			for (int i = max_steps_ - 1; i >= 1; i--) {
				int diff = ABS_DEC(clarity[max_steps_ - 1], clarity[i - 1]);
				if ((clarity[max_steps_ - 1] != 0) && (diff > dx) && (clarity[max_steps_ - 1] > clarity[i - 1]))
				{
					down_count++;
				}
			}

			//回头
			if (down_count >= 10) {
				if (up_flag)
					sys_->xmotor_.set_focus_near(4);
				else
					sys_->xmotor_.set_focus_far(4);
				break;
			}
		}
	}

代码块和之前的代码块作用类似，

跳出循环体后，
判断是否统计出错，

	if (min_clarity >= max_clarity) {
		return -1;
	}

然后，
估算最佳clarity，

	u32 range = max_clarity - min_clarity;
	u32 best_clarity = range * 90 / 100 + min_clarity;

然后，向最佳位置运转，
使用一个while(1)循环体，完成waitfor作业流程，

	start_time = sys_->get_timestamp();
	while(1)
	{
		u32 end_time = sys_->get_timestamp();
		int dlt_time = (end_time > start_time) ? (end_time - start_time) : (start_time - end_time);
		if (dlt_time > timeout) {
			//搜索最佳位置超时，停止
			xmotor->set_motor_stop();
			return -1;
		}

		if (sys_->sensor_frame_count_ != pre_frame_id) {
			pre_frame_id = sys_->sensor_frame_count_;

			u32 current_clarity = current_clarity_;
			if (current_clarity > best_clarity) {
				xmotor->set_motor_stop();
				break;
			}
		}
	}

在电机运行中，检测clarity，如果发现clarity处于best限值之上，则发出指令，使电机停转。

最后，返回，

return 0;

Huskar_Liu

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
1
评论
从SOD到OOD（AutoFocus模块）

先来看看数据的封装class AutoFocus{ ...public: SystemManager* sys_; const int max_steps_ = 16; int af_min_focus_; int af_max_focus_; int af_init_focus_; int af_dx_; u32 current_clarity_; u8 af_doing_;};首先定义了索引类的成员变量，这里定义了一个system manager的索引变量，用于实现
复制链接

扫一扫

专栏目录