Possible memory problem (ArduPilot)

Hello everyone! I am having trouble with some code that I am working on, and I will do my best to provide enough background on the project without being needlessly long.

I am trying to modify existing code for an open source autopilot based on the Arduino, called the ArduPilot. The existing code has working GPS, navigation, one way telemetry and everything else needed to fly. I am trying to implement two way telemetry on an extra serial line using a pair of XBees. The goal of the two way system is to be able to request telemetry data (attitude, GPS, etc) as well as modify way points and maybe some stuff we haven't thought of yet.

Long story short, I have almost everything working (in the lab atleast). The original waypoint system is read from the EEPROM, which is "filled" with way points on the ground. I felt that we should also have a waypoint list not stored on the EEPROM, and that the EEPROM should not be modified with commands from the ground station, so we need a new list of waypoints. This new list was easy to impliment with latitude and longitude, but when I tried to add another array for the altitudes, bad things happen.

So, hopefully that is enough background, now to actual question. When I try to WRITE to the array called "wp_alts" all of my serial communication goes screwey, where it seems that random things are omitted and added. The following is the code that takes in and parses the serial data, trying to pull out commands.

void buffIt(){           //this takes the serial and turns it into a buffer, which must end in $, and isnt too large
  i = 0;
  while(cmd != '

&& mySerial.available() != 0 && i < 100){          //while the string is small enough and the end command isnt sent fill up the buffer (also need to have something left to lo
     buff[i] = mySerial.read();                                      //filling it up
   cmd = buff[i];
   i++;
 }
 while(cmd == '


){             //when the transmission is finished
    mySerial.println("");
    mySerial.println("Successful transmission of:");    //debugging
    mySerial.println(buff);
    rdy = 1;                        //flag that the buffer is good and ready to be processed
    cmd = mySerial.read();          //no clue why this is here....
    i = 0;
  }
  if (i >= 100){                //usually means we missed a $ or crappy data is coming in
    mySerial.println("Careful, buffer too big");
    mySerial.println(buff);
    clearBuff();
  }




}

void saveIt(){      //give the starting location in the buffer and it will save the new wp according to the syntax used throughout AND STORE
  get_wp();
                  mySerial.print("wp:");
                  mySerial.println(wpnum);
                  len = 0;
                  i++;
                  get_lati();
                  mySerial.print("New lat command:");
                  mySerial.println(lati);
                  len = 0;
                  i++;
                  get_loni();
                  mySerial.print("New lon command:");
                  mySerial.println(loni);
                  len = 0;
                  i++;
                  get_alti();
                  mySerial.print("New alt command:");
                  mySerial.println(alti);
                  clearBuff();
                   wp_lons[wpnum] = loni;
                  wp_lats[wpnum] = lati;
                // wp_alts[wpnum] = lati;
                  i = 0;
}

void Print_Waypoint_flash(int val){     //prints the wp you request
  wpnum = val;
 // i = val;
 //i = 3;
  //len = 0;
  //get_wp();       //pointless???
  mySerial.print("wp:");
  mySerial.println(wpnum);
  mySerial.println("Printing Waypoints");
  mySerial.println(wp_lats[wpnum]);
  mySerial.println(wp_lons[wpnum]);


}


void clearBuff(){
  i = 0;
  while(i <= 150){
    buff[i] = 0;
    i++;
  }
  i = 0;
  rdy = 0;
}   

void get_wp(){    //this gets a two digit (3) number and doesnt return it, but rather saves it to wpnum
  len = 0;
  while(buff[i] != ',' && len < 2)
  {
    wp[len] = buff[i];
    i++;
    len++;  
  }
  wp[len] = '\0';
  wpnum = atof(wp);
}


void get_lati(){   //uses the lati working variable
  while(buff[i] != ',' && len < 10)
  {
    lats[len] = buff[i];
    i++;
    len++;  
  }
  lats[len] = '\0';
  lati = atof(lats);
  lati /= pow(10,(len-2));

}


void get_loni(){
  while(buff[i] != ',' && len < 10)
  {
    lons[len] = buff[i];
    i++;
    len++;  
  }
  lons[len] = '\0';
  loni = atof(lons);
  loni /= pow(10,(len-3));

}

void get_alti(){
while(buff[i] != ',' && len < 10)
  {
    alts[len] = buff[i];
    i++;
    len++;  
  }
  alts[len] = '\0';
  alti = atof(alts);
  alti /= pow(10,(len-3));
}

void insertWP(){
  get_wp();
  mySerial.print("WP to insert to: ");
  mySerial.println(wpnum);
  i = 100;
  while(i > wpnum){
    wp_lats[i] = wp_lats[i-1];
    wp_lons[i] = wp_lons[i-1];
    i = i - 1;
  }
  i = 4;      
  saveIt();
  clearBuff();
}


void wipeIt(){
  i = 0;
  while(i < 100){
    wp_lats[i] = 0;
    wp_lons[i] = 0;
    i++;
  }
  mySerial.println("All WP's cleared");
  clearBuff();
}

void deleteIt(){
  
  mySerial.print("WP to delete: ");
  mySerial.println(wpnum);
  clearBuff();
  i = wpnum;
  while(i < 100){
    wp_lats[i] = wp_lats[i+1];
    wp_lons[i] = wp_lons[i+1];
    i++;
  }
}



void commander(){
  buffIt();  //fills the buffer until it recieves a $ sign, needs tweaking to minimize packets and hang time   
  if (rdy == 1){     //siginifies that we have a good packet so we can roll!   
    rdy = 0;         //Don't run again without a good command buffer   
    if(buff[0] == ID){   //make sure we need to even listen     
      rdy = 0;                    //I think this is unneccessary....        
      plane = buff[0];             //could use this, but ID should work and this could be obsoleted....        
      mySerial.print("Plane:");      //debugg
      mySerial.print(plane, BYTE);
      mySerial.print(" ,Mode:");
      if(buff[1] == 'D'){                    //data
        mySerial.println(" Data");
        print_data();
        clearBuff();
      }
      else if(buff[1] == 'W'){       //everything to do with waypoints, most of the code
        wpmode();
      }
      else {
        clearBuff();               
      }
    }
    else {
      clearBuff();
    }     
  }  
}


void wpmode(){
  mySerial.println(" WP");
  if(buff[2] == 'S'){                         //Set WP mode   
    if(buff[3] == 'I'){                     //withing set, this inserts a waypoint
      mySerial.println("Insert");
      len = 0;
      i = 4;
      insertWP();
    }
    else if(buff[3] == 'E'){                //Wipe all the wps
      wipeIt();
    }
    else if(buff[3] == 'D'){                //Delete a specific waypoint
      if(buff[4] == 'L'){
        mySerial.println("Deleting listed WPs");
        len = 0;
        i = 5;
        deleteList();
      }
      else{
        mySerial.println("Delete");
        len = 0;
        i = 4;
        get_wp();
        deleteIt();
      }      
    }             
    else if(buff[3] == 'G'){                //change the current waypoint being flown towards
      current_wp = atoi(&buff[4]);
      mySerial.println(current_wp,BYTE); 
    }                    
    else{ 
      mySerial.println(" Configuration");    //writes over waypoint without the shifting associated with inserting
      i = 3;
      len = 0;
      saveIt(); 
      clearBuff();
    }
  }
  if(buff[2] == 'P'){                             //WP printing, to check on them
                        //print specific waypoint, needs to be appended to take list commandss
    if(buff[3] == 'A'){                          //Print all
      mySerial.println("Printing all Waypoints");
      i = 0;
      while(i <= 100){
        mySerial.print(wp_lats[i]);
        mySerial.print("   ");
        mySerial.println(wp_lons[i]);
        i++;
      }
    }
    else if(buff[3] == 'L'){
      i = 4;
      len = 0;
    mySerial.println("Printing a Waypoint List");
    printList();
    }
    else{
    val = atoi(&buff[3]);
    Print_Waypoint_flash(val);
    }
    clearBuff();           
  }
}


void printList(){
get_wp();
val = wpnum;
mySerial.println(val);
i++;
len = 0;
get_wp();
i = wpnum;
mySerial.println(wpnum);
while(val <= i){
  Print_Waypoint_flash(val); 
  val++;
  }
mySerial.println("Done with list");
}


void deleteList(){
get_wp();
val = wpnum;
i++;
len = 0;
get_wp();
while(wpnum >= val){
  deleteIt(); 
  wpnum--;
  }
mySerial.println("Done with list");
}

All variables used are global. This is just a small portion of the entire code, and I can post the rest it is just long.

Here is the specific portion giving me problems

void saveIt(){      //give the starting location in the buffer and it will save the new wp according to the syntax used throughout AND STORE
  get_wp();
                  mySerial.print("wp:");
                  mySerial.println(wpnum);
                  len = 0;
                  i++;
                  get_lati();
                  mySerial.print("New lat command:");
                  mySerial.println(lati);
                  len = 0;
                  i++;
                  get_loni();
                  mySerial.print("New lon command:");
                  mySerial.println(loni);
                  len = 0;
                  i++;
                  get_alti();
                  mySerial.print("New alt command:");
                  mySerial.println(alti);
                  clearBuff();
                   wp_lons[wpnum] = loni;
                  wp_lats[wpnum] = lati;
                // wp_alts[wpnum] = lati;
                  i = 0;
}

The second to last line is commented out, and with it commented out everything works perfect, every single thing. I can access, change, modify, delete, it all works fine, the only problem being the non-EEPROM system only works with LAT and LON, no ALT. When I remove the comment from the last line, nothing works as it is supposed to, even commands that don't invlove the function saveIt();

So the reason I think it may be memory is the wp_alts[], wp_lons[] and wp_lats[] are all defined as 100 integer long arrays. If I changed the size of wp_alts[] from 100 to 2 the code seems to work fine (except I can only save two waypoints!). This doesn't make sense to me because after compiling and writing the total code is 21k of 30k!

I know this is a long winded question but any help would be greatly appreciated. I am also very open to suggestions on ways to make my code shorter, as after I finished it it was much longer that I thought it would need to be

You've got a lot of debug strings - could they be causing you low RAM problems?
Maybe try PROGMEM strings.

Your code may only be 21k or 30k but that's FLASH. You don't have a lot of RAM to work with (I think it's 2K) and your 3 100-long arrays take up 400 bytes each, or 1200 bytes out of that. If you have other constant data (strings, initialized arrays, etc.) it can easily eat into the rest of the RAM and leave nothing for your stack space.

That makes sense to me and explains why when I change the array sizes things seem to start working. I will try the first suggestion and see if I can free up some RAM without sacrificing number of waypoints.

Thanks

Wait a mo - these waypoints are "int"s ?
So that's two bytes per point, 200 bytes per array.
Which AVR are you using, 'cos the 168 has only 512 bytes of EEPROM, and the 328 has 1K bytes.

none of this gets stored to the EEPROM. Right now the only things being stored to the EEPROM are the sensor calibrations and the home position. If the used wants to store anything else to EEPROM it is done prior to launch with a seperate set-up program

PS it uses a 328

PS it uses a 328

:-[ Oops, yes, just re-read your original post, and realised you couldn't be using a 168.

There's a useful section on PROGMEM strings in the playground
http://www.arduino.cc/playground/Main/PROGMEM

So I changed my array sizes to 75 each and everyone lived happily thereafter! thanks for the feedback, if I should find myself needing more RAM I will try to use PROGMEM.

Thanks everyone