Page 1 of 3

Sonar HC-SR04 Support now implemented in MW2.4

Posted: Mon Mar 30, 2015 6:41 pm
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

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

Posted: Mon Mar 30, 2015 10:47 pm
by FengShuiDrone
.

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

Posted: Tue Mar 31, 2015 1:44 pm
by jaysonragasa
currently using CRIUS v2 AIOP. I'll post a video soon and the code ASAP.

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

Posted: Tue Mar 31, 2015 11:34 pm
by FengShuiDrone
.

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

Posted: Thu Apr 02, 2015 1:21 am
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

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

Posted: Thu Apr 02, 2015 1:27 am
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

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

Posted: Thu Apr 02, 2015 2:12 am
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.

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

Posted: Thu Apr 02, 2015 2:33 am
by jaysonragasa
I am not using stock codes for Sonar

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

Posted: Thu Apr 02, 2015 7:34 am
by jsjc
Do you mind sharing the changes in your code??

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

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

Posted: Fri Apr 03, 2015 12:26 am
by jaysonragasa
post updated. check very first post

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

Posted: Fri Apr 03, 2015 12:13 pm
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?

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

Posted: Fri Apr 03, 2015 1:04 pm
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!

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

Posted: Fri Apr 03, 2015 6:51 pm
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.

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

Posted: Fri Apr 03, 2015 8:53 pm
by jaysonragasa
I made an update on the codes. please check the first post

look for CODE UDPATE line

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

Posted: Fri Apr 03, 2015 11:05 pm
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();

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

Posted: Sat Apr 04, 2015 12:43 am
by jaysonragasa
@dryflyer - check update two now in my first post.

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

Posted: Sat Apr 04, 2015 3:44 am
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.

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

Posted: Sat Apr 04, 2015 3:53 am
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

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

Posted: Sat Apr 04, 2015 11:01 am
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??

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

Posted: Sat Apr 04, 2015 11:41 am
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
.

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

Posted: Sat Apr 04, 2015 11:52 am
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?

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

Posted: Sat Apr 04, 2015 2:17 pm
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

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

Posted: Sat Apr 04, 2015 3:18 pm
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

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

Posted: Sat Apr 04, 2015 11:36 pm
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.

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

Posted: Sat Apr 04, 2015 11:59 pm
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.

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

Posted: Sun Apr 05, 2015 12:09 am
by FengShuiDrone
.

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

Posted: Sun Apr 05, 2015 12:18 am
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

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

Posted: Sun Apr 05, 2015 5:07 am
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.

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

Posted: Sun Apr 05, 2015 6:15 am
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!

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

Posted: Sun Apr 05, 2015 1:28 pm
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?

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

Posted: Sun Apr 05, 2015 2:21 pm
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?

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

Posted: Sun Apr 05, 2015 11:27 pm
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

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

Posted: Mon Apr 06, 2015 12:19 am
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.

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

Posted: Mon Apr 06, 2015 7:48 am
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?

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

Posted: Tue Apr 07, 2015 2:25 am
by FengShuiDrone
.

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

Posted: Wed Apr 08, 2015 1:37 am
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.

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

Posted: Wed Apr 08, 2015 2:40 am
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.

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

Posted: Wed Apr 08, 2015 3:31 am
by FengShuiDrone
.

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

Posted: Wed Apr 08, 2015 5:38 am
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

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

Posted: Wed Apr 08, 2015 1:06 pm
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!

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

Posted: Wed Apr 08, 2015 1:46 pm
by dryflyer
Can you please explain the difference between enabling BARO and enabling SONAR and enabling both?

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

Posted: Wed Apr 08, 2015 1:54 pm
by FengShuiDrone
.

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

Posted: Wed Apr 08, 2015 5:58 pm
by Maine_Guy
deleted - misleading

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

Posted: Sat Apr 11, 2015 4:43 pm
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

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

Posted: Sat Apr 18, 2015 5:30 pm
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?

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

Posted: Sat Apr 18, 2015 7:41 pm
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:



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

Posted: Sun Apr 19, 2015 2:32 am
by FengShuiDrone
.

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

Posted: Sun Apr 19, 2015 3:04 am
by jaysonragasa
Thanks for sharing @ItsOnlyMe! Glad @FengShuiDrone got it working as well!

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

Posted: Sun Apr 19, 2015 3:23 am
by FengShuiDrone
.

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

Posted: Sun Apr 19, 2015 6:03 am
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?