[PATCH] in GPS.cpp if I2c_GPS_SONAR enabled
[PATCH] in GPS.cpp if I2c_GPS_SONAR enabled
Just been working with I2C_GPS and added a Srf04 sonar, but found that once the I2C_GPS_SONAR is enabled it generates a lot of I2C errors. There appears to be an error in that if it is enabled a i2c NAK is not sent to close the I2c before the sonar code tries to open the I2c again.
The error is on line 442 of GPS.cpp. The sonar section needs to be move after the GPS_GROUND Course ( this is where the NAK is to close) then the code for the Sonar.
cheers
JK
SEE patch code from err888
The error is on line 442 of GPS.cpp. The sonar section needs to be move after the GPS_GROUND Course ( this is where the NAK is to close) then the code for the Sonar.
cheers
JK
SEE patch code from err888
Last edited by justk1w1 on Wed Aug 28, 2013 4:52 am, edited 2 times in total.
Re: Error in GPS.cpp if I2c_GPS_SONAR enabled
Great thanks to you justk1w1! You have helped me fixed my i2c gps nav board which was producing endless i2c errors. Now the sonar on my nav board behaves correctly as well!
Kenny
Kenny
Re: Error in GPS.cpp if I2c_GPS_SONAR enabled
How do we get "bug fix" changes implemented??
Re: Error in GPS.cpp if I2c_GPS_SONAR enabled
In perfect world you'd check out MultiWii_shared from the SVN trunk and produce a patch (file) that fixes the problem. Upload it here (git pull requests wont work here) and have some dev test/integrate it.
Re: Error in GPS.cpp if I2c_GPS_SONAR enabled
Sebbi - thanks for that,
Not having the perfect world then what do we do?? Is there a method to submit code changes for review??
Not having the perfect world then what do we do?? Is there a method to submit code changes for review??
-
- Posts: 2261
- Joined: Sat Feb 19, 2011 8:30 pm
Re: Error in GPS.cpp if I2c_GPS_SONAR enabled
justk1w1 wrote:Sebbi - thanks for that,
Not having the perfect world then what do we do?? Is there a method to submit code changes for review??
Put [PATCH] in the Subject line and attach or paste the code to a post. Hopefully EOSBandi will incorporate it into the i2c-gps-nav. He is the only one with access to Google Code.
Re: Error in GPS.cpp if I2c_GPS_SONAR enabled
copterrichie wrote:Hopefully EOSBandi will incorporate it into the i2c-gps-nav. He is the only one with access to Google Code.
The code concerned here is in Multiwii 2.2 - not i2c-gps-nav
Kenny
-
- Posts: 2261
- Joined: Sat Feb 19, 2011 8:30 pm
Re: Error in GPS.cpp if I2c_GPS_SONAR enabled
Sorry, but the same still applies. Hopefully Alex or someone will merge the code. If you post the code her, be sure to edit the first post by updating the subject line with [patch]
Re: Error in GPS.cpp if I2c_GPS_SONAR enabled
copterrichie wrote:Sorry, but the same still applies. Hopefully Alex or someone will merge the code. If you post the code her, be sure to edit the first post by updating the subject line with [patch]
Thanks all - I have edited the original post and inserted the patched code.
Re: Error in GPS.cpp if I2c_GPS_SONAR enabled
justk1w1 wrote:copterrichie wrote:Sorry, but the same still applies. Hopefully Alex or someone will merge the code. If you post the code her, be sure to edit the first post by updating the subject line with [patch]
Thanks all - I have edited the original post and inserted the patched code.
That's not a patch.
That's a dump of ~60k of code without any indication of what was changed or where.
What you need to do is a PATCH.
Example here:
http://ariejan.net/2007/07/03/how-to-cr ... ubversion/
Re: [PATCH] in GPS.cpp if I2c_GPS_SONAR enabled
I believe what justk1w1 mentioned about is the code at around line 424 in GPS.h rev.1548, which involves a move of the #if defined(i2c_gps_sonar) block to 5 lines below its original location, and put the 3 lines of code below the comment "//GPS_ground_course" above that #if defined block. See the resulting code below:
I have tested this patch and find it working.
However, after some fiddling, I have done a small change to make my i2c-gps-sonar starts working without requiring a GPS 3D fix. This is just to move the same #if defined block to line 350, and splitting the & condition of the "if (_i2c_gps_status & I2C_GPS_STATUS_3DFIX)" statement into 2 if statements etc. Thanks timecop for the instructions to create a patch file, and I attached the content of the diff file here for reference:
I have tested this one through debug mode and find it works as well, though not sure which part in the MultiWii code does a sonar unit works in - still reading thru the codes.
Hope this helps.
Kenny
Code: Select all
varptr = (uint8_t *)&GPS_altitude; // altitude in meters for OSD
*varptr++ = i2c_readAck();
*varptr = i2c_readAck();
//GPS_ground_course
varptr = (uint8_t *)&GPS_ground_course;
*varptr++ = i2c_readAck();
*varptr = i2c_readNak();
#if defined(I2C_GPS_SONAR)
i2c_rep_start(I2C_GPS_ADDRESS<<1);
i2c_write(I2C_GPS_SONAR_ALT);
i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
varptr = (uint8_t *)&sonarAlt; // altitude (in cm? maybe)
*varptr++ = i2c_readAck();
*varptr = i2c_readNak();
#endif
if (!f.GPS_FIX_HOME) { //If we don't have home set, do not display anything
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
I have tested this patch and find it working.
However, after some fiddling, I have done a small change to make my i2c-gps-sonar starts working without requiring a GPS 3D fix. This is just to move the same #if defined block to line 350, and splitting the & condition of the "if (_i2c_gps_status & I2C_GPS_STATUS_3DFIX)" statement into 2 if statements etc. Thanks timecop for the instructions to create a patch file, and I attached the content of the diff file here for reference:
Code: Select all
Index: GPS.cpp
===================================================================
--- GPS.cpp (revision 1543)
+++ GPS.cpp (working copy)
@@ -348,124 +348,129 @@
GPS_numSat = (_i2c_gps_status & 0xf0) >> 4;
_i2c_gps_status = i2c_readReg(I2C_GPS_ADDRESS,I2C_GPS_STATUS_00); //Get status register
- if (_i2c_gps_status & I2C_GPS_STATUS_3DFIX) { //Check is we have a good 3d fix (numsats>5)
- f.GPS_FIX = 1;
+ if (_i2c_gps_status) {
- #if !defined(DONT_RESET_HOME_AT_ARM)
- if (!f.ARMED) {f.GPS_FIX_HOME = 0;} //Clear home position if disarmed
- #endif
+ uint8_t *varptr;
- if (!f.GPS_FIX_HOME && f.ARMED) { //if home is not set set home position to WP#0 and activate it
- GPS_reset_home_position();
- }
- if (_i2c_gps_status & I2C_GPS_STATUS_NEW_DATA) { //Check about new data
- if (GPS_update) { GPS_update = 0;} else { GPS_update = 1;} //Fancy flash on GUI :D
- if (!GPS_pids_initialized) {
- GPS_set_pids();
- GPS_pids_initialized = 1;
- }
-
- //Read GPS data for distance, heading and gps position
+#if defined(I2C_GPS_SONAR)
+ i2c_rep_start(I2C_GPS_ADDRESS<<1);
+ i2c_write(I2C_GPS_SONAR_ALT);
+ i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
- i2c_rep_start(I2C_GPS_ADDRESS<<1);
- i2c_write(I2C_GPS_NAV_BEARING); //Start read from here 2x2 bytes distance and direction
- i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
+ varptr = (uint8_t *)&sonarAlt; // altitude (in cm? maybe)
+ *varptr++ = i2c_readAck();
+ *varptr = i2c_readNak();
+#endif
- uint8_t *varptr = (uint8_t *)&nav_bearing;
- *varptr++ = i2c_readAck();
- *varptr = i2c_readAck();
+ if (I2C_GPS_STATUS_3DFIX) { //Check if we have a good 3d fix (numsats>5)
+ f.GPS_FIX = 1;
- varptr = (uint8_t *)&GPS_directionToHome;
- *varptr++ = i2c_readAck();
- *varptr = i2c_readAck();
- GPS_directionToHome = GPS_directionToHome / 100; // 1deg =1000 in the reg, downsize
- GPS_directionToHome += 180; // fix (see http://www.multiwii.com/forum/viewtopic.php?f=8&t=2892)
- if (GPS_directionToHome>180) GPS_directionToHome -= 360;
+ #if !defined(DONT_RESET_HOME_AT_ARM)
+ if (!f.ARMED) {f.GPS_FIX_HOME = 0;} //Clear home position if disarmed
+ #endif
- varptr = (uint8_t *)&GPS_distanceToHome;
- *varptr++ = i2c_readAck();
- *varptr = i2c_readNak();
- GPS_distanceToHome = GPS_distanceToHome / 100; //register is in CM, we need in meter. max= 655 meters with this way
+ if (!f.GPS_FIX_HOME && f.ARMED) { //if home is not set set home position to WP#0 and activate it
+ GPS_reset_home_position();
+ }
+ if (_i2c_gps_status & I2C_GPS_STATUS_NEW_DATA) { //Check about new data
+ if (GPS_update) { GPS_update = 0;} else { GPS_update = 1;} //Fancy flash on GUI :D
+ if (!GPS_pids_initialized) {
+ GPS_set_pids();
+ GPS_pids_initialized = 1;
+ }
- i2c_rep_start(I2C_GPS_ADDRESS<<1);
- i2c_write(I2C_GPS_LOCATION); //Start read from here 2x2 bytes distance and direction
- i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
+ //Read GPS data for distance, heading and gps position
- varptr = (uint8_t *)&GPS_coord[LAT]; // for latitude displaying
- *varptr++ = i2c_readAck();
- *varptr++ = i2c_readAck();
- *varptr++ = i2c_readAck();
- *varptr = i2c_readAck();
+ i2c_rep_start(I2C_GPS_ADDRESS<<1);
+ i2c_write(I2C_GPS_NAV_BEARING); //Start read from here 2x2 bytes distance and direction
+ i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
- varptr = (uint8_t *)&GPS_coord[LON]; // for longitude displaying
- *varptr++ = i2c_readAck();
- *varptr++ = i2c_readAck();
- *varptr++ = i2c_readAck();
- *varptr = i2c_readAck();
+ varptr = (uint8_t *)&nav_bearing;
+ *varptr++ = i2c_readAck();
+ *varptr = i2c_readAck();
- varptr = (uint8_t *)&nav[LAT];
- *varptr++ = i2c_readAck();
- *varptr++ = i2c_readAck();
+ varptr = (uint8_t *)&GPS_directionToHome;
+ *varptr++ = i2c_readAck();
+ *varptr = i2c_readAck();
+ GPS_directionToHome = GPS_directionToHome / 100; // 1deg =1000 in the reg, downsize
+ GPS_directionToHome += 180; // fix (see http://www.multiwii.com/forum/viewtopic.php?f=8&t=2892)
+ if (GPS_directionToHome>180) GPS_directionToHome -= 360;
- varptr = (uint8_t *)&nav[LON];
- *varptr++ = i2c_readAck();
- *varptr++ = i2c_readNak();
-
- i2c_rep_start(I2C_GPS_ADDRESS<<1);
- i2c_write(I2C_GPS_GROUND_SPEED);
- i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
+ varptr = (uint8_t *)&GPS_distanceToHome;
+ *varptr++ = i2c_readAck();
+ *varptr = i2c_readNak();
+ GPS_distanceToHome = GPS_distanceToHome / 100; //register is in CM, we need in meter. max= 655 meters with this way
- varptr = (uint8_t *)&GPS_speed; // speed in cm/s for OSD
- *varptr++ = i2c_readAck();
- *varptr = i2c_readAck();
+ i2c_rep_start(I2C_GPS_ADDRESS<<1);
+ i2c_write(I2C_GPS_LOCATION); //Start read from here 2x2 bytes distance and direction
+ i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
- varptr = (uint8_t *)&GPS_altitude; // altitude in meters for OSD
- *varptr++ = i2c_readAck();
- *varptr = i2c_readAck();
-
-#if defined(I2C_GPS_SONAR)
- i2c_rep_start(I2C_GPS_ADDRESS<<1);
- i2c_write(I2C_GPS_SONAR_ALT);
- i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
+ varptr = (uint8_t *)&GPS_coord[LAT]; // for latitude displaying
+ *varptr++ = i2c_readAck();
+ *varptr++ = i2c_readAck();
+ *varptr++ = i2c_readAck();
+ *varptr = i2c_readAck();
- varptr = (uint8_t *)&sonarAlt; // altitude (in cm? maybe)
- *varptr++ = i2c_readAck();
- *varptr = i2c_readNak();
-#endif
+ varptr = (uint8_t *)&GPS_coord[LON]; // for longitude displaying
+ *varptr++ = i2c_readAck();
+ *varptr++ = i2c_readAck();
+ *varptr++ = i2c_readAck();
+ *varptr = i2c_readAck();
- //GPS_ground_course
- varptr = (uint8_t *)&GPS_ground_course;
- *varptr++ = i2c_readAck();
- *varptr = i2c_readNak();
+ varptr = (uint8_t *)&nav[LAT];
+ *varptr++ = i2c_readAck();
+ *varptr++ = i2c_readAck();
- if (!f.GPS_FIX_HOME) { //If we don't have home set, do not display anything
- GPS_distanceToHome = 0;
- GPS_directionToHome = 0;
- }
+ varptr = (uint8_t *)&nav[LON];
+ *varptr++ = i2c_readAck();
+ *varptr++ = i2c_readNak();
- //Adjust heading when navigating
- if (f.GPS_HOME_MODE) {
- if ( !(_i2c_gps_status & I2C_GPS_STATUS_WP_REACHED) ) {
- //Tail control
- if (NAV_CONTROLS_HEADING) {
- if (NAV_TAIL_FIRST) {
- magHold = nav_bearing/100-180;
- if (magHold > 180) magHold -= 360;
- if (magHold < -180) magHold += 360;
- } else {
- magHold = nav_bearing/100;
+ i2c_rep_start(I2C_GPS_ADDRESS<<1);
+ i2c_write(I2C_GPS_GROUND_SPEED);
+ i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
+
+ varptr = (uint8_t *)&GPS_speed; // speed in cm/s for OSD
+ *varptr++ = i2c_readAck();
+ *varptr = i2c_readAck();
+
+ varptr = (uint8_t *)&GPS_altitude; // altitude in meters for OSD
+ *varptr++ = i2c_readAck();
+ *varptr = i2c_readAck();
+
+ //GPS_ground_course
+ varptr = (uint8_t *)&GPS_ground_course;
+ *varptr++ = i2c_readAck();
+ *varptr = i2c_readNak();
+
+ if (!f.GPS_FIX_HOME) { //If we don't have home set, do not display anything
+ GPS_distanceToHome = 0;
+ GPS_directionToHome = 0;
+ }
+
+ //Adjust heading when navigating
+ if (f.GPS_HOME_MODE) {
+ if ( !(_i2c_gps_status & I2C_GPS_STATUS_WP_REACHED) ) {
+ //Tail control
+ if (NAV_CONTROLS_HEADING) {
+ if (NAV_TAIL_FIRST) {
+ magHold = nav_bearing/100-180;
+ if (magHold > 180) magHold -= 360;
+ if (magHold < -180) magHold += 360;
+ } else {
+ magHold = nav_bearing/100;
+ }
}
+ } else { //Home position reached
+ if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; }
}
- } else { //Home position reached
- if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; }
}
}
+ } else { //We don't have a fix zero out distance and bearing (for safety reasons)
+ GPS_distanceToHome = 0;
+ GPS_directionToHome = 0;
+ GPS_numSat = 0;
+ f.GPS_FIX = 0;
}
- } else { //We don't have a fix zero out distance and bearing (for safety reasons)
- GPS_distanceToHome = 0;
- GPS_directionToHome = 0;
- GPS_numSat = 0;
- f.GPS_FIX = 0;
}
#endif
I have tested this one through debug mode and find it works as well, though not sure which part in the MultiWii code does a sonar unit works in - still reading thru the codes.
Hope this helps.
Kenny
Re: [PATCH] in GPS.cpp if I2c_GPS_SONAR enabled
Kenny - thanks for that - good pickup on the conditional statements, want the sonar data to come through no matter what the status of the GPS. I've actually replace the code in the I2C_gps to use a counter timer with overflow vector to stabalise the sonar output and also added code to accommodate additional rx channels so that I can use a full 4 AUX channels, through the I2C_GPS, on a 328P board that does have the rx inputs available.
The current code release only uses the sonar alt to control the landing lights, however there are a couple of branches that have code for sonar/baro fusion which I have taken info from and am currently testing.
I think it is in the list of requests for V2.3
Timecop - thanks for the instructions to create a patch file
The current code release only uses the sonar alt to control the landing lights, however there are a couple of branches that have code for sonar/baro fusion which I have taken info from and am currently testing.
I think it is in the list of requests for V2.3
Timecop - thanks for the instructions to create a patch file
Re: [PATCH] in GPS.cpp if I2c_GPS_SONAR enabled
Hi Kenny,
I have some issues when I try to apply your patch/diff.
Could you please also upload your modified GPS.CPP file ?
Thanks,
Patrick
I have some issues when I try to apply your patch/diff.
Could you please also upload your modified GPS.CPP file ?
Thanks,
Patrick
Re: [PATCH] in GPS.cpp if I2c_GPS_SONAR enabled
justk1w1, I haven't get that far, and am just trying to complete my little 240mm quad project using Crius MultiWii SE 1.0 and the i2c_gps_nav board with sonar module. Are you trying to connect more rx channels via the nav board? That's interesting
Would you mind let me know which branches you prefer the most on the best sonar/baro fusion, can't wait to give it a try.
Pat1300, the change is in fact very minor, but the comparison function under eclipse seems a little bit dumb - it picks up lines of identical codes with differences of only spaces and tabs and generates a big diff file. I have created another diff file which should work better. Please patch it to rev 1548 from the trunk version.
Here is also the patched GPS.cpp.
Kenny
Would you mind let me know which branches you prefer the most on the best sonar/baro fusion, can't wait to give it a try.
Pat1300, the change is in fact very minor, but the comparison function under eclipse seems a little bit dumb - it picks up lines of identical codes with differences of only spaces and tabs and generates a big diff file. I have created another diff file which should work better. Please patch it to rev 1548 from the trunk version.
Here is also the patched GPS.cpp.
Kenny
Re: [PATCH] in GPS.cpp if I2c_GPS_SONAR enabled
Hi Kenyy,
Thank you for your new .diff and patched gps.cpp files.
I think there is an error in your modifications in gps.cpp, line 362:
if (I2C_GPS_STATUS_3DFIX) {
...
Because I2C_GPS_STATUS_3DFIX is a constant not null, the if condition is always true, and the consequent instructions are always executed.
Correct line 362 should be:
if (_i2c_gps_status & I2C_GPS_STATUS_3DFIX) {
So , the code should be:
Patrick
Thank you for your new .diff and patched gps.cpp files.
I think there is an error in your modifications in gps.cpp, line 362:
if (I2C_GPS_STATUS_3DFIX) {
...
Because I2C_GPS_STATUS_3DFIX is a constant not null, the if condition is always true, and the consequent instructions are always executed.
Correct line 362 should be:
if (_i2c_gps_status & I2C_GPS_STATUS_3DFIX) {
So , the code should be:
Code: Select all
...
if (I2C_GPS_STATUS_3DFIX) { //Check is we have a good 3d fix (numsats>5)
f.GPS_FIX = 1;
#if !defined(DONT_RESET_HOME_AT_ARM)
if (!f.ARMED) {f.GPS_FIX_HOME = 0;} //Clear home position if disarmed
#endif
if (!f.GPS_FIX_HOME && f.ARMED) { //if home is not set set home position to WP#0 and activate it
GPS_reset_home_position();
}
if (_i2c_gps_status & I2C_GPS_STATUS_NEW_DATA) { //Check about new data
if (GPS_update) { GPS_update = 0;} else { GPS_update = 1;}
...
Patrick
Re: [PATCH] in GPS.cpp if I2c_GPS_SONAR enabled
Ha, you're correct Patrick. This is such a low lever mistake I made, see how new I am in C++...
After a look again, it seems that the & operator in the concerned if statement is just to perform a binary AND manipulation between the variable _i2c_gps_status and the constant I2C_GPS_STATUS_3DFIX, and will let go only if the result is bigger than 0, which will happen only if _i2c_gps_status equals to 0x04, 0x05, 0x06, 0x07, 0x0C, 0x0D, 0x0E, or 0x0F... etc., and I think 0x04 is the only value that the if statement is looking for. My earlier modification can reach my intention, but will also make MultiWii always thinks it got a 3D fix, which is another bug, sorry
Let's start all over again. The simplest solution should be even simpler, just move the whole block of #if defined(i2c_gps_sonar) code that justk1w1 concerned up in front of line 349 in GPS.cpp rev1548, and put this line: in front of the block, finally change line 386 from this:to this: .
More thorough solution should require looking into possible values of _i2c_gps_status and see if it is meaningful to the sonar status of the i2c_gps_nav board, to determine whether to put a conditional statement over the block of #if defined(i2c_gps_sonar) code or not.
Hope this rectifies something at least
Kenny
After a look again, it seems that the & operator in the concerned if statement is just to perform a binary AND manipulation between the variable _i2c_gps_status and the constant I2C_GPS_STATUS_3DFIX, and will let go only if the result is bigger than 0, which will happen only if _i2c_gps_status equals to 0x04, 0x05, 0x06, 0x07, 0x0C, 0x0D, 0x0E, or 0x0F... etc., and I think 0x04 is the only value that the if statement is looking for. My earlier modification can reach my intention, but will also make MultiWii always thinks it got a 3D fix, which is another bug, sorry
Let's start all over again. The simplest solution should be even simpler, just move the whole block of #if defined(i2c_gps_sonar) code that justk1w1 concerned up in front of line 349 in GPS.cpp rev1548, and put this line:
Code: Select all
uint8_t *varptr;
Code: Select all
uint8_t *varptr = (uint8_t *)&nav_bearing;
Code: Select all
varptr = (uint8_t *)&nav_bearing;
More thorough solution should require looking into possible values of _i2c_gps_status and see if it is meaningful to the sonar status of the i2c_gps_nav board, to determine whether to put a conditional statement over the block of #if defined(i2c_gps_sonar) code or not.
Hope this rectifies something at least
Kenny