search

MegaPirateNG / ardupilot-mpng

MegaPirateNG
GNU General Public License v3.0
117 stars 105 forks source link

Sonar Support #32

Open mfc123 opened 10 years ago

mfc123 commented 10 years ago

I have the HC-SR04 ultrasonic module on the crius AIO v2 and I'm trying to mess with your code to try make it work but no luck :/ On your 2.9.1b addons it works good but I have the mottor twitch bug :/ today throttle was at 0% and when disconnecting the battery it ripped the flesh out of my fingers :( now back to 2.8r3... Can you help please?

evil-m commented 8 years ago

I was thinking about this right now. There's any good info about how the HAL layer works? I've already took a look at some parts of the code and got the idea of how it works. I could just write a sketch that handles the hc-sr04, burn on the attiny or similar, and then output the range value. What you think should be the best way to output this value, PWM, digital, analog? I mean what is the less hacky to work with the HAL layer? Is the support for analog sonars removed from the mpgn 3.x versions? The arduplilot 3.1 supports the mb1200 analog sonar series, I could emulate the m1200 analog output or buy one. The mpgn supports this sonar? I'll take a look to these mb1200 datasheet later to see how it works. Thanks.

smurfy commented 8 years ago

about HAL, no sorry. For output i would suggest analog voltage, it should be the easiest to READ on the mpng side. Reading PWM probably works too but i think it takes more time to do it.

And now again the problem, attiny (and most atmels) don't have a D-A only A-D converters. So the tiny cant output an analog voltage.

Thats why i choose to use i2c :) I found some documentation about creating a simple low pass filter to convert PWM to analog voltage You could try that or PWM. With analog you maybe even can use the AP_RangeFinder_analog as it is.

evil-m commented 8 years ago

Thanks again! I think the same about pwm. As for the analog output on the attiny, I'm aware of that, I was thinking about something like this you shared. I'll try later and share my results here for anyone interested on this approach. :D

evil-m commented 8 years ago

Hey @smurfy , I got the basic idea working on mpgn 3.1.5-R2. Just need to tweak a little the code on attiny or on the mpgn to get more accurate results. Latter when I came back home I'll share the info here. :)

evil-m commented 8 years ago

As I said earlier, I made it work here, but there are some tuning needed to get good values, altough I have one doubt: When I go to the terminal->tests->sonar I got the values from the sensor, however I'm not able to select the sonar type or enable through mission planner under initial configuration->optional hardware->sonar, also in this same menu I see no data values for Distance and Voltage and the same thing under flight data->status->sonarrange & sonarvoltage. Those 2 screens display the same info, I know, if doesn't show in one it would not show on the other. I can only activate the sonar through terminal->setup->sonar or by changing the parameters under configuring/tunning->full parameter list->sonar. My doubt is if this data are not shown because of some parts of the code were removed to save space but the sonar will work anyway once I can get the values on terminal-test->sonar, or if I need to look for something else to change in the mpgn source to get the sonar working. By what I saw on the code, the sonar variables are still in the source and all the methods are there, so it's probably working even if it's not showing values on the gui and only showing through terminal. What you think? I'll test this soon as I fix one burned esc.

Thanks.

evil-m commented 8 years ago

Hey, @smurfy, I've ported some parts of the RangeFinder code from the arducopter 3.2 source to the mpgn 3.1.5-R4 source. Now the sonar is configurable via mission planner with the new RNGFND parameters and shows accurate sonar data for the hc-sr04 on the gui and through the mavlink. I've did some lab testing to find the best value to scale volts in meters and also did some measurements with a ruller to see if the value was accurate enough. Still I haven't tested the stability of the changes during flight, and I'll not be able to do that this month because I'm waiting for some parts to arrive. Altough I think that the sonar must work fine on the mpgn 3.1.5-R4 without any modifications onde the code. The main reason for this changes was be able to see sonar data through mavlink. With my changes maybe the other sonars can work as well, but I don't own none of them, so for now I can't test it. I'll test the stability of my version and @SirAlex version as soon as I can. At least for the hc-sr04 with the attiny85 handling the interrupts and sending an analog voltage, through the method we discussed earlier in this post, everything seems to work fine. If there's anyone interested in the code changes for the mpgn source or in code and schematic to connect the hc-sr04 to the RCTimer Crius AIOP V2, apm 2.x, or similar, feel free to contact me. Ps: Any analog sonar shoud work, the only thing you need is find the correct value to scale volts/meters. To anyone reading this, as said earlier if you intend to use the hc-sr04 you need an attiny or similar to drive this sonar. Cya!

JonathancalderonIL commented 8 years ago

Hi @evil-m I have recently ordered this sonar http://www.aliexpress.com/item/Ultrasonic-Sonar-module-dedicated-for-APM-2-5-2-6-2-8-flight-controller-plug-and/32413098417.html It is a hc-sr04 sonar with an additional board soldered to it that makes it compatible with APM I haven't had time to try it yet and I will try to find some in the next 2 weeks The price was OK so if it works its a better way of getting it done,

I will post results as soon as I test it PS my quad went from home made frame to a arduphantom configuration.

evil-m commented 8 years ago

Hey @JonathancalderonIL, I think, by what I saw in the pictures of the product, that it uses the same idea we discussed earlier in this post, that is about using a microcontroller to handle the hc-sr04. It should work fine. I gave a try to the @smurfy 's code and got the same results you said, with the difference that you used the apm hardware and I got the RCTimer Crius V2 with the megapirate firmware. Are you still using the apm 2.x flight controller with the official arducopter software our using other board with the megapirateng? The making of the circuit and the implementation of the software to connect the hc-sr04 to the apm and variants is very cheap an easy. Instead of converting to i2c like @smurfy did, I used a method to output an analog signal from the attiny, so in the mission planner I can configure it as an analog sonar. A think it will be almost the same configuration for this sensor you bought.

bektorkhan commented 8 years ago

I thought I should not get involved... I have a working module in my drawers somewhere with analog and I2C output. The analog is an average on 8 samples in a FiFo, works nice. Not flown with though! The thing is, Sonar is inaccurate by physics: 1 The angle of beam is to be fixed for a good reading. (CamStab on Sonar?!) 2 The surface has be smooth and hard for sound to bounce nicely HC-SR04 is a cheap one.. so readings from 15 - 240 cm can be good in above circumstances. So an analog Voltage 0.15 - 2.56 V 1 - 2 cm resolution is accurate enough!!

The I2C bus is also swamped with traffic from Gyro/Acc, Mag, Baro readings. I would not use it!

The code should be outhere somewhere!!

bektorkhan commented 8 years ago

Sorry can't find the code.. but PB1 is free and should be pwm capable.. anyone object?! And I think there was some special trick to get it to run on 16MHz it defaults to 8 MHz in Arduino I think!! Filter it with a RC with 47k ohm and 100nF should be adequate.

evil-m commented 8 years ago

Hey @bektorkhan, for the hc-sr04 with the attiny85 connected to the A0 (analog 0) on the RCTimer Crius V2 I got almost the same results you said. For me is accurate enough, once I didn't pretend to use it to fly indoors but just want the sonar to get another source of information on landing, besides the gps and baro. I've done the circuit and implemented the code on the attiny85 to make it possible to work with apm and variants. I connected the pwm output, PB0 of the attiny85 running on 8mhz internal clock, to the analog0 port of the CRIUS board through an R/C filter where the resistor is 10kΩ and the capacitor is 1µf. It works flawlessly, although I wasn't been able to test it yet during flight. I'll take some pictures and do some schematics later, it's very easy to implement but needs to be tested to see it's reliability.

bektorkhan commented 8 years ago

Found the code in a laptop.. 5Hz and 4 times oversampling. I have test compiled it.. but I attach the file also, if it works:

/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with this program. If not, see http://www.gnu.org/licenses/. */

/*

include

include

define I2C_SLAVE_ADDR 0x70

define trigPin 3

define echoPin 4

define pwmPin 1

define sampleFreq 5 // Frequency of sampling Hz Keep Freq * Samples under 25

define overSample 4 // Average over X samples

define pauseTime 20 // Pause time in ms between samples, for echoes to die out!!

define maxDistance 240 // Keep <= 255

define minDistance 10

define maxPWMScale 330

define maxPWM 175 // Set to 175 for max 3.3V output @ 5V Vcc

define sonarScale 29.1

volatile unsigned long timer_start; volatile unsigned int pulse_time; boolean sendSecondByte = false; unsigned int Distance = 0; // byte MSB_Dist = 0; byte LSB_Dist = 0; unsigned long sampleTime = 0; unsigned int sampleArray[overSample]; byte sampleIdx = 0;

void handleSonar() { if(digitalRead(echoPin) == HIGH) { timer_start = micros(); } else { if(timer_start > 0) { pulse_time = ((volatile int)micros() - timer_start); timer_start = 0; } } }

void triggerSonar() { digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); }

unsigned int sonarCalcDist() { unsigned int distance = (pulse_time/2) / sonarScale; if (distance >= maxDistance || distance <= minDistance){ return(0); } else { return((unsigned int) (distance & 0x1ff)); } }

void setup() { pulse_time = 17410; pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); pinMode(pwmPin, OUTPUT); // PWM analog out by Bektorkhan attachPcInterrupt(echoPin, handleSonar, CHANGE); TinyWireS.begin(I2C_SLAVE_ADDR); triggerSonar(); }

void loop() { byte i = 0; if (sendSecondByte) { TinyWireS.send(LSB_Dist); sendSecondByte = false; } else { byte received = 0; if (TinyWireS.available())
{
received = TinyWireS.receive(); 

  if (received == 0x51)
  {
    LSB_Dist = (byte) (Distance & 0xFF);
    // MSB_Dist = (byte) (Distance >> 8) & 0x01;
    TinyWireS.send(0); 
    sendSecondByte = true;
  }
}

} if (micros() > (sampleTime + ( ( unsigned long) ( 1e6 / ((unsigned int) (sampleFreq * overSample))) - (pauseTime * 1000))) + (pauseTime * 1000)) { if (timer_start == 0) { // Sonar is ready sampleTime = (unsigned long) micros(); sampleArray[sampleIdx] = sonarCalcDist(); sampleIdx += 1; if ( sampleIdx >= overSample ) sampleIdx = 0; // Average samples unsigned int avgDistance = 0; for ( i=0; i < overSample ; i++) { avgDistance += sampleArray[i]; } Distance = (unsigned int) (avgDistance / overSample) & 0x1ff; analogWrite(pwmPin,map(Distance, 0, maxPWMScale, 0, maxPWM)); // PWM analog out 1V/m by Bektorkhan triggerSonar(); } } // Add fix for micros() overflow later }

evil-m commented 8 years ago

Cool, you added analog ouput to the @smurfy 's code. I'll try this later to see what I get. I've been reading around 10 to 15 samples for a more accurate value in the code on the attiny85. The only thing about your code is that for the output on most atmel chips you will need an R/C filter like this: http://provideyourown.com/2011/analogwrite-convert-pwm-to-voltage/ Besides that, it should work fine on the apm2.x or on the megapirateng with some modifications on the source code for the later versions(3.x). Cya!

bektorkhan commented 8 years ago

Well zip files did not go.. so rename the file below .ino and put it in a directory with its same name, compile for Tiny85@8MHz internal (Need to get Tiny installed). I think I did last year.. on Arduino 1.05r2, correct me if I am wrong! It produced a hex file on the laptop anyway. I have not done enough testing, being from Hi-Rel systemswork.. but looks to work fine! Good Luck!

hcsr04_i2c_attiny85_analog.ino.txt

bektorkhan commented 8 years ago

@evil-m RC filtering. Yes thats how DA works on Arduino, 47k and 100nF would work good enough. Good point though.

evil-m commented 8 years ago

@bektorkhan, nice. I'll take a look at your code later. Cya!

smurfy commented 8 years ago

btw. i'm happy to host the analog or other variants of the sketch in my repo. for example via pull request. so everything is in one place.

bektorkhan commented 8 years ago

@smurfy Please do host it.. do you make a branch or do we include it ( not that into github/svn)? I vote for a branch.. don't change anything that works!! Let me know.

smurfy commented 8 years ago

I will put on my repository: https://github.com/smurfy/hcsr04_i2c_attiny85 Will rename the repo and add the analog code.

Can you or evil-m create a pull request with the code. if possible add a diagram for the DA as-well (like i did for the pull-ups with i2c)

If the code and the used components are the same it makes sense to post the apm parameters as-well.

dodothesaumure commented 7 years ago

Hello,

Sorry to revive this post but I'm considering installing a sonar on my Crius AOIP 2 running mpng and I can't find an answer to the following question : is it compatible with MaxSonar sensor?

Thanks