Attemp to integrate sonar (ultrasonic sensor)

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Olaf.J » Sun Apr 08, 2012 4:34 pm

Hi zjojos and noobee,

thanks for your info about the D-parameter - I successfully implemented it and found suitable parameters for PID. When hoovering, the height is hold pretty good, if I move the copter, it first goes down 50-80 cm and then goes up to the target-height slowly. When stopping, it first goes up 50-80 cm and then back to the target-height. I think, the main reason for this is the weight of the copter - the total flight-weight is 1435 g. So everything looks fine at the moment :)
Olaf.J
 
Posts: 16
Joined: Tue Jul 12, 2011 10:10 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby sandmen » Mon Apr 09, 2012 8:02 am

@Olaf,
do you want to share your implementation?
Thanks
Peter
sandmen
 
Posts: 24
Joined: Mon Feb 21, 2011 3:26 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Olaf.J » Mon Apr 09, 2012 12:53 pm

sandmen wrote:@Olaf,
do you want to share your implementation?
Thanks
Peter


Hi, it's not yet ready (it's sonar-mode only without mixing baro at the moment and I use a fix height of 180 cm at the moment, instead of height at activating), but if you want to test it already, here's the modified code for flyduino (atmega2560):

def.h - the pins should be changed to pin 9 for trigger and pin 10 for echo for compatibility-reasons (flyduino connection diagram from 13/6/11), but I haven't implemented that yet.
Code: Select all

/* Pin to interrupt map:
* PCINT 16-23 = PCIE2 = pcmsk2 = PCINT2_vect
* PCINT 0-7 = PCIE0 = pcmsk0 = PCINT0_vect
* PCINT 8-15 = PCIE1 = pcmsk1 = PCINT1_vect
*/

#if defined MEGA
#if defined(HCSR04)
  #define SONAR 1
  #define HCSR04_TriggerPin 12 // should be modified to 9 in next version
  #define HCSR04_TriggerPin_PINMODE_OUT            pinMode(HCSR04_TriggerPin,OUTPUT);
  #define HCSR04_TriggerPin_PIN_HIGH           PORTB |= 1<<6;  // should be modified for pin 9 in next version
  #define HCSR04_TriggerPin_PIN_LOW            PORTB &= ~(1<<6);   // should be modified for pin 9 in next version
  #define HCSR04_EchoPin 11     // should be modified to 10 in next version
  #define HCSR04_EchoPin_PINMODE_IN             pinMode(HCSR04_EchoPin,INPUT);
  #define HCSR04_EchoPin_PCINT  PCINT5     // should be modified for pin 10 in next version
  #define HCSR04_EchoPin_PCICR   PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
  #define HCSR04_EchoPin_PCMSK   PCMSK0 = (1<<HCSR04_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
  #define HCSR04_EchoPin_PCINT_vect    PCINT0_vect  // PCINT0-7 belog PCINT0_vect
  #define HCSR04_EchoPin_PIN  PINB  // PCINT0-7 belong to PINB
#else
  #define SONAR 0
#endif
#else
  #define SONAR 0
#endif


config.h
Code: Select all
#define HCSR04



sensors
Code: Select all
// ************************************************************************************************************
// HC-SR04 Ultrasonic Sonar
// ************************************************************************************************************

#if defined(HCSR04)

volatile unsigned long HCSR04_startTime = 0;
volatile unsigned long HCSR04_echoTime = 0;
volatile unsigned long SonarSuccess = 0;
volatile unsigned long SonarFail = 0;
unsigned int HCSR04_current_loops = 0;
volatile static int32_t  tempSonarAlt=0;

// The cycle time is between 3000 and 6000 microseconds
// The recommend cycle period for sonar request should be no less than 40ms -> 40000 microseconds
// A reading every 13 loops (40000 / 3000 aprox)
unsigned int HCSR04_loops = 13;

void Sonar_init()
{
  HCSR04_EchoPin_PCICR;
  HCSR04_EchoPin_PCMSK;
  HCSR04_EchoPin_PINMODE_IN;
  HCSR04_TriggerPin_PINMODE_OUT;
  Sonar_update();
}

// EchoPin will change to high signalize beginning
// and back to low after 58*cm us
// First interrupt is needed to start measurement, second interrupt to calculate the distance


ISR(HCSR04_EchoPin_PCINT_vect) {
  //
  // Here is a routine missing, to check, if the interrupt was raised for echo pin - not needed at the moment, because we don't have any interrupts
  // for this interrupt group, but maybe later
  //
  if (HCSR04_EchoPin_PIN & (1<<HCSR04_EchoPin_PCINT)) {            //indicates if the EchoPin is at a high state
    HCSR04_startTime = micros();
  }
  else {
    HCSR04_echoTime = micros() - HCSR04_startTime;
    if (HCSR04_echoTime <= 25000)                                  // maximum = 4,31 meter - 30000 us means out of range
      tempSonarAlt = HCSR04_echoTime / 58;
    else
      tempSonarAlt = 9999;
  }
}

void Sonar_update()
{
  HCSR04_current_loops++;
  if ((HCSR04_current_loops >= HCSR04_loops) && baroMode) {
    HCSR04_current_loops = 0;
    LastSonarAlt = SonarAlt;
    SonarAlt = tempSonarAlt;
    debug2 = SonarAlt;
    if (SonarAlt < 500) {
      SonarSuccess += 1;  // count successful readings of height
      debug3 = SonarSuccess;
    } else {
      SonarAlt = 9999;
      SonarFail += 1;  // count failed readings of heigt
      debug4 = SonarFail;
    }

    // create a trigger pulse for 10 us
    HCSR04_TriggerPin_PIN_LOW;
    delayMicroseconds(2);
    HCSR04_TriggerPin_PIN_HIGH;
    delayMicroseconds(10);
    HCSR04_TriggerPin_PIN_LOW;
  }
}

#endif



imu
Code: Select all
void getEstimatedAltitude(){
  uint8_t index;
  static uint32_t deadLine = INIT_DELAY;

  static int16_t BaroHistTab[BARO_TAB_SIZE];
  static int8_t BaroHistIdx;
  static int32_t BaroHigh,BaroLow;
  int32_t temp32;
  int16_t last;
 
  if (currentTime < deadLine) return;
  deadLine = currentTime + UPDATE_INTERVAL;

  //**** Alt. Set Point stabilization PID ****
  //calculate speed for D calculation
  last = BaroHistTab[BaroHistIdx];  // letzter Wert aus BaroHistTab
  BaroHistTab[BaroHistIdx] = BaroAlt/10;  // durch 10 teilen
  BaroHigh += BaroHistTab[BaroHistIdx];
  index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
  BaroHigh -= BaroHistTab[index];
  BaroLow  += BaroHistTab[index];
  BaroLow  -= last;

  BaroHistIdx++;
  if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;

  BaroPID = 0;

  //D
  #ifdef SONAR
    temp32 = D8[PIDALT]*(SonarAlt-LastSonarAlt) / 40; // example climbed from 150 to 170 cm, D=7 -> temp32=7*(170-150)/40=3,5 -> BaroPID = -3,5
  #else
    temp32 = D8[PIDALT]*(BaroHigh - BaroLow) / 40;
  #endif
  BaroPID-=temp32;

  EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);

  // Olaf
  // Use sonar to hold at 180 cm instead of last height
 
  #ifdef SONAR
    if (SonarAlt < 500)
      temp32 = 180 - SonarAlt;
    else
      temp32 = 0;  // if no valid result from SONAR -> no P-Value!
  #else
    temp32 = AltHold - EstAlt;
    if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0;  //remove small differences to reduce noise near zero position
  #endif

 
  //P
  BaroPID += P8[PIDALT]*constrain(temp32,(-2)*P8[PIDALT],2*P8[PIDALT])/100;  // temp32 auf should be for example in range +- 32 for P=16 (= 1.6 in GUI)   
  // example: climbed from 150 cm to 170 cm, target height: 180 cm, P=40
  // BaroPID += 40*(180-170)/100 = increase BaroPID for 4,0
 
  BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150

  //I
  errorAltitudeI += temp32*I8[PIDALT]/50; //example: height is 170 cm, target heigth 180 cm, I = 15 (0.015 in GUI) -> 10*15/50 = increase for 3
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  temp32 = errorAltitudeI / 500; //I in range +/-60 // this is a long term PID - will for example compensate decreasing battery voltage
  BaroPID+=temp32;
}


MultiWii_2_0
Code: Select all
static int32_t  SonarAlt;
static int32_t  LastSonarAlt;

[...]

      case 1:
        taskOrder++;
        #if BARO
          Baro_update(); 
          #if defined(SONAR)
          Sonar_update(); 
          #endif
          break;
        #endif



I began with the standard values with not really good success. So I increased the D-value to 20, this made it much more stable. Then I increase P still it began to oscillate (slowly) and then decreased it (I think it's 3 at the moment). I increased the I-value slightly from 0.015 to 0.020, otherwise the copter came down sloswly, when the battery voltage decreased.

If the code is ready incl. baro-mixing, I will share the code again of course, if it's of interest for anybody. I'm not sure at the moment, if the baro-PID-values and the sonar-PID-values can be the same or if we need different PIDs for baro and sonar, because baro has (at least at my copter) much more noise.
Olaf.J
 
Posts: 16
Joined: Tue Jul 12, 2011 10:10 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby nicodh » Mon Apr 09, 2012 6:59 pm

Hi Olaf,
About baro mixing, i remember in arducopter, they use a condition to use the baro or the sonar. Because the sonar is usable only if distance to obstacle is less than (let's say 6 meters) you can use the sonar reading in the loops when you are at a maximum of that altitude, other wise you use the baro.
nicodh
 
Posts: 44
Joined: Sun Apr 08, 2012 1:42 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby copterrichie » Mon Apr 23, 2012 6:32 pm

If anyone is interested, $2.80US

Ultrasonic Module HC-SR04

http://cgi.ebay.com/ws/eBayISAPI.dll?Vi ... SS:US:1123
copterrichie
 
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby bverissimo » Sat Apr 28, 2012 3:29 am

adam wrote:
Vallekano wrote:I use an Arduino ONE that has an output of 3.3V. I think the Arduino Pro Mini (5V version) also has an output of 3.3V that can be used instead of pin 12.

If you do not have that output you will have to use a LM117 to pass the 5V to 3.3V, as illustrated in the diagrams.


Hi Vallekano ,

Thanks for your help.

I follow your steps, Now can get altitude reading from DEBUG3, How can I get altitude hold ? Next for autolanding...

My QuadX , wmp + HCSR04, No Baro.





Hi , how did you manage to conect the HC-Sr04 on the arduino pro mini ..wich pins you use ?
And please can o send me yyour code try to edit mine but nothing ..many thanks in advance ..
bverissimo
 
Posts: 1
Joined: Sat Apr 28, 2012 3:21 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby didlawowo69 » Wed May 23, 2012 11:57 am

no one have a video of demo ?
didlawowo69
 
Posts: 38
Joined: Tue Oct 11, 2011 1:42 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Dehas » Sat Jun 02, 2012 7:34 am

@ penpen77: I try to use your code and i have this error "smartDigitalWrite' was not declared in this scope" in arduino 1.0.
Dehas
 
Posts: 14
Joined: Sun Aug 14, 2011 8:19 am
Location: France Les Vosges

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Katch » Mon Jun 04, 2012 9:22 pm

Any updates with this?

Seeing a lot of other dev using i2c/secondary MCU set ups but would like to try my HC-SR04 on my 2560 Megapirate board. It already has a header for sonar so I'll trace the pins and work out what they are connected to and see about applying this code.

If it follows Megapirate_ng code specs it should be pins 9 and 10
Katch
 
Posts: 280
Joined: Thu Aug 04, 2011 1:44 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby copterrichie » Tue Jun 05, 2012 1:16 am

I really want to give this a try, any updates?
copterrichie
 
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby copterrichie » Tue Jun 05, 2012 1:21 am

Dehas wrote:@ penpen77: I try to use your code and i have this error "smartDigitalWrite' was not declared in this scope" in arduino 1.0.


If my memory serves me right, the smartDigitalwrite is a special library but I believe you can use digitalWrite().

viewtopic.php?f=7&t=1033&start=20
copterrichie
 
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Lapino » Fri Jun 08, 2012 9:50 am

push :)
any updates regarding implementation of HC-SR04 sonar?
Lapino
 
Posts: 84
Joined: Tue Aug 16, 2011 10:01 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Fri Jun 08, 2012 5:36 pm

smartDigitalWrite is just digitalwrite without all check/test, comment unnecessary lines, that's all. You can use digitalWrite, it's just mater of binary size.

Lapino, once you have an i2c wrapper for hcsr04, the implementation is done, just use code of other i2c sonar.
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Lapino » Sat Jun 09, 2012 10:32 am

So I've to build a ATTINY4313 Sonar device or how can I realize an i2c wrapper? Must be some kind of hardware am I right?
Lapino
 
Posts: 84
Joined: Tue Aug 16, 2011 10:01 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Cronalex » Fri Jun 15, 2012 4:51 pm

I would use this sonar Maxbotix LV-EZ0 you can?
Cronalex
 
Posts: 51
Joined: Tue Mar 20, 2012 8:41 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Tommie » Fri Jun 15, 2012 5:07 pm

Lapino wrote:So I've to build a ATTINY4313 Sonar device or how can I realize an i2c wrapper? Must be some kind of hardware am I right?

Right. And it's not that difficult: viewtopic.php?f=8&t=1549&p=16225#p16225
Tommie
 
Posts: 438
Joined: Sun Apr 08, 2012 9:50 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby wilco1967 » Sun Jun 17, 2012 1:49 pm

Just managed to get the HCSR04 working using the TinyGPS route in altitude hold mode.
I'm using the modified BARO PID code derived from Olaf.J 's solution posted earlier in this tread....

I initially wanted to use my Mega, and directly wire the sonar to the Flyduino, but I could not get Olaf's code compiled, as it seems to interfere with the recently added TinyGPS code in dev20120606. And I'm only starting to figure out how to program the Arduino, so not good enough to figure it out myself (yet)...
I assume Olaf used the bare 2.0 code (or even earlier) for the HCSR04 to Mega connection, and I'm using dev20120606.

With the TinyGPS route, the Sonar is working,, and I used the basis of Olaf Baro code (the getEstimatedAltitude part that goes in IMU) to get altitude hold....
Code: Select all
//wilco
void getEstimatedAltitude(){
  uint8_t index;
  static uint32_t deadLine = INIT_DELAY;

  static int16_t BaroHistTab[BARO_TAB_SIZE];
  static int8_t BaroHistIdx;
  static int32_t BaroHigh,BaroLow;
  int32_t temp32;
  int16_t last;
 
  if (currentTime < deadLine) return;
  deadLine = currentTime + UPDATE_INTERVAL;

  //**** Alt. Set Point stabilization PID ****
  //calculate speed for D calculation
  last = BaroHistTab[BaroHistIdx];  // letzter Wert aus BaroHistTab
  BaroHistTab[BaroHistIdx] = BaroAlt/10;  // durch 10 teilen
  BaroHigh += BaroHistTab[BaroHistIdx];
  index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
  BaroHigh -= BaroHistTab[index];
  BaroLow  += BaroHistTab[index];
  BaroLow  -= last;

  BaroHistIdx++;
  if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;

  if (SonarAlt < 0) return; //wilco.... -1 = not a correct reading, Exit this routine and leave BaroPID at last value, so copter does not jump.
  BaroPID = 0;

  //D
  #ifdef SONAR
    temp32 = conf.D8[PIDALT]*(SonarAlt-LastSonarAlt) / 40; // example climbed from 150 to 170 cm, D=7 -> temp32=7*(170-150)/40=3,5 -> BaroPID = -3,5
  #else
    temp32 = conf.D8[PIDALT]*(BaroHigh - BaroLow) / 40;
  #endif
  BaroPID-=temp32;

  EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);

  // Olaf
  // Use sonar to hold at 180 cm instead of last height
 
  #ifdef SONAR
    if (SonarAlt < 500)
      temp32 = 100 - SonarAlt;
    else
      temp32 = 0;  // if no valid result from SONAR -> no P-Value!
  #else
    temp32 = AltHold - EstAlt;
    if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0;  //remove small differences to reduce noise near zero position
  #endif

 
  //P
  BaroPID += conf.P8[PIDALT]*constrain(temp32,(-2)*conf.P8[PIDALT],2*conf.P8[PIDALT])/100;  // temp32 auf should be for example in range +- 32 for P=16 (= 1.6 in GUI)   
  // example: climbed from 150 cm to 170 cm, target height: 180 cm, P=40
  // BaroPID += 40*(180-170)/100 = increase BaroPID for 4,0
 
  BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150

  //I
  errorAltitudeI += temp32*conf.I8[PIDALT]/50; //example: height is 170 cm, target heigth 180 cm, I = 15 (0.015 in GUI) -> 10*15/50 = increase for 3
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  temp32 = errorAltitudeI / 500; //I in range +/-60 // this is a long term PID - will for example compensate decreasing battery voltage
  BaroPID+=temp32;
}
//wilco


I'm running a normal 328P on the multiwii, and a Attiny4313 to connect the HCSR04 (No GPS on this copter). I've temporarily connected all to my quad, and it is holding altitude pretty good.

Weather is very windy at the moment, and I can only fly around the house, where there is a lot of turbulence, but still, it manages quite OK...

made a small modification that if the sonar data is faulty/out of range (-1), the BaroPID is not reset to zero, (or even worse, run through the PID algorithm), but simply left at the last state.... That way, the copter does not jump if momentarily loosing the sonar distance (which happens quite often).

I need to activate Baro, even though my quad does not have a baro (just sonar now), otherwise the baroPID routine will not run.... but time permitting, I'm going to have a shot at improving that.... Right now I get lots of I2C errors, due to the missing Baro... (zero errors if no baro defined, so TinyGPS is working perfect).
And if I'm feeling very lucky, I might even attempt to integrate Baro + Sonar in the same routine.....
These are my first attempts to actually change something more that the config.h.... so please bare with me.... :roll:

But I still would like to go the route with directly connected HCSR04 to the Flyduino.... Any progress on that with the latest dev as a base ?
I think we're getting close to a working Sonar hold.

Wilco
wilco1967
 
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Tommie » Sun Jun 17, 2012 1:57 pm

Can you please publish a patch relative to the latest svn _shared version? I'd be delighted to include it into my git branch.
Tommie
 
Posts: 438
Joined: Sun Apr 08, 2012 9:50 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Cronalex » Sun Jun 17, 2012 4:18 pm

Cronalex wrote:I would use this sonar Maxbotix LV-EZ0 you can?

up
Cronalex
 
Posts: 51
Joined: Tue Mar 20, 2012 8:41 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Bazed » Sat Jun 30, 2012 4:44 pm

Has anyone had issues with the TinyWire library in the TinySonarI2C code? I'm not sure, but it looks like the tinywire libs have been updated, now it's called tinywireS and tinywireM (slave & master).
Been trying to change the original code in the tinysonari2c to work with the new libs, but no luck so far. Anyone else having this problem?
Bazed
 
Posts: 1
Joined: Mon Jun 25, 2012 4:44 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Gaijin » Fri Jul 06, 2012 6:26 am

Just pIcked up a nice ready made AT328 based I2C module, would it be difficult to modify the ATTiny code to run on it?

I have an HC-SRF04 kicking about, between them it could make for a very simple to build and low cost I2C sonar sensor, any thoughts

http://www.rctimer.com/index.php?gOo=goods_details.dwt&goodsid=762&productname=
User avatar
Gaijin
 
Posts: 82
Joined: Sat Jan 14, 2012 8:00 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby xpix » Tue Jul 31, 2012 7:51 am

wilco1967 wrote:With the TinyGPS route, the Sonar is working,, and I used the basis of Olaf Baro code (the getEstimatedAltitude part that goes in IMU) to get altitude hold....

I'm running a normal 328P on the multiwii, and a Attiny4313 to connect the HCSR04 (No GPS on this copter). I've temporarily connected all to my quad, and it is holding altitude pretty good.


Hello Wilco, i put ur patch in the new MW 2.1 in my IMU Module. The compiler dont like 'SonarAlt' ... i renamed in ur Code to 'sonarAlt'. What is happend with 'lastSonarAlt' in ur Code, u dont use it (everytime on 0) or u refresh this magic in ur Code?

My Solution:

Code: Select all
  #ifdef SONAR
    if (sonarAlt < 500){
      lastSonarAlt = sonarAlt; // save Alt for later
      temp32 = 100 - sonarAlt;
    } else {
      temp32 = 0;  // if no valid result from SONAR -> no P-Value!
    }
  #else
    temp32 = AltHold - EstAlt;
    if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0;  //remove small differences to reduce noise near zero position
  #endif


in IMU, is this so correct?
xpix
 
Posts: 2
Joined: Wed Jul 11, 2012 10:54 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby dr.tom » Tue Jul 31, 2012 9:33 am

Gaijin wrote:Just pIcked up a nice ready made AT328 based I2C module, would it be difficult to modify the ATTiny code to run on it?

I have an HC-SRF04 kicking about, between them it could make for a very simple to build and low cost I2C sonar sensor, any thoughts

http://www.rctimer.com/index.php?gOo=goods_details.dwt&goodsid=762&productname=



I like the idea, I've build an attiny+sonar just for curiosity, but MWC code didn't have real use for it.
but some day when it will use it for althold, this mod with rctimer 328p would be plug&play,
easier to get, connect and program and cleaner look to integrate to other models...

+1
dr.tom
 
Posts: 141
Joined: Fri Mar 30, 2012 4:46 pm
Location: Croatia

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby scanman » Wed Aug 22, 2012 10:25 am

Hello, i have read through all the multiwii posts on sonar and i still dont really know where to start, what sonar to buy (they seem to range from $60 to $6), what code i need to modify , and where i need to wire up the sonar. would someone be so kind as to post a few simple steps to getting started?
Note: i have got the i2C nav board from rc timer wired up to a GPS using EOSBandi's code , as discussed at length in the GPS threads, so i am wondering how to connect the sonar? Do I need another i2c board? What code do i flash onto that board? do I wire it up to the same I2C port as the GPS i2C board?
Any tips would be useful before i get started!

I2C nav board link: http://www.rctimer.com/index.php?gOo=go ... oductname=
scanman
 
Posts: 74
Joined: Thu Jun 21, 2012 9:26 am
Location: Durban, South Africa

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Sat Sep 01, 2012 12:13 pm

i'm using this code for my sr04 sonar/baro alt mode, juste mixing both and altering "EstAlt" as in megapirate/arducopter. No tilt correction or advanced stuff, but works fine, alt hold logic isn't altered neither code for sonar device. With a good tuned Alt hold pids value, it's working perfectly.

in MultiWii_2_1.ino
Code: Select all
//add in global decl
static int32_t BaroHome;
static int8_t BaroHome_set=0;


in IMU.ino
Code: Select all
//replace "EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);" by

#if SONAR
  int32_t tempBaro = BaroHigh*10/(BARO_TAB_SIZE/2);
 
  if(!f.ARMED && f.BARO_MODE) { //init offset
    BaroHome_set = 1;
    BaroHome = tempBaro;
  }
 
  if(f.ARMED && BaroHome_set ==0) { // if forgotten
    BaroHome_set = 1;
    BaroHome = tempBaro;
  }

  //mix baro/sonar   
  if(SonarAlt<SONAR_SONARFULL) {
    EstAlt = BaroHome+sonarAlt;
  }
  else if(SonarAlt>=SONAR_SONARFULL && SonarAlt <SONAR_BAROFULL) {
    float fade = (float)(SONAR_BAROFULL-sonarAlt)/(SONAR_BAROFULL-SONAR_SONARFULL);
    fade = constrain(fade, 0.0, 1.0);
    EstAlt = (BaroHome+sonarAlt)*fade + tempBaro*(1-fade);
  } else {
    EstAlt = tempBaro;
  }
#else
    EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
#endif


and in config.h
Code: Select all
     //mixing values between sonar and baro
     #define SONAR_SONARFULL 150 //full sonar reading before this point (cm)
     #define SONAR_BAROFULL 300 //full baro reading after this point (cm)


baro offset is done by activating baro mode in non armed mode or at the first arming action.
Last edited by penpen77 on Wed Oct 17, 2012 5:21 pm, edited 1 time in total.
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby crazyal » Tue Sep 04, 2012 11:32 pm

If anybody is interested in using the HC-sr04 with the i2c-gps-nav, I have added some code, so the i2c-gps reads the sonar and the data can be read via the I2C bus.
It basically works the same as the tyiny-gps with sonar.
The code is on github: http://github.com/luggi
@penpen77 what pid values are you using ?
crazyal
 
Posts: 84
Joined: Tue Sep 04, 2012 11:25 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Magnetron » Wed Sep 05, 2012 1:32 am

Is possible to integrate dyp-me007?
Magnetron
 
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Wed Sep 05, 2012 10:57 am

@crazyal

1.2/0.0/14, i've noticed playing more with D param has better effect than playing with P,I like other mode.

@magnetron

dyp-me007 works like sr04
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Magnetron » Wed Sep 05, 2012 6:11 pm

I have one dyp-me007v2 and a mega2560. What pin I must use to connect to?
Magnetron
 
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Mon Sep 17, 2012 11:19 am

updated code for new alt hold code (for MS561101BA)

works fine (on pos hold and at low speed), for better result, perform your althold pid tuning around 50% of sonar/baro fusion altitude.
at "high" speed, the baro-only EstAlt could increase if not wind-protected, and sonar/baro fusion could produce wrong alt reading, so be carefull...

Code: Select all
void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

  static int16_t baroHistTab[BARO_TAB_SIZE];
  static int8_t baroHistIdx;
  static int32_t baroHigh;
 

  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  uint16_t dTime = currentTime - deadLine;
  deadLine = currentTime;
 

  //**** Alt. Set Point stabilization PID ****
  baroHistTab[baroHistIdx] = BaroAlt/10;
  baroHigh += baroHistTab[baroHistIdx];
  baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
 
  baroHistIdx++;
  if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;

 
#if SONAR
  int32_t tempBaro = EstAlt*0.6f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.4f;
 
  if(!f.ARMED && f.BARO_MODE) { //init offset
    BaroHome_set = 1;
    BaroHome = tempBaro;
  }
 
  if(f.ARMED && BaroHome_set ==0) { // if forgotten
    BaroHome_set = 1;
    BaroHome = tempBaro;
  }

  //mix baro/sonar   
  if(SonarAlt<SONAR_SONARFULL) {
    EstAlt = BaroHome+SonarAlt;
  }
  else if(SonarAlt>=SONAR_SONARFULL && SonarAlt <SONAR_BAROFULL) {
    float fade = (float)(SONAR_BAROFULL-SonarAlt)/(SONAR_BAROFULL-SONAR_SONARFULL);
    fade = constrain(fade, 0.0, 1.0);
    EstAlt = (BaroHome+SonarAlt)*fade + tempBaro*(1-fade);
  } else {
    EstAlt = tempBaro;
  }
#else

    EstAlt = EstAlt*0.6f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.4f;
   
#endif
 
  //P
  int16_t error = constrain(AltHold - EstAlt, -300, 300);
  error = applyDeadband16(error, 10); //remove small P parametr to reduce noise near zero position
  BaroPID = constrain((conf.P8[PIDALT] * error / 100), -150, +150);
 
  //I
  errorAltitudeI += error * conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  BaroPID += (errorAltitudeI / 500); //I in range +/-60
 
 
  // projection of ACC vector to global Z, with 1G subtructed
  // Math: accZ = A * G / |G| - 1G
  float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
  //int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - 1/invG;
  accZ = applyDeadband16(accZ, ACC_Z_DEADBAND);
 
  static float vel = 0.0f;
  static float accVelScale = 9.80665f / acc_1G / 10000.0f;
 
  // Integrator - velocity, cm/sec
  vel+= accZ * accVelScale * dTime;
 
  static int32_t lastBaroAlt = EstAlt;
  float baroVel = (EstAlt - lastBaroAlt) / (dTime/1000000.0f);
  baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
  baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero 
  lastBaroAlt = EstAlt;
 
  // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
  // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
  vel = vel * 0.985f + baroVel * 0.015f;
  //vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
 
  //D
  BaroPID -= constrain(conf.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
}


all depencies are in previous code snippet or alt hold topic last code snippet

This is a "attemp to integrate sonar" topic, not an "early adopting perfectly working feature" topic, you have to know what you are doing, please stop pm/blame me because you crash your stuff... :shock:
Last edited by penpen77 on Wed Oct 17, 2012 5:20 pm, edited 1 time in total.
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Magnetron » Sat Sep 22, 2012 2:31 pm

there are new on sonar / baro implementation?
Baro altitude routine only seems to be very responsive.
Magnetron
 
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby KasparsL » Mon Sep 24, 2012 12:32 am

+1,
Is there some news for using HC-SR04 or other?
User avatar
KasparsL
 
Posts: 75
Joined: Wed May 16, 2012 3:31 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby janekx » Mon Sep 24, 2012 9:38 am

+1 for HC-SR04
janekx
 
Posts: 63
Joined: Wed Sep 12, 2012 10:08 pm
Location: Brno, Czech Republic

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Tue Sep 25, 2012 9:07 pm

sr04 and simili works since several months.....
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Tue Sep 25, 2012 9:14 pm

EDIT: use code snippet instead

full code, based on last build (r1129)

althold with sonar/baro mixed works fine, tested on sr04, dypme007 and tinygps (i dont have i2c native devantech)

current define are for crius/mega board, don't forget to enable once baro mode before arming to set "baro home/zero alt level" (visible in debug[3])

sonar value are mixed before baro lpf, more robust but more "floppy", free to change before/after lpf

BTW, very low althold must be reached by slow going down, not by going up

last thing, ultrasonic doesn't like carpet, dont try inside over a carpet >_<

in config.h

hardware part
SONAR_GENERIC_ECHOPULSE is define for interrupt driven tx/trigger/pulse/echo sonar, pinout ar mapped in def.h (fallow the define)
SONAR_GENERIC_SCALE is the value for converting us to cm (not the same between sr04,me007, etc...
SONAR_GENERIC_MAX_RANGE is the range of sonar (used for validating reading, which now i remembered i've forgotten to test in code.... be carefull, add a test with 9999 value)

baro/sonar mixing part (assuming sonarAlt is correctly provided by hardware)
SONAR_TILT_CORRECTION define if sonar reading wil be corrected during tilting quad (use triggo, ressources hungry, could be optimized)
SONAR_BARO_FUSION activating fusion
SONAR_BARO_FUSION_LC low cut, below this value, cm, only sonar reading are used for EstAlt/AltHold
SONAR_BARO_FUSION_HC high cut, above this value, cm, only baro are used
SONAR_BARO_FUSION_RATIO specify amount of each sensor are used (sonar mult prior), if 0.0, its proportional, close to LC, it's more sonar, close to HC, more baro

there aren't really many heavy modification, but all is here and working so far, so good
Last edited by penpen77 on Tue Oct 16, 2012 1:05 pm, edited 2 times in total.
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby Magnetron » Tue Sep 25, 2012 11:11 pm

penpen77 wrote:full code, based on last build (r1129)

althold with sonar/baro mixed works fine, tested on sr04, dypme007 and tinygps (i dont have i2c native devantech)

current define are for crius/mega board, don't forget to enable once baro mode before arming to set "baro home/zero alt level" (visible in debug[3])

sonar value are mixed before baro lpf, more robust but more "floppy", free to change before/after lpf

BTW, very low althold must be reached by slow going down, not by going up

last thing, ultrasonic doesn't like carpet, dont try inside over a carpet >_<

in config.h

hardware part
SONAR_GENERIC_ECHOPULSE is define for interrupt driven tx/trigger/pulse/echo sonar, pinout ar mapped in def.h (fallow the define)
SONAR_GENERIC_SCALE is the value for converting us to cm (not the same between sr04,me007, etc...
SONAR_GENERIC_MAX_RANGE is the range of sonar (used for validating reading, which now i remembered i've forgotten to test in code.... be carefull, add a test with 9999 value)

baro/sonar mixing part (assuming sonarAlt is correctly provided by hardware)
SONAR_TILT_CORRECTION define if sonar reading wil be corrected during tilting quad (use triggo, ressources hungry, could be optimized)
SONAR_BARO_FUSION activating fusion
SONAR_BARO_FUSION_LC low cut, below this value, cm, only sonar reading are used for EstAlt/AltHold
SONAR_BARO_FUSION_HC high cut, above this value, cm, only baro are used
SONAR_BARO_FUSION_RATIO specify amount of each sensor are used (sonar mult prior), if 0.0, its proportional, close to LC, it's more sonar, close to HC, more baro

there aren't really many heavy modification, but all is here and working so far, so good


I would to try it.
A pinout to connect a DYP-ME007 on a Mega2560?
Magnetron
 
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby mahowik » Wed Sep 26, 2012 4:30 am

penpen77 wrote:full code, based on last build (r1129)

althold with sonar/baro mixed works fine, tested on sr04, dypme007 and tinygps (i dont have i2c native devantech)

current define are for crius/mega board, don't forget to enable once baro mode before arming to set "baro home/zero alt level" (visible in debug[3])

Hi penpen!

In general it looks ok, but i propose to add some changes
1) to avoid manual init and keep in mind additional instruction probably make sense to change this
Code: Select all
if(!f.ARMED && f.BARO_MODE) { //init offset
      BaroHome = (baroHigh*10.0f/(BARO_TAB_SIZE - 1));
    }
 
    if(f.ARMED && BaroHome ==0) { // if forgotten
      BaroHome = (baroHigh*10.0f/(BARO_TAB_SIZE - 1));
    }

to this:
Code: Select all
if(!f.ARMED) { //init offset till motors not armed
      BaroHome = BaroHome*0.9f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.1f; // play with optimal coef. here
    }


thx-
Alex
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Wed Sep 26, 2012 7:34 am

indeed, that was my first impl, but it's doing some probs:
- until copter armed, EstAlt value isn't correct...
- using some OSD (mobidrone for example) firsts reading after startup are used for offset, so if waiting too long for setting barohome, incorrect value are used
- if windy day, it's good to be able to "reset" barohome to avoid crazy value

in my opinion, the best option should be: auto set after XX seconds (delay define) and possibility to override manually barohome stored value
But, that's mean adding again some define (already added a lot for sonar) and mapping an aux statement for set barohome

I'd try giving value at compile time, baro home value int the code, but it was bad idea, readings have too many way of changing (inside, outside, weather, temperature, etc...)

i don't really know what the better to do, but for testing, activating althold once if not armed to store barohome (with the ability to store when arming the first time if forgotten) do the job, and doesn't need to arm for ground testing.

One other impl i've done (not in code here) but usefull, is adding a "buzzing" event if EstAlt is too low, kind of collision alarm since Alt reading is more accurate with sonar. Just some mod with lighting landing code
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby mahowik » Wed Sep 26, 2012 3:43 pm

penpen77 wrote:indeed, that was my first impl, but it's doing some probs:
- until copter armed, EstAlt value isn't correct...
- using some OSD (mobidrone for example) firsts reading after startup are used for offset, so if waiting too long for setting barohome, incorrect value are used
- if windy day, it's good to be able to "reset" barohome to avoid crazy value

in my opinion, the best option should be: auto set after XX seconds (delay define) and possibility to override manually barohome stored value
But, that's mean adding again some define (already added a lot for sonar) and mapping an aux statement for set barohome

I'd try giving value at compile time, baro home value int the code, but it was bad idea, readings have too many way of changing (inside, outside, weather, temperature, etc...)

i don't really know what the better to do, but for testing, activating althold once if not armed to store barohome (with the ability to store when arming the first time if forgotten) do the job, and doesn't need to arm for ground testing.

One other impl i've done (not in code here) but usefull, is adding a "buzzing" event if EstAlt is too low, kind of collision alarm since Alt reading is more accurate with sonar. Just some mod with lighting landing code


Strange that it's not working for you. Actually it's averaged all time altitude while motors are not armed...
Code: Select all
if(!f.ARMED) { //init offset till motors not armed
      BaroHome = BaroHome*0.9f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.1f; // play with optimal coef. here
    }

But yes it takes some time for initialization. As I mentioned here new alt hold will take about 10-15 sec to init/stabilize all LPFs and CF...
- wait 10-15 sec after power on OR after ACC calibration... all LPFs, velocity integrator and CF need to be syncronized
During this time light BaroHome LPF also should be stabilized...

Also I'm sure that you know but just for your information on altitudes less then 1..2m there ground effect and baro can give minus 1..2m in measurement and become very unstable with their raw data... Probably it's reason why you was confused with init baro values... According to this to avoid baro ground effect it make sense to set SONAR_BARO_FUSION_LC at least to 150cm and from my experience sonars like dyp-me007 and hc-sr04 gives unstable values after 2..2.5m on the fly because of the acoustic noise form motors and electrical influence from power and ESC cords... so I propose
Code: Select all
      #define SONAR_BARO_FUSION_LC 150 //cm, low cut, below = full sonar
      #define SONAR_BARO_FUSION_HC 250 //cm, high cut, above = full baro


thx-
Alex
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Thu Sep 27, 2012 10:22 am

it was just testing values, i've archived my working dir "as is", it's not intented to be working out the box or using perfectly tuned constants
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby mahowik » Thu Sep 27, 2012 3:26 pm

penpen77 wrote:it was just testing values, i've archived my working dir "as is", it's not intented to be working out the box or using perfectly tuned constants

It was not a critique, it was constructive discussion in terms of this feature ;)
Also most people here are not programmers and actually have not experience to tune any parameters without exact manual/guide... so it's useful to put tuned feature at least checked in your config...
and if feature has a lot of good feedback from diff pilots it's very possible to see this in new release ;)
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby KasparsL » Wed Oct 10, 2012 1:03 pm

Will the support for HC-SR04 included in some release, or there is some reason not to use it, but take some i2c sonar sensor?
User avatar
KasparsL
 
Posts: 75
Joined: Wed May 16, 2012 3:31 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Thu Oct 11, 2012 2:02 pm

dunno
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby jhgeesink » Sat Oct 13, 2012 1:35 pm

Trying your sketch now penpen77, works now but the altitude is going up sometimes and then goes back to the correct altitude.
Motors off.
Image

Han.
jhgeesink
 
Posts: 3
Joined: Fri Jul 20, 2012 10:31 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby mahowik » Mon Oct 15, 2012 7:32 pm

Hi guys,

I have spent a lot of time this weekend to play with sonar solutions...

About implementation from penpen77. I have found our at least 2 main issues with this approach:
a) it's requires to average only correct raw sonar data readings but not 9999
b) there was not workable mix from sonar to baro because "fade" var was always null... I checked this in debug (fade*10.0f)
Code: Select all
fade = (SONAR_BARO_FUSION_HC-sonarAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);

you should specify as float before division
Code: Select all
fade = ((float)(SONAR_BARO_FUSION_HC-sonarAlt))/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);


I also added smooth (on the fly) ground offset correction because baro have drift and did some refactoring:

Code: Select all
int32_t tempAlt = baroHigh*10/(BARO_TAB_SIZE - 1);

  #if defined(SONAR)
 
   
    if(!f.ARMED && (sonarAlt < SONAR_BARO_FUSION_HC)) { //init offset till motors not armed
      EstAltStart = EstAltStart*0.95f + (tempAlt-sonarAlt)*0.05f;
    }
   
    //mix baro/sonar
    debug[0] = sonarAlt;
    if(EstAltStart != 0) {   
      if(sonarAlt < SONAR_BARO_FUSION_LC) {
        tempAlt = EstAltStart + sonarAlt;
      } else if(sonarAlt < SONAR_BARO_FUSION_HC) {
     
        // make smooth EstAltStart correction because baro have drift
        EstAltStart = EstAltStart*0.98f + (tempAlt-sonarAlt)*0.02f;
     
        float fade = SONAR_BARO_FUSION_RATIO;
        if(fade == 0.0) {
          fade = ((float)(SONAR_BARO_FUSION_HC-sonarAlt))/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
        }
        fade = constrain(fade, 0.0f, 1.0f);
        //debug[2] = fade*10.0f;
       
        tempAlt = (EstAltStart + sonarAlt)*fade + tempAlt*(1.0f - fade);
      }
    }
  #else
    if(!f.ARMED) { //init offset till motors not armed
      EstAltStart = EstAltStart*0.95f + tempAlt*0.05f;
    }
  #endif
  debug[1] = EstAltStart;
 
  //EstAlt = EstAlt*0.6f + tempAlt*0.4f; // additional LPF to reduce baro noise 
  EstAlt = (EstAlt*3 + tempAlt*2)/5; // additional LPF to reduce baro noise 


But even after this it was not workable in real environment (e.g. when grass on ground and some sonar reading are not correct). So I started to think/implement error-based handler instead of current only-alt-based to mix/switch beetween sonar and baro and get much better results but not so satisfactory...
So I investigated solution from alexmos. Wow! It was that I have tried to get! :)
So I extended his sonar driver for mega board and have integrated his solution with new alt hold... then I tried to check it on the fly (but only solid on surface for now... will test more)... only one word: PERFECT! :)

thx-
Alex
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby shikra » Tue Oct 16, 2012 12:56 pm

Alex that is very cool. looking forward to the vid.

Which sensor do you use?
User avatar
shikra
 
Posts: 783
Joined: Wed Mar 30, 2011 7:58 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Tue Oct 16, 2012 1:03 pm

yep, you need to use the code snippet of the topic, not the archive, i have mixed 2 version by mistake (current_dev and 2.1)
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby mahowik » Tue Oct 16, 2012 5:09 pm

shikra wrote:Alex that is very cool. looking forward to the vid.

Which sensor do you use?


Tried with HC-SR04 on the fly and DYP-ME007 in GUI... Still need to clean up some points and will post the code soon...
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby doughboy » Tue Oct 16, 2012 10:31 pm

question on this part of the code (from http://code.google.com/p/multiwii/sourc ... Bsonar.rar)

Code: Select all
         #ifdef SONAR_TILT_CORRECTION
          float temp = cos(float(angle[1])/10) * cos(float(angle[0])/10);           
       temp = max(temp+0.05, 0.707);
            temp = constrain(temp,0.707,1.0);
            temp = (float)sonarAlt * temp;
            if(temp<sonarAlt) sonarAlt = temp;
     #endif
 


The comment for angle array says
Code: Select all
static int16_t angle[2]    = {0,0};  // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800


so I get the divide by 10 part. But arduino cos function takes radians
http://www.arduino.cc/en/Reference/Cos

so is the code passing angle in degrees?

if you look in other parts of the source, you will see the conversion to radians like this
Code: Select all
float cos_yaw_x = cos(heading*0.0174532925f);


where 0.0174532925 is PI/180.

Arduino.h defines the following constants
Code: Select all
#define PI 3.1415926535897932384626433832795
#define HALF_PI 1.5707963267948966192313216916398
#define TWO_PI 6.283185307179586476925286766559
#define DEG_TO_RAD 0.017453292519943295769236907684886
#define RAD_TO_DEG 57.295779513082320876798154814105
doughboy
 
Posts: 252
Joined: Tue Sep 04, 2012 7:20 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Postby penpen77 » Tue Oct 16, 2012 10:56 pm

yep, mistake, wrong adaptation from apm in my work dir at the time of zipping. Use code snippet instead for bug-free/less
penpen77
 
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

PreviousNext

Return to Ideas

Who is online

Users browsing this forum: No registered users and 1 guest

cron