http://www.goodluckbuy.com/ultrasonic-w ... ensor.html
I'm trying to integrate the DYP-ME007 sonar to multiwii for more precised alt hold (with altitude <5m) or for auto landing etc...
With simple example the sonar work perfect
Code: Select all
// variables to take x number of readings and then average them
// to remove the jitter/noise from the DYP-ME007 sonar readings
const int numOfReadings = 10; // number of readings to take/ items in the array
int readings[numOfReadings]; // stores the distance readings in an array
int arrayIndex = 0; // arrayIndex of the current item in the array
int total = 0; // stores the cumlative total
int averageDistance = 0; // stores the average value
// setup pins and variables for DYP-ME007 sonar device
int echoPin = 2; // DYP-ME007 echo pin (digital 2)
int initPin = 3; // DYP-ME007 trigger pin (digital 3)
unsigned long pulseTime = 0; // stores the pulse in Micro Seconds
unsigned long distance = 0; // variable for storing the distance (cm)
//setup
void setup() {
pinMode(initPin, OUTPUT); // set init pin 3 as output
pinMode(echoPin, INPUT); // set echo pin 2 as input
// create array loop to iterate over every item in the array
for (int thisReading = 0; thisReading < numOfReadings; thisReading++) {
readings[thisReading] = 0;
}
// initialize the serial port, lets you view the
// distances being pinged if connected to computer
Serial.begin(9600);
}
// execute
void loop() {
digitalWrite(initPin, HIGH); // send 10 microsecond pulse
delayMicroseconds(10); // wait 10 microseconds before turning off
digitalWrite(initPin, LOW); // stop sending the pulse
pulseTime = pulseIn(echoPin, HIGH); // Look for a return pulse, it should be high as the pulse goes low-high-low
distance = pulseTime/58; // Distance = pulse time / 58 to convert to cm.
total= total - readings[arrayIndex]; // subtract the last distance
readings[arrayIndex] = distance; // add distance reading to array
total= total + readings[arrayIndex]; // add the reading to the total
arrayIndex = arrayIndex + 1; // go to the next item in the array
// At the end of the array (10 items) then start again
if (arrayIndex >= numOfReadings) {
arrayIndex = 0;
}
averageDistance = total / numOfReadings; // calculate the average distance
Serial.println(averageDistance, DEC); // print out the average distance to the debugger
delay(100); // wait 100 milli seconds before looping again
}
But pulseIn() take a lot of time for one iteration because it wait for the response...
So we need the driver based on interrupts. I tried to get it from the megapirateng http://code.google.com/p/megapirateng/downloads/list
but haven't enough of knowledge in low level Arduino programming to run this.
Here is the sketch for mega1280. Could someone look at this and give some comments how to fix this driver?
Code: Select all
// setup pins and variables for DYP-ME007 sonar device
int echoPin = 10; // DYP-ME007 echo pin (digital 2)
int initPin = 9; // DYP-ME007 trigger pin (digital 3)
//setup
void setup() {
pinMode(initPin, OUTPUT); // set init pin 3 as output
pinMode(echoPin, INPUT); // set echo pin 2 as input
// initialize the serial port, lets you view the
// distances being pinged if connected to computer
Serial.begin(115200);
// Sonar INIT
//=======================
//D48 (PORTL.1) = sonar input
//D47 (PORTL.2) = sonar Tx (trigger)
//The smaller altitude then lower the cycle time
// 0.034 cm/micros
//PORTL&=B11111001;
//DDRL&=B11111101;
//DDRL|=B00000100;
PORTH&=B10111111; // H6 -d9 - sonar TX
DDRH |=B01000000;
PORTB&=B11101111; // B4 -d10 - sonar Echo
DDRB &=B11101111;
//PORTG|=B00000011; // buttons pullup
//div64 = 0.5 us/bit
//resolution =0.136cm
//full range =11m 33ms
// Using timer5, warning! Timer5 also share with RC PPM decoder
TCCR5A =0; //standard mode with overflow at A and OC B and C interrupts
TCCR5B = (1<<CS11); //Prescaler set to 8, resolution of 0.5us
TIMSK5=B00000111; // ints: overflow, capture, compareA
OCR5A=65510; // approx 10m limit, 33ms period
OCR5B=3000;
PCMSK0 = B00010000; // sonar port B4 - d10 echo
PCICR |= 1; // PCINT activated for PORTB
}
// Sonar read interrupts
volatile char sonar_meas=0;
volatile unsigned int sonar_data=0, sonar_data_start=0, pre_sonar_data=0; // Variables for calculating length of Echo impulse
ISR(TIMER5_COMPA_vect) // This event occurs when counter = 65510
{
if (sonar_meas == 0) // sonar_meas=1 if we not found Echo pulse, so skip this measurement
sonar_data = 0;
PORTH|=B01000000; // set Sonar TX pin to 1 and after ~12us set it to 0 (below) to start new measurement
}
ISR(TIMER5_OVF_vect) // Counter overflowed, 12us elapsed
{
PORTH&=B10111111; // set TX pin to 0, and wait for 1 on Echo pin (below)
sonar_meas=0; // Clean "Measurement finished" flag
}
ISR(PCINT0_vect)
{
if (PINB & B00010000) {
sonar_data_start = TCNT5; // We got 1 on Echo pin, remeber current counter value
}
else {
sonar_data=TCNT5-sonar_data_start; // We got 0 on Echo pin, calculate impulse length in counter ticks
sonar_meas=1; // Set "Measurement finished" flag
}
}
void loop() {
if ( (sonar_data < 354) && (pre_sonar_data > 0) ) { //wrong data from sonar (3cm * 118 = 354), use previous value
sonar_data=pre_sonar_data;
}
else {
pre_sonar_data=sonar_data;
}
unsigned int distance = (sonar_data / 118); // Magic conversion sonar_data to cm
Serial.println(distance, DEC); // print out the average distance to the debugger
delay(100); // wait 100 milli seconds before looping again
}
p.s. With MegaPirateNG_2.0.49_Beta4 it works but not with this separated sketch...
thx-
Alex