第四篇?Rico Board测量前方任意角度的距离

在上篇舵机的基础上，添加了超声波雷达测距模块，并融合到了舵机的驱动中。主要实现功能，通过舵机调整前方180度的任意方向，然后，读取该方向到障碍物的距离。先普及下雷达测距原理：

上图，为我的驱动编写提供了基础。
先罗列下我的前期调试过程，主要是在雷达反射波的中断处，关键代码如下：

正常调试过程的debug输出：

接下来就是完整的测距代码了。
代码如下：

1. /**
   
2. * [url=home.php?mod=space&uid=1455510]@file[/url]   motor.c
   
3. * [url=home.php?mod=space&uid=40524]@author[/url] yang yongsheng
   
4. * @date   2016.11.2
   
5. */
   
6. 
   
7. #include
   
8. #include
   
9. #include
   
10. #include                  // Required for the GPIO functions
    
11. #include
    
12. #include
    
13. #include
    
14. #include
    
15. #include
    
16. #include
    
17. #include
    
18. #include
    
19. #include
    
20. #include
    
21. 
    
22. #define NAME_LEN 16
    
23. 
    
24. /*gpio5_8*/
    
25. //#define MOTOR_GPIO_NUM 168
    
26. 
    
27. #define RADAR_TIME_SCALE 1000
    
28. 
    
29. /*gpio5_9*/
    
30. #define LED_GPIO_NUM 169
    
31. 
    
32. 
    
33. #define MOTOR_NUM 1
    
34. #define MOTOR_NAME "rico_lan motor"
    
35. 
    
36. #define MOTOR_MAJOR 0
    
37. static int motor_major = MOTOR_MAJOR;
    
38. static dev_t motor_dev_num;
    
39. int motor_gpio[MOTOR_NUM] = {113};
    
40. int trig_gpio[MOTOR_NUM] = {110};
    
41. int echo_gpio[MOTOR_NUM] = {111};
    
42. 
    
43. 
    
44. 
    
45. MODULE_LICENSE("GPL");
    
46. MODULE_AUTHOR("yang yongsheng ");
    
47. MODULE_VERSION("0.1");
    
48. 
    
49. struct motor_dev{
    
50.         struct cdev cdev;
    
51.         int num;//舵机引脚
    
52.         int trig_num;//雷达触发引脚
    
53.         int echo_num;//雷达接受引脚
    
54.         unsigned int radar_time;//雷达时间
    
55.         unsigned int pwm_value_us;//舵机高脉冲时间
    
56.         unsigned int echo_irq_num;
    
57.         struct hrtimer radar_hrt;
    
58.         struct hrtimer hrt;
    
59.         struct semaphore radar_sem;
    
60.         char name[NAME_LEN];
    
61.         bool pwm_state;
    
62.         bool hrt_state;
    
63.         bool radar_hrt_state;
    
64. } *motor_devp;
    
65. 
    
66. struct class *motor_class;
    
67. 
    
68. 
    
69. 
    
70. static enum hrtimer_restart motor_hrtimer_callback(struct hrtimer *hrt)
    
71. {
    
72.         struct motor_dev *devp_tmp;
    
73.         devp_tmp = container_of(hrt, struct motor_dev, hrt);
    
74.         devp_tmp->pwm_state = ！devp_tmp->pwm_state;
    
75.         gpio_set_value(devp_tmp->num, devp_tmp->pwm_state);
    
76.         if(devp_tmp->pwm_state == false)
    
77.         hrtimer_forward_now(hrt, ns_to_ktime((20000-devp_tmp->pwm_value_us)*1000));//T=20,000ns
    
78.         else if(devp_tmp->pwm_state == true)
    
79.         hrtimer_forward_now(hrt, ns_to_ktime((devp_tmp->pwm_value_us) * 1000));//T=20,000ns
    
80.         return HRTIMER_RESTART;
    
81. }
    
82. 
    
83. static enum hrtimer_restart radar_hrtimer_callback(struct hrtimer *hrt)
    
84. {
    
85.         struct motor_dev *devp_tmp;
    
86.         devp_tmp = container_of(hrt, struct motor_dev, radar_hrt);
    
87.         (devp_tmp->radar_time)++;
    
88.         hrtimer_forward_now(hrt, ns_to_ktime(RADAR_TIME_SCALE));
    
89.         return HRTIMER_RESTART;
    
90. }
    
91. 
    
92. irqreturn_t radar_echo_interrupt(int irq, void *dev_id)
    
93. {
    
94.         struct motor_dev *devp_tmp = (struct motor_dev *)dev_id;
    
95.         if (gpio_get_value(devp_tmp->echo_num))//上升沿触发               
    
96.         {
    
97.                 if(！down_trylock(&devp_tmp->radar_sem))
    
98.                 {
    
99.                 hrtimer_init(&devp_tmp->radar_hrt, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
    
100.                 devp_tmp->radar_hrt.function = radar_hrtimer_callback;
     
101.                 devp_tmp->radar_time = 0;
     
102.                 hrtimer_start(&devp_tmp->radar_hrt, ns_to_ktime(RADAR_TIME_SCALE), HRTIMER_MODE_REL);
     
103.                 devp_tmp->radar_hrt_state = false;
     
104.                 }
     
105.         }
     
106.         else  if (！gpio_get_value(devp_tmp->echo_num))//下降沿触发
     
107.         {       
     
108.                 devp_tmp->radar_hrt_state = true;
     
109.                 up(&devp_tmp->radar_sem);
     
110.                 hrtimer_cancel(&devp_tmp->radar_hrt);
     
111.         }
     
112.         return IRQ_HANDLED;
     
113. }
     
114. 
     
115. int motor_open (struct inode *inode, struct file *filp)
     
116. {
     
117.         struct motor_dev *devp_tmp;
     
118.         devp_tmp = container_of(inode->i_cdev, struct motor_dev, cdev);
     
119.         filp->private_data = devp_tmp;
     
120.         if(devp_tmp->hrt_state == false)
     
121.         {
     
122.                 hrtimer_init(&devp_tmp->hrt, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
     
123.                 devp_tmp->hrt.function = motor_hrtimer_callback;
     
124.                 gpio_set_value(devp_tmp->num, devp_tmp->pwm_state);
     
125.                 hrtimer_start(&devp_tmp->hrt, ns_to_ktime(devp_tmp->pwm_value_us), HRTIMER_MODE_REL);       
     
126.                 devp_tmp->hrt_state = true;
     
127.                 devp_tmp->radar_hrt_state = false;
     
128.         }
     
129.         sema_init(&devp_tmp->radar_sem, 1);
     
130.         printk(KERN_INFO "motor_open.\n");
     
131.         return 0;
     
132. }
     
133. 
     
134. int motor_release (struct inode *inode, struct file *filp)
     
135. {
     
136.         struct motor_dev *devp_tmp;
     
137.         devp_tmp = filp->private_data;
     
138.         printk(KERN_INFO "motor_release and pwm_value_us is %d.\n", devp_tmp->pwm_value_us);
     
139.         return 0;       
     
140. }
     
141. 
     
142. ssize_t motor_write (struct file *filp, const char __user *buf, size_t count, loff_t *ppos)
     
143. {
     
144.         char pwm_value_tmp[8];
     
145.         struct motor_dev *motor_devp_tmp = filp->private_data;
     
146.         copy_from_user(pwm_value_tmp, buf, sizeof(buf));       
     
147.         gpio_set_value(LED_GPIO_NUM        , motor_devp_tmp->pwm_state);       
     
148.         motor_devp_tmp->pwm_value_us = simple_strtoul(pwm_value_tmp, NULL, 10);
     
149.         if(motor_devp_tmp->pwm_value_us > 2500)
     
150.                 motor_devp_tmp->pwm_value_us = 2500;
     
151.         else if(motor_devp_tmp->pwm_value_us pwm_value_us = 500;
     
152.         printk(KERN_INFO "pwm_value is %d.\n", motor_devp_tmp->pwm_value_us);
     
153.         printk(KERN_INFO "motor_write.\n");
     
154.         return count;
     
155. }
     
156. 
     
157. ssize_t radar_read (struct file *filp, char __user *buf, size_t count, loff_t *ppos)
     
158. {
     
159.         struct motor_dev *motor_devp_tmp = filp->private_data;
     
160.         gpio_set_value(motor_devp_tmp->trig_num, false);
     
161.         udelay(10);
     
162.         gpio_set_value(motor_devp_tmp->trig_num, true);
     
163.         udelay(100);
     
164.         gpio_set_value(motor_devp_tmp->trig_num, false);
     
165.         mdelay(10);
     
166.         down(&motor_devp_tmp->radar_sem);
     
167.         if(motor_devp_tmp->radar_hrt_state == true)
     
168.                 printk(KERN_INFO "radar_read ente radar_timer is %dus and radar_len is %dmm.\n", motor_devp_tmp->radar_time, 34*(motor_devp_tmp->radar_time)/100);
     
169.         up(&motor_devp_tmp->radar_sem);
     
170.         return 0;
     
171. }
     
172. 
     
173. 
     
174. static struct file_operations motor_fops =
     
175. {
     
176.         .owner = THIS_MODULE,
     
177.         .open = motor_open,
     
178.         .release = motor_release,
     
179.         .write = motor_write,
     
180.         .read = radar_read,
     
181. };
     
182. 
     
183. int motor_setup(struct motor_dev *devp, int min, int num, int trig, int echo)
     
184. {
     
185.         int ret = 0;
     
186.         sprintf(devp->name, "motor%d", min);       
     
187.         if(！gpio_is_valid(num))
     
188.         {
     
189.                 printk(KERN_INFO "invalid motor%d_gpio_num:%d.\n",min, num);
     
190.                 goto fail_num;
     
191.         }
     
192.         else
     
193.         {
     
194.         gpio_request(num, "sysfs");
     
195.         gpio_direction_output(num, false);
     
196.         gpio_export(num, false);        /*cause GPIO_NUM to appear in /sys/class/gpio*/
     
197.         }
     
198.         if(！gpio_is_valid(trig))
     
199.         {
     
200.                 printk(KERN_INFO "invalid motor%d_gpio_trig:%d.\n",min, trig);
     
201.                 goto fail_trig;
     
202.         }
     
203.         else
     
204.         {
     
205.         gpio_request(trig,"sysfs");
     
206.         gpio_direction_output(trig, false);
     
207.         gpio_export(trig, false);
     
208.         }
     
209.         if(！gpio_is_valid(echo))
     
210.         {
     
211.                 printk(KERN_INFO "invalid motor%d_gpio_trig:%d.\n",min, trig);
     
212.                 goto fail_echo;
     
213.         }
     
214.         else
     
215.         {
     
216.         gpio_request(echo,"sysfs");
     
217.         gpio_direction_input(echo);
     
218.         gpio_export(echo, false);
     
219.         }
     
220.         devp->num = num;
     
221.         devp->trig_num = trig;
     
222.         devp->echo_num = echo;
     
223.         devp->pwm_value_us = 1500;//500~2500 us
     
224.         devp->pwm_state = true;
     
225.         devp->hrt_state = false;
     
226.         devp->radar_hrt_state = false;
     
227.         devp->echo_irq_num = gpio_to_irq(echo);
     
228.         request_irq(devp->echo_irq_num, radar_echo_interrupt, IRQF_TRIGGER_RISING|IRQF_TRIGGER_FALLING, "radar_echo_handler", devp);
     
229.         devp->cdev.owner = THIS_MODULE;
     
230.         cdev_init(&devp->cdev, &motor_fops);
     
231.         ret = cdev_add(&devp->cdev, MKDEV(motor_major, min), 1);
     
232.         if(ret)
     
233.         {
     
234.                 printk(KERN_INFO "error %d adding motor%d.\n", ret, min);
     
235.         }
     
236.         else
     
237.         {
     
238.                 device_create(motor_class, NULL, MKDEV(motor_major, min), NULL,devp->name);
     
239.         }
     
240.         return 0;
     
241. fail_echo:
     
242.         gpio_unexport(trig);
     
243. fail_trig:
     
244.         gpio_unexport(num);
     
245. fail_num:
     
246.         return -ENODEV;
     
247. }
     
248. 
     
249. int __init motor_init(void)
     
250. {
     
251.         int ret,i;
     
252.         printk(KERN_INFO "motor_init begin.\n");
     
253.         if(！gpio_is_valid(LED_GPIO_NUM))
     
254.         {
     
255.                 printk(KERN_INFO "invalid LED_GPIO_NUM:%d.\n",LED_GPIO_NUM);
     
256.                 return -ENODEV;
     
257.         }
     
258. 
     
259.         motor_devp = kzalloc(sizeof(struct motor_dev) * MOTOR_NUM, GFP_KERNEL);
     
260.         if(IS_ERR(motor_devp))
     
261.         {
     
262.                 printk(KERN_INFO "no space for motor_dev.\n");
     
263.                 return -ENOMEM;
     
264.         }
     
265. 
     
266.         motor_class = class_create(THIS_MODULE, MOTOR_NAME);
     
267. 
     
268.         if(motor_major == 0)
     
269.         {
     
270.                 ret = alloc_chrdev_region(&motor_dev_num, 0, MOTOR_NUM, MOTOR_NAME);
     
271.                 motor_major = MAJOR(motor_dev_num);
     
272.         }
     
273.         else
     
274.         {
     
275.                 motor_dev_num = MKDEV(motor_major, 0);
     
276.                 ret = register_chrdev_region(motor_dev_num, MOTOR_NUM, MOTOR_NAME);
     
277.         }
     
278.         if(ret hrt);
     
279.                 gpio_unexport((motor_devp+i)->num);
     
280.                 gpio_free((motor_devp+i)->num);
     
281.                 gpio_unexport((motor_devp+i)->trig_num);
     
282.                 gpio_free((motor_devp+i)->trig_num);
     
283.                 gpio_unexport((motor_devp+i)->echo_num);
     
284.                 gpio_free((motor_devp+i)->echo_num);
     
285.                 free_irq((motor_devp+i)->echo_irq_num, (motor_devp+i));
     
286.                 device_destroy(motor_class,MKDEV(motor_major,i));
     
287.                 cdev_del(&(motor_devp+i)->cdev);
     
288.                 unregister_chrdev_region(MKDEV(motor_major, i), 1);
     
289.                 kfree(motor_devp+i);
     
290.         }
     
291. 
     
292.        
     
293.         gpio_unexport(LED_GPIO_NUM);
     
294.         gpio_free(LED_GPIO_NUM);
     
295.         class_destroy(motor_class);       
     
296.        
     
297.         printk(KERN_INFO "motor_exit finished.\n");
     
298. }
     
299. 
     
300. module_init(motor_init);
     
301. module_exit(motor_exit);
     

复制代码

我选取了三个方向，现场图示如下：

它们依次对应的脉宽长度为1500us、1000us、2000us。
debug口的调试信息如下：

舵机加雷达的驱动调试告一段落，代码中关键的部分是我引入信号量来告知距离信息可用。