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.
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_ */
#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
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
#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.
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;
#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
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
#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;
#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
// 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
#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
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
Back of the board
Re: Sonar HC-SR04 Support now implemented in MW2.4
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.
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
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
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
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.
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?