Sonar HC-SR04 Support now implemented in MW2.4

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

Image
just got my HC-SR04 sonar module supported and implemented in MW2.4

first of all, I'd like to give credit to: kataventos for his codes and I just ported it to MultiWii 2.4. There are some changes too. This should work fairly in MultiWii 2.3 as well.

Here's the video
https://www.youtube.com/watch?v=QMQyNspc2Mg


the whole source code is uploaded here. No need to copy&paste the codes below
https://onedrive.live.com/redir?resid=a714effa21482b83!6680&authkey=!AMIFxrHS9w-pnek&ithint=file%2czip


Let's start! Please follow accordingly and let me know if you had some troubles making it work.

Let's first add a support to our sonar
add new parameter in config.h, put it anywere as long as it is inside the #ifndef CONFIG_H_ ..... #endif /* CONFIG_H_ */
I added that just before #endif /* CONFIG_H_ */

Code: Select all

#pragma region GENERIC SONAR
/* Generic sonar: hc-sr04, srf04, dyp-me007, all generic sonar with echo/pulse pin
default pulse is PH6/12, echo is PB4/11
*/
#define SONAR_GENERIC_ECHOPULSE
#define SONAR_GENERIC_SCALE 58            //scale for ranging conversion (hcsr04 is 58)
#define SONAR_GENERIC_MAX_RANGE 500         //cm (could be more)
#define SONAR_GENERIC_TRIGGER_PIN 12      // motor 12
#define SONAR_GENERIC_ECHO_PIN 11         // motor 11

/************************* Sonar alt hold / precision / ground collision keeper *******/
#define SONAR_MAX_HOLD 400               //cm, kind of error delimiter, for now to avoid rocket climbing, only usefull if no baro

//if using baro + sonar       
#define SONAR_BARO_FUSION_LC 100         //cm, baro/sonar readings fusion, low cut, below = full sonar
#define SONAR_BARO_FUSION_HC SONAR_MAX_HOLD   //cm, baro/sonar readings fusion, high cut, above = full baro
#define SONAR_BARO_FUSION_RATIO 0.0         //0.0-1.0,  baro/sonar readings fusion, amount of each sensor value, 0 = proportionnel between LC and HC
#define SONAR_BARO_LPF_LC 0.9f
#define SONAR_BARO_LPF_HC 0.9f
#pragma endregion


def.h
look for this line

Code: Select all

#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(I2C_GPS_SONAR)

and add our new identifier

Code: Select all

#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(I2C_GPS_SONAR) || defined(SONAR_GENERIC_ECHOPULSE)


still in def.h
just before
#endif /* DEF_H_ */
add this

Code: Select all

#pragma region GENERIC SONAR SUPPORT
#if defined(SONAR_GENERIC_ECHOPULSE)
#define SONAR_GEP_TriggerPin             SONAR_GENERIC_TRIGGER_PIN
#define SONAR_GEP_TriggerPin_PINMODE_OUT pinMode(SONAR_GEP_TriggerPin,OUTPUT);
#define SONAR_GEP_TriggerPin_PIN_HIGH    PORTB |= 1<<6;
#define SONAR_GEP_TriggerPin_PIN_LOW     PORTB &= ~(1<<6);
#define SONAR_GEP_EchoPin                SONAR_GENERIC_ECHO_PIN
#define SONAR_GEP_EchoPin_PINMODE_IN     pinMode(SONAR_GEP_EchoPin,INPUT);
#define SONAR_GEP_EchoPin_PCINT          PCINT5
#define SONAR_GEP_EchoPin_PCICR          PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
#define SONAR_GEP_EchoPin_PCMSK          PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
#define SONAR_GEP_EchoPin_PCINT_vect     PCINT0_vect  // PCINT0-7 belog PCINT0_vect
#define SONAR_GEP_EchoPin_PIN            PINB  // PCINT0-7 belong to PINB
#endif
#pragma endregion


moving on to Sensors.cpp
look for this line

Code: Select all

// ************************************************************************************************************
// I2C Sonar SRF08
// ************************************************************************************************************
// first contribution from guru_florida (02-25-2012)
//
#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235)
   .
   .
   .
   .
   .
#else
   void Sonar_init() {}
   void Sonar_update() {}
#endif

and in the

Code: Select all

#else
   void Sonar_init() {}
   void Sonar_update() {}
#endif


change the two line inside the #else and #endif to this

Code: Select all

#if defined(SONAR_GENERIC_ECHOPULSE)
// ************************************************************************************************************
// Generic Sonar Support
// ************************************************************************************************************
volatile unsigned long SONAR_GEP_startTime = 0;
volatile unsigned long SONAR_GEP_echoTime = 0;
volatile static int32_t  tempSonarAlt = 0;

void Sonar_init() {
   SONAR_GEP_EchoPin_PCICR;
   SONAR_GEP_EchoPin_PCMSK;
   SONAR_GEP_EchoPin_PINMODE_IN;
   SONAR_GEP_TriggerPin_PINMODE_OUT;
}

uint8_t Sonar_update() {
   sonarAlt = 1 + tempSonarAlt;
   SONAR_GEP_TriggerPin_PIN_LOW;
   delayMicroseconds(2);
   SONAR_GEP_TriggerPin_PIN_HIGH;
   delayMicroseconds(10);
   SONAR_GEP_TriggerPin_PIN_LOW;

   return sonarAlt;
}

ISR(SONAR_GEP_EchoPin_PCINT_vect) {
   if (SONAR_GEP_EchoPin_PIN & (1 << SONAR_GEP_EchoPin_PCINT)) {
      SONAR_GEP_startTime = micros();
   }
   else {
      SONAR_GEP_echoTime = micros() - SONAR_GEP_startTime;
      if (SONAR_GEP_echoTime <= SONAR_GENERIC_MAX_RANGE*SONAR_GENERIC_SCALE)
         tempSonarAlt = SONAR_GEP_echoTime / SONAR_GENERIC_SCALE;
      else
         tempSonarAlt = -1;
   }
}
#else
void Sonar_init() {}
uint8_t Sonar_update() {}
#endif


ok so this time, our generic sonar modules should be working already. try to throw the sonar altitude on any debug array.

next thing we want to do is a way to activate/deactivate the sonar, much like the BARO mode.
Now we need a SONAR mode. So let's add a new box called "SONAR"
going back to MultiWii.cpp file. Look for this line

Code: Select all

#if GPS
  "MISSION;"
  "LAND;"
#endif

and add this line below

Code: Select all

#if SONAR
  "SONAR;"
#endif


look for this line again

Code: Select all

#if GPS
  20, //"MISSION;"
  21, //"LAND;"
#endif

and add this one below

Code: Select all

#if SONAR
  23, //"SONAR;"
#endif


in Protocol.cpp
look for this line

Code: Select all

#if defined(OSD_SWITCH)
      if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
#endif

and add this one below

Code: Select all

#if SONAR
      if (f.SONAR_MODE) tmp |= 1 << BOXSONAR;
#endif


now that we have a way to get the sonar status and a way to activate/deactivate our sonar.
what we need to do now is to have a way to integrate the sonar altitude once we activate it.
MultiWii.h
look for this line

Code: Select all

extern uint16_t calibratingA;
extern uint16_t calibratingB;
extern uint16_t calibratingG;

and add this one below

Code: Select all

#if SONAR
extern uint16_t calibratingS;
#endif


MultiWii.cpp
in loop() method
look for this lines

Code: Select all

#if ACC
      if (rcOptions[BOXANGLE] || (failsafeCnt > 5 * FAILSAFE_DELAY)) {
         // bumpless transfer to Level mode
         if (!f.ANGLE_MODE) {
            errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
            f.ANGLE_MODE = 1;
         }
      }
      else {
         if (f.ANGLE_MODE) {
            errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0;
         }
         f.ANGLE_MODE = 0;
      }
      if (rcOptions[BOXHORIZON]) {
         f.ANGLE_MODE = 0;
         if (!f.HORIZON_MODE) {
            errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
            f.HORIZON_MODE = 1;
         }
      }
      else {
         if (f.HORIZON_MODE) {
            errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0;
         }
         f.HORIZON_MODE = 0;
      }
#endif

and this one below

Code: Select all

#if SONAR
      if (rcOptions[BOXSONAR]) {
         if (f.SONAR_MODE == 0) {
            f.SONAR_MODE = 1;

            AltHold = alt.EstAlt;

#if defined(ALT_HOLD_THROTTLE_MIDPOINT)
            initialThrottleHold = ALT_HOLD_THROTTLE_MIDPOINT;
#else
            initialThrottleHold = rcCommand[THROTTLE];
#endif

            errorAltitudeI = 0;
            BaroPID = 0;
            f.THROTTLE_IGNORED = 0;
         }
      }
      else {
         f.SONAR_MODE = 0;
      }
#endif


still at MultiWii.cpp
again, look for this line

Code: Select all

uint16_t calibratingA = 0;  // the calibration is done in the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
uint16_t calibratingB = 0;  // baro calibration = get new ground pressure value
uint16_t calibratingG;

and add this below

Code: Select all

#if SONAR
uint16_t calibratingS = 0; // sonar calibration = get new ground altitude
#endif


still at MultiWii.cpp file
in setup() method, look for this line

Code: Select all

calibratingB = 200;  // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles

and add this below

Code: Select all

#if SONAR
  calibratingS = 10;
#endif


and in go_arm() method
look for this line

Code: Select all

#if BARO
      calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif

and add this line below

Code: Select all

#if SONAR
     calibratingS = 10;
#endif


and in loop() method
look for this line

Code: Select all

#if BARO
               calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif

and add this line below

Code: Select all

#if SONAR
               calibratingS = 10;
#endif


still at loop() method. Look for this line

Code: Select all

#if GPS
         if (GPS_Compute() != 0) break;  // performs computation on new frame only if present
#if defined(I2C_GPS)
         if (GPS_NewData() != 0) break;  // 160 us with no new data / much more with new data
#endif
#endif

and add/uncomment this line above!!

Code: Select all

#if SONAR
         Sonar_update();
#endif


it would be better though the task order of the sonar is next to BARO like mine here

Code: Select all

static uint8_t taskOrder = 0; // never call all functions in the same loop, to avoid high delay spikes
switch (taskOrder) {
case 0:
   taskOrder++;
#if MAG
   if (Mag_getADC() != 0) break; // 320 µs
#endif
case 1:
   taskOrder++;
#if BARO
   if (Baro_update() != 0) break;
#endif
case 2:
   taskOrder++;   
#if SONAR
   Sonar_update();
#endif
   break;
case 3:
   taskOrder++;
#if BARO || SONAR
   if (getEstimatedAltitude() != 0) break; // 280 us
#endif
case 4:
   taskOrder++;
#if GPS
   if (GPS_Compute() != 0) break;  // performs computation on new frame only if present
   #if defined(I2C_GPS)
      if (GPS_NewData() != 0) break;  // 160 us with no new data / much more with new data
   #endif
#endif
case 5:
   taskOrder = 0;
//#if SONAR
//         Sonar_update();
//         //debug[0] = sonarAlt;
//#endif
#ifdef LANDING_LIGHTS_DDR
   auto_switch_landing_lights();
#endif
#ifdef VARIOMETER
   if (f.VARIO_MODE) vario_signaling();
#endif
   break;
}


still at loop() and in switch case for taskOrders. Look for this line

Code: Select all

#if BARO
         if (getEstimatedAltitude() != 0) break; // 280 us
#endif

and update that line to make it look like this

Code: Select all

#if BARO || SONAR
         if (getEstimatedAltitude() != 0) break; // 280 us
#endif


still at loop()! look for this line

Code: Select all

#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
   /* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease
   * altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms)
   */
   if (f.BARO_MODE) {
      static uint8_t isAltHoldChanged = 0;
      static int16_t AltHoldCorr = 0;

and update it to look like this

Code: Select all

#if (BARO || SONAR) && (!defined(SUPPRESS_BARO_ALTHOLD))
   /* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease
   * altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms)
   */
   if (f.BARO_MODE || f.SONAR_MODE) {
      static uint8_t isAltHoldChanged = 0;
      static int16_t AltHoldCorr = 0;


now let's move on to IMU.cpp and look for getEstimatedAltitude() method
look for this line

Code: Select all

// baroGroundPressureSum is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
BaroAlt = (logBaroGroundPressureSum - log(baroPressureSum)) * baroGroundTemperatureScale;
alt.EstAlt = (alt.EstAlt * 6 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)

uncomment this line or add "//" like this

Code: Select all

//alt.EstAlt = (alt.EstAlt * 6 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)

and add this lines

Code: Select all

#if SONAR
if (f.SONAR_MODE) {
  if (calibratingS > 0) {
     if (!f.ARMED) { //init offset till motors not armed
        //alt.EstAlt = alt.EstAlt * SONAR_BARO_LPF_LC + sonarAlt * (1 - SONAR_BARO_LPF_LC); // additional LPF to reduce baro noise (faster by 30 µs)

        BaroHome = (alt.EstAlt * 6 + BaroAlt * 2) >> 3; // play with optimal coef. here
     }

     calibratingS--;
  }
}
#endif

#if BARO && !SONAR
   alt.EstAlt = (alt.EstAlt * 6 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
#elif SONAR && !BARO
   alt.EstAlt = alt.EstAlt * SONAR_BARO_LPF_LC + sonarAlt * (1 - SONAR_BARO_LPF_LC);
#elif SONAR && BARO
   // limit sonar altitude
   //if (sonarAlt > SONAR_MAX_HOLD) {
   //  sonarAlt = SONAR_MAX_HOLD;
   //}

   if (sonarAlt < SONAR_BARO_FUSION_LC) {
     alt.EstAlt = alt.EstAlt * SONAR_BARO_LPF_LC + (BaroHome + sonarAlt) * (1 - SONAR_BARO_LPF_LC); // additional LPF to reduce baro noise (faster by 30 µs)
   }
   else if (sonarAlt < SONAR_BARO_FUSION_HC) {
     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);

     // LOG: will LPF should be faded too ? sonar is less sloppy than baro and will be oversmoothed
     // LOG: try same as baro alone 6/4 ratio (same as above about smoothing)
     alt.EstAlt = alt.EstAlt * SONAR_BARO_LPF_HC + ((BaroHome + sonarAlt) * fade + (BaroAlt) * (1 - fade)) * (1 - SONAR_BARO_LPF_HC);
   }
   else {
     alt.EstAlt = (alt.EstAlt * 6 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
   }
#endif

debug[0] = sonarAlt; // raw sonar altitude
debug[1] = BaroAlt; // barometer altitude
debug[2] = alt.EstAlt;

then what follows those debug lines should be this

Code: Select all

#if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)



--------------- CODE UPDATE ---------------

this is update should be for our box name
in file types.h
look for this line

Code: Select all

#if GPS
  uint8_t GPS_FIX :1 ;
  uint8_t GPS_FIX_HOME :1 ;
  uint8_t GPS_BARO_MODE : 1;         // This flag is used when GPS controls baro mode instead of user (it will replace rcOptions[BARO]
  uint8_t GPS_head_set: 1;           // it is 1 if the navigation engine got commands to control heading (SET_POI or SET_HEAD) CLEAR_HEAD will zero it
  uint8_t LAND_COMPLETED: 1;
  uint8_t LAND_IN_PROGRESS: 1;
#endif

and below that, just add

Code: Select all

#if SONAR
  uint8_t SONAR_MODE : 1;
#endif


still in types.h
look for this line

Code: Select all

#if GPS
    BOXGPSNAV,
    BOXLAND,
#endif

and below that, juts add

Code: Select all

#if SONAR
   BOXSONAR,
#endif


go back to MultiWii.cpp and update the box ID. Reason why 23 was because I had other custom box names such as the inflight pid tuning

Code: Select all

#if SONAR
  23, //"SONAR;"
#endif

to

Code: Select all

#if SONAR
  22, //"SONAR;"
#endif


and other updates
again in types.h
look for this line

Code: Select all

#if BARO || GPS
  uint8_t THROTTLE_IGNORED : 1;      // If it is 1 then ignore throttle stick movements in baro mode;
#endif

and change it to

Code: Select all

#if BARO || GPS || SONAR          // SONAR update
  uint8_t THROTTLE_IGNORED : 1;      // If it is 1 then ignore throttle stick movements in baro mode;
#endif


go back to imu.cpp
look for getEstimatedAltitude() method and above that, change this

Code: Select all

#if BARO

to

Code: Select all

#if BARO | SONAR
   static int32_t  BaroHome = 0;

(yes, that BaroHome is included)


--------------- CODE UPDATE TWO ---------------

go to Sensors.cpp and change the return type to void and do not return anything.
change this

Code: Select all

uint8_t Sonar_update() {
   sonarAlt = 1 + tempSonarAlt;
   SONAR_GEP_TriggerPin_PIN_LOW;
   delayMicroseconds(2);
   SONAR_GEP_TriggerPin_PIN_HIGH;
   delayMicroseconds(10);
   SONAR_GEP_TriggerPin_PIN_LOW;

   return sonarAlt;
}

to this

Code: Select all

void Sonar_update() {
   sonarAlt = 1 + tempSonarAlt;
   SONAR_GEP_TriggerPin_PIN_LOW;
   delayMicroseconds(2);
   SONAR_GEP_TriggerPin_PIN_HIGH;
   delayMicroseconds(10);
   SONAR_GEP_TriggerPin_PIN_LOW;
}


and this

Code: Select all

#else
void Sonar_init() {}
uint8_t Sonar_update() {}
#endif

to this

Code: Select all

#else
void Sonar_init() {}
void Sonar_update() {}
#endif


to save you guys from trouble implementing the codes. I uploaded the the entire source code here
MultiWii_Generic_Sonar.zip
open MultiWii_Generic_Sonar.ino in Arduino IDE dont worry about the other files included in there. I write my codes in Visual Studio 2013 with Visual Micro to support Arduino. Make sure to setup your parameters at config.h before uploading the firmware
Attachments
wp_ss_20150331_0001_1024x614.jpg
Last edited by jaysonragasa on Sat May 09, 2015 3:40 pm, edited 11 times in total.

FengShuiDrone
Posts: 234
Joined: Wed Dec 24, 2014 1:20 am
Location: ......

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by FengShuiDrone »

.
Last edited by FengShuiDrone on Fri Aug 07, 2015 1:11 am, edited 1 time in total.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

currently using CRIUS v2 AIOP. I'll post a video soon and the code ASAP.

FengShuiDrone
Posts: 234
Joined: Wed Dec 24, 2014 1:20 am
Location: ......

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by FengShuiDrone »

.
Last edited by FengShuiDrone on Fri Aug 07, 2015 1:12 am, edited 1 time in total.

Maine_Guy
Posts: 27
Joined: Fri Mar 06, 2015 7:07 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by Maine_Guy »

UPDATE: After much research - I've determined that implementing this on this particular HK board is near-impossible.

I updated my base code to 2.4 - mostly hoping to use the sonar portion. Trying to connect a SFR04 board to my Multiwii_32U4_SE - which is connected like the sketch shown below. What pins does the current code for the sonar i2c bus use? I thought it was SCL / SCA for the Ic2 bus? Connected as shown, just makes the plane go crazy.... servos rapidly changing position. A check in the gui shows the sensor numbers jumping all over. This particular board has an i2c-based magnetometer, which I'm wondering - may conflict?


HK Board Wiring for plane
HK Board Wiring for plane


Back of the board
Back of the board
Last edited by Maine_Guy on Tue May 12, 2015 1:04 am, edited 2 times in total.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

yes, connecting that to I2C wont work. Here's why
https://www.youtube.com/watch?v=y-G-SlUuDho


I connected that via I2C bus and look what happen when I start to lift off the quad by hand.

I'll post an update, video, and code as soon as I get back from travel. I'll get back tonight anyway so stay tune. I'll make a video first though..

So for the mean time, my Trigger and Echo is connected in motor signal pins
Trigger is 12
Echo is 11

Maine_Guy
Posts: 27
Joined: Fri Mar 06, 2015 7:07 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by Maine_Guy »

So, are you using 'stock' code for those pins to work (11/12)? I can't figure out where the stock code assigns the pins.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

I am not using stock codes for Sonar

jsjc
Posts: 2
Joined: Thu Apr 02, 2015 7:31 am

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jsjc »

Do you mind sharing the changes in your code??

I have tried do some changes but cannot get it to work...

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

post updated. check very first post

dryflyer
Posts: 10
Joined: Fri Apr 03, 2015 12:02 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by dryflyer »

I love your work. Have you been able to fly with sonar altitude hold? Can we see a vid?

I've tried incorporating your code with the standard 2.4 code but I get compiling errors. Can you provide a link to the 2.4 code with your changes included?

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

hi dryflyer,

unfortunately, I can't share my entire MW2.4 code because there are a lot of experimental work going on in it and I am using different IDE and there are other files that was incorporated in it.

What you can do is to try to "verify" the code and below the Arduino IDE, copy the messages and post it in here so I can help you determining the issue.

Thanks for the support!

Maine_Guy
Posts: 27
Joined: Fri Mar 06, 2015 7:07 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by Maine_Guy »

I worked on this a bit last night, will update with the errors I got. I took notes - but they are at home, I'm at work..

I *almost* got it to compile

missing were:

Variable declaration for a few of the variables - like adding the boxmode, and declaration for other variables.

The last error I got was one I could not locate - something about __vector___9?

Anyway, thanks for your contribution, much more comprehensive than just getting a SONAR signal. I will update this post later with the errors I got.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

I made an update on the codes. please check the first post

look for CODE UDPATE line

dryflyer
Posts: 10
Joined: Fri Apr 03, 2015 12:02 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by dryflyer »

My first issue occurs when I add the generic sonar support to sensors.cpp. I've tried to interpret your instructions several ways but get different compile errors. I don't want to go any further if I'm already getting errors.

If I follow your instructions I get this error.
Sensors.cpp: In function 'uint8_t Sonar_update()':
Sensors.cpp:1529:22: error: new declaration 'uint8_t Sonar_update()'
uint8_t Sonar_update() {
Sensors.h:8:6: error: ambiguates old declaration 'void Sonar_update()'
void Sonar_update();

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

@dryflyer - check update two now in my first post.

Maine_Guy
Posts: 27
Joined: Fri Mar 06, 2015 7:07 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by Maine_Guy »

So very close (I think) I had corrected a few of those already.

also, in types.h

find:

enum box {
BOXARM,
#if ACC
BOXANGLE,
BOXHORIZON,
#endif

add (at end?)


#if SONAR
BOXSONAR,
#endif


down to this error now, not sure how to call the external function?:


MultiWii.cpp: In function 'void loop()':
MultiWii.cpp:1422:23: error: 'Sonar_update' was not declared in this scope
Sonar_update();
^
Error compiling.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

do you have the Sensors.h included?

Code: Select all

#include "Sensors.h"


and please make sure there are no uncommented default sonar identifiers.
This should be uncommented in config.h
//#define SRF02 // use the Devantech SRF i2c sensors
//#define SRF08
//#define SRF10
//#define SRF23

just the SONAR_GENERIC_ECHOPULSE identifier and all its required identifiers

jsjc
Posts: 2
Joined: Thu Apr 02, 2015 7:31 am

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jsjc »

I have tested all the codes and updates on a fresh Multiwii 2.4 and I dont know the reason but on MultiwiiGui goes all over the place, up down, and many i2C errors.
I have plugged sonar in i2c Echo to SDA and TRIG to SCL, VCC to 5V and GND to GND.

What could be the reason??

QuadBow
Posts: 532
Joined: Fri Jan 04, 2013 10:06 am

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by QuadBow »

jsjc wrote:I have plugged sonar in i2c Echo to SDA and TRIG to SCL, VCC to 5V and GND to GND.
What could be the reason??
If you are tallking about that HC-SR04 connecting to the i2c bus will not make it run as already mentioned earlier.

Either you get one of the following i2c compatible sonars devices mentioned in the file sensor.cpp

Code: Select all

#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235)
or you use an additional i2c_gps_nav module (version with sonar support). The proposed code for non i2c sonar deviced mentioned in this thread does not seem to be ready yet
jaysonragasa wrote:unfortunately, I can't share my entire MW2.4 code
.
Last edited by QuadBow on Sat Apr 04, 2015 11:57 am, edited 1 time in total.

dryflyer
Posts: 10
Joined: Fri Apr 03, 2015 12:02 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by dryflyer »

So I have been able to compile the code successfully. Not getting any response from the Baro or Sonar within the MultiWiiGui.

Using a HRSC-04 and Multiwii and Megapirate AIO Flight Controller w/FTDI (ATmega 2560) V2.0 from HobbyKing.

Has anyone got any advice for me?

Also, how do I get the debug values to display in the GUI?

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

dryflyer wrote:So I have been able to compile the code successfully. Not getting any response from the Baro or Sonar within the MultiWiiGui.

Using a HRSC-04 and Multiwii and Megapirate AIO Flight Controller w/FTDI (ATmega 2560) V2.0 from HobbyKing.

Has anyone got any advice for me?

Also, how do I get the debug values to display in the GUI?


in config.h, I included a new identifier named
#define SONAR_GENERIC_TRIGGER_PIN 12 // motor 12
#define SONAR_GENERIC_ECHO_PIN 11 // motor 11

I am using the same board as yours and I have connected the Trigger to Motor 12 and Echo in Motor 11

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

to save you guys from trouble implementing the codes. I uploaded the the entire source code here
MultiWii_Generic_Sonar.zip
open MultiWii_Generic_Sonar.ino in Arduino IDE dont worry about the other files included in there. I write my codes in Visual Studio 2013 with Visual Micro to support Arduino. Make sure to setup your parameters at config.h before uploading the firmware

dryflyer
Posts: 10
Joined: Fri Apr 03, 2015 12:02 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by dryflyer »

Thank you so much for the code.

I cannot enable the SONAR in the gui. The box is there and I can select it but it doesn't activate green.
There is no value showing in the ALT box. Only the second debug field is working. The first debug is stuck with a 1. The third and fourth fields have 0.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

dryflyer wrote:Thank you so much for the code.

I cannot enable the SONAR in the gui. The box is there and I can select it but it doesn't activate green.
There is no value showing in the ALT box. Only the second debug field is working. The first debug is stuck with a 1. The third and fourth fields have 0.


make sure to have your battery connected as well.

FengShuiDrone
Posts: 234
Joined: Wed Dec 24, 2014 1:20 am
Location: ......

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by FengShuiDrone »

.
Last edited by FengShuiDrone on Fri Aug 07, 2015 1:13 am, edited 2 times in total.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

FengShuiDrone wrote:Would I just change the pins to 9 and 10 for my Multiwii pro2 from RTFQ? It doesn't have 11 and 12...... :?


you could try :D

Maine_Guy
Posts: 27
Joined: Fri Mar 06, 2015 7:07 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by Maine_Guy »

I have not had opportunity to play with the code further yet. On my board, a leonardo - pins 11 and 12 correspond (I think) to

Pin 11 = MISO - (echo)
Pin 12 = D11 - (trigger)

I've been afraid to plug the SONAR in - in fear of letting the 'magic smoke' out.

So, just because the program calls 'em 11 and 12 - a Leonardo has different names for the outputs.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

Maine_Guy wrote:I have not had opportunity to play with the code further yet. On my board, a leonardo - pins 11 and 12 correspond (I think) to

Pin 11 = MISO - (echo)
Pin 12 = D11 - (trigger)

I've been afraid to plug the SONAR in - in fear of letting the 'magic smoke' out.

So, just because the program calls 'em 11 and 12 - a Leonardo has different names for the outputs.


Echo and Trigger only send signals and the code just reads them. Just make sure those ECHO and TRIGGER connects to [S]ignal pins and not [+] or [-] try to connect the echo to D11 and trigger to D12. Make sure they are connected to [S]ignal pins!

dryflyer
Posts: 10
Joined: Fri Apr 03, 2015 12:02 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by dryflyer »

Awesome, I now have the debug displaying with the estimated altitude being used for altitude reading.

The SONAR box in the control list does not activate green like it does in your video. Does this mean that altitude hold will not work? Any ideas what I might be doing wrong?

dryflyer
Posts: 10
Joined: Fri Apr 03, 2015 12:02 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by dryflyer »

Also, your task order in loop method in MultiWii.cpp is different when comparing your post to the file you provided. Will this make a difference? Which one is right/better?

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

thanks Dryflyer,

I got confused with my multiple MultiWii.cpp tabs in my IDE.

here's a new download link. I have already tested that and corrected the taskOrder as well. Hopefully you got it working
MultiWii_Generic_Sonar.zip

dryflyer
Posts: 10
Joined: Fri Apr 03, 2015 12:02 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by dryflyer »

Brilliant. Love your work. It seems to be working.

I'll have a test fly today and let you know how it goes.

dryflyer
Posts: 10
Joined: Fri Apr 03, 2015 12:02 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by dryflyer »

Hi Jaysonragasa, I tested the code with a live run and my quadcopter, I have to say that you've done a great job. The altitude hold is very stable. So much fun flying in my underground carpark. I was able to fly up and down ramps and the altitude adjusted accordingly.

When I pitch or roll the quadcopter drops altitude. I assume this is because the sonar is angled away from the ground and it thinks that it needs to reduce height. It does restore correct altitude when I level out but do you have any idea how this can be overcome? There is some code in the MultiWii that applies a throttle boost when pitch or roll is applied to overcome this but it would be much better if used baro/sonar data instead.

Can you expain how the controls work for the SONAR and SONAR/BARO altitude hold? I recall reading in your code that the altitude is adjusted by using throttle once the SONAR is active. Does this mean it is better to have a centre positioned throttle stick?

FengShuiDrone
Posts: 234
Joined: Wed Dec 24, 2014 1:20 am
Location: ......

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by FengShuiDrone »

.
Last edited by FengShuiDrone on Fri Aug 07, 2015 1:44 am, edited 1 time in total.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

dryflyer wrote:When I pitch or roll the quadcopter drops altitude. I assume this is because the sonar is angled away from the ground and it thinks that it needs to reduce height. It does restore correct altitude when I level out but do you have any idea how this can be overcome? There is some code in the MultiWii that applies a throttle boost when pitch or roll is applied to overcome this but it would be much better if used baro/sonar data instead.


I'll see what I can do with that. I'll let you know

dryflyer wrote:Can you expain how the controls work for the SONAR and SONAR/BARO altitude hold? I recall reading in your code that the altitude is adjusted by using throttle once the SONAR is active. Does this mean it is better to have a centre positioned throttle stick?


It would be better not to use the ALT_HOLD_THROTTLE_MIDPOINT which is set to 1500. The neutral zone would depend on when you activated the sonar then it will slowly go up or down when you change your throttle.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

FengShuiDrone wrote:Tried changing pins 11 and 12 to 9 and 10. Not working. There seems to be no trigger or echo happening. The sonar shows up on my GUI as active and I can switch it on and off but only in RC Control. The sonar shows active in the RC Control box but when it says it's on there, it is not lit up in the Flight Deck portion of the GUI. I can't hear it clicking or anything. My debug reads 1 on 1, random numbers up and down from negatives to plus 100's on debug 2, and the sensors graph follows accordingly. debug 3 and 4 have no values at all. I'm trying to get this to work on a RTFQ MW Pro2. Any suggestions out there?


debug 1 is the sonar
debug 2, barometer
debug 3 and 4 will only activates once it is armed and switched sonar mode

when testing, make sure to connect the battery as well so it can power up the motor pins.

FengShuiDrone
Posts: 234
Joined: Wed Dec 24, 2014 1:20 am
Location: ......

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by FengShuiDrone »

.
Last edited by FengShuiDrone on Fri Aug 07, 2015 1:45 am, edited 1 time in total.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

FengShuiDrone wrote:The only code I changed was in the config.h file, changing motor pins 11 and 12 to 9 and 10. Do I need to make more changes to other parts of the sketch to go along with the re assigned motor pins?


actually no, just change SONAR_GENERIC_TRIGGER_PIN and SONAR_GENERIC_ECHO_PIN

I saw this, and I think this is your board.
http://www.hobbyking.com/hobbyking/store/uploads/741171780X846863X13.pdf
4, 13, 11, 6, 5, 10, 9 are motor pins and quad uses
06\/05
10/\09

try to play with other pins A1, A0, 4, 13, 11

dryflyer
Posts: 10
Joined: Fri Apr 03, 2015 12:02 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by dryflyer »

Went for a fly tonight and I am loving the accuracy of altitude hold with SONAR. I don't have to worry about throttle anymore! So much more fun for a rookie pilot like me. Thank you!

dryflyer
Posts: 10
Joined: Fri Apr 03, 2015 12:02 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by dryflyer »

Can you please explain the difference between enabling BARO and enabling SONAR and enabling both?

FengShuiDrone
Posts: 234
Joined: Wed Dec 24, 2014 1:20 am
Location: ......

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by FengShuiDrone »

.
Last edited by FengShuiDrone on Fri Aug 07, 2015 1:45 am, edited 3 times in total.

Maine_Guy
Posts: 27
Joined: Fri Mar 06, 2015 7:07 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by Maine_Guy »

deleted - misleading
Last edited by Maine_Guy on Tue May 12, 2015 1:05 am, edited 1 time in total.

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by PatrikE »

dryflyer wrote:When I pitch or roll the quadcopter drops altitude. I assume this is because the sonar is angled away from the ground and it thinks that it needs to reduce height.


This code is already in IMU.cpp.
It's something people have played with to compensate Throttle/Tilt on Helicopters.
Maybe it can be used for sonar to compensate altitude with Angle.

Code: Select all

 #if defined(THROTTLE_ANGLE_CORRECTION)
    cosZ = mul(EstG.V16.Z , 100) / ACC_1G ;                                                   // cos(angleZ) * 100
    throttleAngleCorrection = THROTTLE_ANGLE_CORRECTION * constrain(100 - cosZ, 0, 100) >>3;  // 16 bit ok: 200*150 = 30000 
  #endif

ItsOnlyMe
Posts: 18
Joined: Sat Apr 18, 2015 4:25 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by ItsOnlyMe »

Hi Guys,

I finally got the Sonar working on my MultiWii Pro / MTK GPS Controller on PIN 9 and 10. (Pin 11/12 is not available on this board)
Example: http://www.hobbyking.com/hobbyking/stor ... 082X23.pdf
in config.h set

Code: Select all

#define SONAR_GENERIC_TRIGGER_PIN 9
#define SONAR_GENERIC_ECHO_PIN 10 

and in def.h change the follwing

Code: Select all

..
#define SONAR_GEP_TriggerPin_PIN_HIGH    PORTH |= 1<<6;    // WAS: PORTB |= 1<<6;
#define SONAR_GEP_TriggerPin_PIN_LOW     PORTH &= ~(1<<6); // WAS: PORTB &= ~(1<<6);
..
#define SONAR_GEP_EchoPin_PCINT          PCINT4            // WAS: PCINT5
..

up to about 2 meters this works really well. if i put a box or something on the floor and fly over it, the copter rises and falls accordingly.
Very well done.
BUT:
above the 2 or 3 Meters the copter rises with nearly full throttle and never holds his height.

I use one switch to switch on sonar & baro. is this correct, or must I use 2 separate switches?
I don't know if i understand #define SONAR_BARO_FUSION_RATIO correctly. What does this? 0.0 is only sonar, and 1.0 is only Baro? anything between is a mix of this 2 modes?

ItsOnlyMe
Posts: 18
Joined: Sat Apr 18, 2015 4:25 pm

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by ItsOnlyMe »

Just reviewed the recorded Onboard Video. Up to 2 Meters the OSD shows the height correctly. Above that it drops to 0 Meters and sometimes shows random numbers.
Also the Vario points full down.

short examples:



FengShuiDrone
Posts: 234
Joined: Wed Dec 24, 2014 1:20 am
Location: ......

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by FengShuiDrone »

.
Last edited by FengShuiDrone on Fri Aug 07, 2015 1:46 am, edited 1 time in total.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

Thanks for sharing @ItsOnlyMe! Glad @FengShuiDrone got it working as well!

FengShuiDrone
Posts: 234
Joined: Wed Dec 24, 2014 1:20 am
Location: ......

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by FengShuiDrone »

.
Last edited by FengShuiDrone on Fri Aug 07, 2015 1:46 am, edited 1 time in total.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: Sonar HC-SR04 Support now implemented in MW2.4

Post by jaysonragasa »

ItsOnlyMe wrote:Hi Guys,
I finally got the Sonar working on my MultiWii Pro / MTK GPS Controller on PIN 9 and 10. (Pin 11/12 is not available on this board)


What board did you define on config.h?
Does your board has BARO?

Post Reply