blob: e6fd3f872ef5208197e63bb3df66a2cfc8763365 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
|
#include "distance_sensor.h"
//-----------------------------------------------------------------------------
float Distance_Sensor::GetDistance()
{
uint32 result = 0;
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Gen Pulse; Pin:",1,4);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
msleep(500);
//Generate pulse
*hardwareDDR |= pin;//Set pin output
*hardwarePort |= pin;//Activate port
usleep(10);//Wait for 10µs
*hardwareDDR &= ~pin;//Set pin input
*hardwarePort &= ~pin;//Deactivate port
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Wait response; Pin:",1,4);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
uint16 i;
//Wait for response
for(i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); }
//Calculate duration of response
while((*hardwarePin & pin)&&(result < 300000))
{
result++;
asm volatile("nop");
asm volatile("nop");
asm volatile("nop");
asm volatile("nop");
asm volatile("nop");
asm volatile("nop");
asm volatile("nop");
}
return (float(result) * DISTANCE_PER_VALUE);
}
|
