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
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

sensor.c
-----------------------
#if SONAR
 
#define TRIG 7
#define ECHO 4 // PD4
 
#define PCINT_RX_BIT               (1<<4)
#define PCIR_PORT_BIT              (1<<2)
 
volatile uint32_t sonarTrigTime = 0;
volatile boolean gotecho = false;
 
ISR(PCINT2_vect)
{
  // check for falling edge
  if (!(PIND & PCINT_RX_BIT))
  {
    sonarTrigTime = micros();
    gotecho = true;
  }
}
 
void init_sonar() 
{ 
  pinMode(TRIG, OUTPUT); 
  pinMode(ECHO, INPUT);
  PORTD  = PCINT_RX_BIT;
  PCMSK2 = PCINT_RX_BIT;
  PCICR  = PCIR_PORT_BIT;
}  
 
uint16_t distance = 0;
uint32_t sonarStartTime = 0;
 
void sonar() 
{ 
  distance = 0;
  if (gotecho)
  {
    gotecho = false;
    distance =  (sonarTrigTime - sonarStartTime) / 58;
  }
  // trig
  digitalWrite(TRIG, HIGH); //trig sonar high
  sonarStartTime = micros();
  digitalWrite(TRIG, LOW); //trig sonar low
}    
#endif
--------------------
 
multiwii.c
--------------------
  #if SONAR
  if (currentTime >= SONARTime)
  {
    SONARTime = currentTime + 100000;
    sonar();
    debug[0] = 10 * distance;
  }
#endif 
--------------------