View Single Post
Old Apr 25, 2010, 02:10 PM
HappyKillmore is offline
Find More Posts by HappyKillmore
Suspended Account
Brunswick, OH
Joined Nov 2005
5,547 Posts
Step 3 -Getting the ArduIMU to work with the Remzibi GPS unit.

This step will require your Remzibi NMEA GPS, ArduIMU and your FTDI cable. Our mission is to get the GPS talking to the ArduIMU and passing all of the roll, pitch and yaw data along with the GPS lat, long, speed, etc all in one message. We're working towards getting everything working using the "new" binary protocol to talk between the ArduIMU and the ArduPilot.

So solder your headers on the IMU board and connect everything as shown below:



The pins on the top edge of the board are for an optional magnetometer which can be added to increase the accuracy (and remove drift) of the IMU.

Next, download Doug Weibel's code from the ArduIMU Download Page. At the time of this writing, v1.7 was the most current http://ardu-imu.googlecode.com/files/ArduIMU_1.7.zip

Unzip the contents into a folder called C:\Projects\ArduIMU

Double-click on any of the .pde files (ie arduimu.pde)

Once the editor opens, you're going to need to edit a few of the settings in the arduimu tab (far left)

Code:
#define BOARD_VERSION 2 // 1 For V1 and 2 for V2

// Ublox gps is recommended!
#define GPS_PROTOCOL 3    // 1 - Ublox,  2 - EM406,  3 - NMEA    We have only tested with Ublox

// Enable Air Start uses Remove Before Fly flag - connection to pin 6 on ArduPilot 
#define ENABLE_AIR_START 1  //  1 if using airstart/groundstart signaling, 0 if not
#define GROUNDSTART_PIN 8    //  Pin number used for ground start signal (recommend 10 on v1 and 8 on v2 hardware)

/*Min Speed Filter for Yaw drift Correction*/
#define SPEEDFILT 2 // >1 use min speed filter for yaw drift cancellation, 0=do not use speed filter

/*For debugging propurses*/
#define PRINT_DEBUG 0   //Will print Debug messages

//OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data
#define OUTPUTMODE 1

#define PRINT_DCM 0     //Will print the whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
#define PRINT_EULER 1   //Will print the Euler angles Roll, Pitch and Yaw
#define PRINT_GPS 1     //Will print GPS data

// *** NOTE!   To use ArduIMU with ArduPilot you must select binary output messages
#define PRINT_BINARY 1   //Will print binary message and suppress ASCII messages (above)

// *** NOTE!   Performance reporting is only supported for Ublox.  Set to 0 for others
#define PERFORMANCE_REPORTING 1  //Will include performance reports in the binary output ~ 1/2 min

/* Support for optional magnetometer (1 enabled, 0 dissabled) */
#define USE_MAGNETOMETER 0 // use 1 if you want to make yaw gyro drift corrections using the optional magnetometer                   

/* Support for optional barometer (1 enabled, 0 dissabled) */
#define USE_BAROMETER 0 	// use 1 if you want to get altitude using the optional absolute pressure sensor                  
#define ALT_MIX	50			// For binary messages: GPS or barometric altitude.  0 to 100 = % of barometric.  For example 75 gives 25% GPS alt and 75% baro
In that same tab scroll about 1/2 way down to the function "void setup()"

Replace the function from the lin void setup() down to "debug_print("Welcome...");" with the text below.
Code:
void setup()
{ 
  #if GPS_CONNECTION == 0
      pinMode(2,OUTPUT); //Serial Mux
      digitalWrite(2,HIGH); //Serial Mux
  #endif
  //Serial.begin(38400);
  
  //Serial.begin(57600);
  pinMode(5,OUTPUT); //Red LED
  pinMode(6,OUTPUT); // Blue LED
  pinMode(7,OUTPUT); // Yellow LED
  pinMode(GROUNDSTART_PIN,INPUT);  // Remove Before Fly flag (pin 6 on ArduPilot)
  digitalWrite(GROUNDSTART_PIN,HIGH);

  init_gps();
  
  Analog_Reference(EXTERNAL);//Using external analog reference
  Analog_Init();
  
  debug_print("Welcome...");
Next, click the tab for GPS_NMEA

Highlight all the way down to the end of the init_gps function (right above where it says void decode_gps(void) and delete everything you just highlighted. Replace it with the code below. I have added some more options in the PMTK strings that don't exist in Doug's online version.
Code:
#if GPS_PROTOCOL == 3

// LED Status indication
//----------------------
// Blue + Red flash twice = Sending GPS configuration commands
// Blue flashing = Receiving data from GPS but no fix lock yet (flash rate shows hertz/messages per second)
// Blue solid = GPS fix locked
// Red solid = Gyro saturation
// Yellow solid = Speed too slow and yaw correction supressed

//*********************************************************************************************
//  You may need to insert parameters appropriate for your gps from this list into init_gps()
//GPS SIRF configuration strings... 
#define SIRF_BAUD_RATE_4800    "$PSRF100,1,4800,8,1,0*0E\r\n"
#define SIRF_BAUD_RATE_9600    "$PSRF100,1,9600,8,1,0*0D\r\n"
#define SIRF_BAUD_RATE_19200    "$PSRF100,1,19200,8,1,0*38\r\n"
#define SIRF_BAUD_RATE_38400    "$PSRF100,1,38400,8,1,0*3D\r\n"  
#define SIRF_BAUD_RATE_57600    "$PSRF100,1,57600,8,1,0*36\r\n"
#define GSA_ON   "$PSRF103,2,0,1,1*27\r\n"   // enable GSA
#define GSA_OFF  "$PSRF103,2,0,0,1*26\r\n"   // disable GSA
#define GSV_ON   "$PSRF103,3,0,1,1*26\r\n"  // enable GSV
#define GSV_OFF  "$PSRF103,3,0,0,1*27\r\n"  // disable GSV
#define USE_WAAS   1     //1 = Enable, 0 = Disable, good in USA, slower FIX... 
#define WAAS_ON    "$PSRF151,1*3F\r\n"       // enable WAAS
#define WAAS_OFF   "$PSRF151,0*3E\r\n"       // disable WAAS

//GPS Locosys configuration strings...
#define USE_SBAS 0
#define SBAS_ON "$PMTK313,1*2E\r\n"
#define SBAS_OFF "$PMTK313,0*2F\r\n"

#define DGPS_OFF "$PMTK301,0*2C\r\n"
#define DGPS_RTCM "$PMTK301,1*2D\r\n"
#define DGPS_SBAS "$PMTK301,2*2E\r\n"

#define DATUM_GOOGLE "$PMTK330,0*2E\r\n"

#define NMEA_OUTPUT_5HZ "$PMTK314,0,5,0,5,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 5HZ  
#define NMEA_OUTPUT_4HZ "$PMTK314,0,4,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 4HZ 
#define NMEA_OUTPUT_3HZ "$PMTK314,0,3,0,3,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 3HZ 
#define NMEA_OUTPUT_2HZ "$PMTK314,0,2,0,2,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 2HZ 
#define NMEA_OUTPUT_1HZ "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 1HZ

#define LOCOSYS_REFRESH_RATE_100 "$PMTK220,100*2F\r\n" //100 milliseconds 
#define LOCOSYS_REFRESH_RATE_200 "$PMTK220,200*2C\r\n" //200 milliseconds 
#define LOCOSYS_REFRESH_RATE_250 "$PMTK220,250*29\r\n" //250 milliseconds
#define LOCOSYS_REFRESH_RATE_333 "$PMTK220,333*2D\r\n" //500 milliseconds
#define LOCOSYS_REFRESH_RATE_500 "$PMTK220,500*2B\r\n" //500 milliseconds
#define LOCOSYS_REFRESH_RATE_1000 "$PMTK220,1000*1F\r\n" //500 milliseconds

#define LOCOSYS_BAUD_RATE_4800 "$PMTK251,4800*14\r\n"
#define LOCOSYS_BAUD_RATE_9600 "$PMTK251,9600*17\r\n"
#define LOCOSYS_BAUD_RATE_19200 "$PMTK251,19200*22\r\n"
#define LOCOSYS_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
#define LOCOSYS_BAUD_RATE_57600 "$PMTK251,57600*2C\r\n"
#define LOCOSYS_BAUD_RATE_115200 "$PMTK251,115200*1F\r\n"

//**************************************************************************************************************

/****************************************************************
 Parsing stuff for NMEA
 ****************************************************************/
#define BUF_LEN 200

// GPS Pointers
char *token;
char *search = ",";
char *brkb, *pEnd;
char gps_buffer[BUF_LEN]; //The traditional buffer.

void init_gps(void)
{
	Serial.begin(38400); //This value must be the original "default" GPS speed
	delay(2000);
        digitalWrite(5,HIGH); // Turn Red LED ON to indicate PMTK strings sending
        digitalWrite(6,HIGH); // Turn Blue LED ON to indicate PMTK strings sending
	Serial.println(LOCOSYS_BAUD_RATE_38400); //This is your desired speed
	delay(500);
        digitalWrite(5,LOW); // Turn Red LED OFF to indicate PMTK strings sending
        digitalWrite(6,LOW); // Turn Blue LED OFF to indicate PMTK strings sending
        Serial.end();
	Serial.begin(38400); //This is your desired speed
	delay(500);
        digitalWrite(5,HIGH); // Turn Red LED ON to indicate PMTK strings sending
        digitalWrite(6,HIGH); // Turn Blue LED ON to indicate PMTK strings sending
	Serial.println(LOCOSYS_REFRESH_RATE_200); // Edit this to change Hertz. 100 = 10Hz, 200 = 5Hz, 250 = 4Hz
	Serial.println(NMEA_OUTPUT_1HZ); // Don't edit this
	Serial.println(SBAS_ON); 
	Serial.println(DGPS_SBAS);
	Serial.println(DATUM_GOOGLE);
	delay(500);
        digitalWrite(5,LOW); // Turn Red LED OFF to indicate PMTK strings sending
        digitalWrite(6,LOW); // Turn Blue LED OFF to indicate PMTK strings sending

/* EM406 example init
	Serial.begin(4800); //Universal Sincronus Asyncronus Receiveing Transmiting 
	delay(1000);
	Serial.print(SIRF_BAUD_RATE_9600);
 
	Serial.begin(9600);
	delay(1000);
	
	Serial.print(GSV_OFF);
	Serial.print(GSA_OFF);
	
	#if USE_WAAS == 1
	Serial.print(WAAS_ON);
	#else
	Serial.print(WAAS_OFF);
	#endif*/
}
Once you've got that code pasted in, look for the line that reads "Serial.begin(38400);" If you have one of the older Remzibi units, your GPS may default to 9600 baud. You can tell when you first start up the OSD and you're looking at the monitor/goggles it will tell you what baud rate it's using. Newer units will use 38400 baud by default.

Simply make sure the line "Serial.begin(38400);" is filled in with the default speed of your GPS.

Next, edit the lines "Serial.println(LOCOSYS_BAUD_RATE_57600);" and "Serial.begin(57600);" to be whatever speed you'd like to use.

I recommend 38400 or 19200. You can try 57600 if you want, but I'd strongly suggest not trying 115200 in flight. There will be far too much noise in the plane to use this baud rate without shielding on your wires (or that is my assumption anyhow).

NOTE: If you mess around with your baud rate and you're setting the "default" value of your GPS to something else, you'll need to unplug your GPS or FTDI cable (remove power) to the GPS to get it to recognize the new desired baud rate. It's a bit confusing, but if you change the default from 9600 to 19200, for example, and then decide you want to change it to 38400, disconnect power to the GPS first. You NEED to use the default that's burned in the memory on the GPS (ie 9600 or 38400) as the starting value or when you get out in the field, it won't work right. To get it back to the factory defaults, pull the power.

You can try editing the LOCOSYS_REFRESH_RATE_200 value if you want. In theory, this command determines how often the GPS messages get sent. 100 = every 100ms or 10 times a second = 10hz. 200 = 200ms or 5 times a second = 5hz. 250 = 4hz. Those are the only 3 options I have in there right now but if you need more options, let me know and I'll calculate the checksum and post them here. In practice, you won't get anywhere near this rate even with 115200 baud... so I'd stick with 200 or 250.

Finally, you can edit NMEA_OUTPUT_1HZ if you'd like. The naming convention for this variable is a bit deceiving since these settings do not determine the Hz. These settings have to do with how often each message gets sent. The 1HZ setting means all messages get sent every LOCOSYS_REFRESH_RATE_200 milliseconds (in the case of 200). On the 2Hz setting, all messages will get sent every 200ms X 2 = 400ms. It's a bit confusing, but it would allow you to have the GPS send different messages at different intervals. In our case, we just want them as fast as we can get them and we want both messages (GPRMC and GPGGA) every cycle.

Make sure to save your work and upload to the ArduIMU.

Next, download the latest version of my software found here: http://www.happykillmore.com/Softwar...etup/Setup.exe and run the ArduIMU Test application.

Make sure you have selected the right COM port and baud rate (the baud rate you specified on the GPS_NMEA tab Serial.begin(57600); //This is your desired speed).

Your output should look something like what is shown below:



Your blue light "should" be on solid on your ArduIMU (give it at least 3 minutes!!) and you should be able move the ArduIMU and see the plane move on the screen. The performance data is updated every 20 seconds by default except the "health" field which gets updated every cycle.

You'll notice the "GPS messages" field near the bottom right. This is how many messages came in over 20 seconds. The best I've seen here in my office is 42 in 20 secs or just over 2 per second with 38400 baud, LOCOSYS_REFRESH_RATE_200, NMEA_OUTPUT_1HZ, SBAS_ON, DGPS_SBAS.
HappyKillmore is offline Find More Posts by HappyKillmore
Last edited by HappyKillmore; May 04, 2010 at 10:38 PM.
Reply With Quote  (Disabled)