[PATCH] in GPS.cpp if I2c_GPS_SONAR enabled

[justk1w1]

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

[err888]

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

[justk1w1]

How do we get "bug fix" changes implemented??

[Sebbi]

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.

[justk1w1]

Sebbi - thanks for that,

Not having the perfect world then what do we do?? Is there a method to submit code changes for review??

[copterrichie]

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.

[err888]

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

[copterrichie]

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]

[justk1w1]

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.

[timecop]

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/

[err888]

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:

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

[justk1w1]

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

[Pat1300]

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

[err888]

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.

GPS1548.zip

Diff file for patch to GPS.cpp rev1548

(854 Bytes) Downloaded 192 times

Here is also the patched GPS.cpp.

GPS1548+patched.zip

GPS.cpp rev1548 PATCHED

(14.04 KiB) Downloaded 194 times

Kenny

[Pat1300]

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:

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

[err888]

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:

Code: Select all

uint8_t *varptr;

in front of the block, finally change line 386 from this:

Code: Select all

uint8_t *varptr = (uint8_t *)&nav_bearing;

to this:

Code: Select all

varptr = (uint8_t *)&nav_bearing;

.

GPS.cpp.1548_patched_by_err888-2.zip

GPS.cpp rev1548 PATCHED

(14.04 KiB) Downloaded 184 times

GPS.cpp.1548-2.zip

Diff file for patch to GPS.cpp rev1548

(769 Bytes) Downloaded 200 times

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