41 lines
1.2 KiB
C
Executable file
41 lines
1.2 KiB
C
Executable file
#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);
|
|
}
|