32 lines
888 B
C
Executable file
32 lines
888 B
C
Executable file
#include "distance_sensor.h"
|
|
|
|
//-----------------------------------------------------------------------------
|
|
float Distance_Sensor::GetDistance()
|
|
{
|
|
uint32 result = 0;
|
|
|
|
//Generate pulse
|
|
*hardwareDDR |= pin;//Set pin output
|
|
*hardwarePort |= pin;//Activate port
|
|
usleep(10);//Wait for 10ľs
|
|
*hardwarePort &= ~pin;//Deactivate port
|
|
*hardwareDDR &= ~pin;//Set pin input
|
|
|
|
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1);
|
|
|
|
//Wait for response
|
|
for(int i=0;(!(PINC & pin))&&(i < 1000);i++) {asm volatile("nop");}
|
|
|
|
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1);
|
|
|
|
//Calculate duration of response
|
|
while((*hardwarePin & pin)&&(result < 300000))
|
|
{
|
|
result++;
|
|
asm volatile("nop");
|
|
}
|
|
|
|
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1);
|
|
|
|
return (float(result) * DISTANCE_PER_VALUE);
|
|
}
|