Fixed GPS activation issue, Serial/Serial3 selection and new automatic mode switch feature

git-svn-id: https://arducopter.googlecode.com/svn/trunk@445 f9c3cf11-9bcb-44bc-f272-b75c42450872

Arducopter/ArduCopter.h

@@ -212,6 +212,8 @@ int Sonar_Counter=0;
 
 // AP_mode : 1=> Position hold  2=>Stabilization assist mode (normal mode)
 byte AP_mode = 2;  
+byte FL_mode = 0; // Flight mode : 0 => Stable, 1 => Acro mode. Stable as default
+
 
 // Mode LED timers and variables, used to blink LED_Green
 byte gled_status = HIGH;

Arducopter/Arducopter.pde

@@ -54,6 +54,8 @@
 #define IsMAG      // Do we have a Magnetometer connected, if have remember to activate it from Configurator
 //#define IsTEL    // Do we have a telemetry connected, eg. XBee connected on Telemetry port
 #define IsAM       // Do we have motormount LED's. AM = Atraction Mode
+#define AUTOMODE   // New experimental Automode to change between Stable <=> Acro. If pitch/roll stick move is more than 50% change mode
+//#define IsXBEE     // Moves all serial communication to Telemetry port when activated.
 
 #define CONFIGURATOR  // Do se use Configurator or normal text output over serial link
 
@@ -73,6 +75,26 @@
 #define FLIGHT_MODE_+    // Traditional "one arm as nose" frame configuration
 //#define FLIGHT_MODE_X  // Frame orientation 45 deg to CCW, nose between two arms
 
+// Quick and easy hack to change FTDI Serial output to Telemetry port. Just activate #define IsXBEE some lines earlier
+#ifndef IsXBEE
+#define SerBau  115200
+#define SerPri  Serial.print
+#define SerPriln Serial.println
+#define SerAva  Serial.available
+#define SerRea  Serial.read
+#define SerFlu  Serial.flush
+#define SerBeg  Serial.begin
+#define SerPor  "FTDI"
+#else
+#define SerBau  115200
+#define SerPri  Serial3.print
+#define SerPriln Serial3.println
+#define SerAva  Serial3.available
+#define SerRea  Serial3.read
+#define SerFlu  Serial3.flush
+#define SerBeg  Serial3.begin
+#define SerPor  "Telemetry"
+#endif
 
 /* ****************************************************************************** */
 /* ****************************** Includes ************************************** */
@@ -349,16 +371,19 @@ void setup()
 
   DataFlash.StartWrite(1);   // Start a write session on page 1
 
-    //Serial.begin(57600);
+  SerBeg(SerBau);                      // Initialize SerialXX.port, IsXBEE define declares which port
-  Serial.begin(115200);
+#ifndef CONFIGURATOR  
-  //Serial.println();
+  SerPri("ArduCopter Quadcopter v");
-  //Serial.println("ArduCopter Quadcopter v1.0");
+  SerPriln(VER)
+  SerPri("Serial ready on port: ");    // Printout greeting to selecter serial port
+  SerPriln(SerPor);                    // Printout serial port name
+#endif
 
   // Check if we enable the DataFlash log Read Mode (switch)
   // If we press switch 1 at startup we read the Dataflash eeprom
   while (digitalRead(SW1_pin)==0)
   {
-    Serial.println("Entering Log Read Mode...");
+    SerPriln("Entering Log Read Mode...");
     Log_Read(1,2000);
     delay(30000);
   }
@@ -382,10 +407,10 @@ void setup()
     for(int y=0; y<=2; y++)   // Read initial ADC values for gyro offset.
     {
       aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2;
-      //Serial.print(AN[y]);
+      //SerPri(AN[y]);
-      //Serial.print(",");
+      //SerPri(",");
     }
-    //Serial.println();
+    //SerPriln();
     Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
     delay(10);
     
@@ -419,12 +444,12 @@ void setup()
 #ifndef CONFIGURATOR
   for(i=0;i<6;i++)
   {
-    Serial.print("AN[]:");
+    SerPri("AN[]:");
-    Serial.println(AN_OFFSET[i]);
+    SerPriln(AN_OFFSET[i]);
   }
-  Serial.print("Yaw neutral value:");
+  SerPri("Yaw neutral value:");
-  //  Serial.println(Neutro_yaw);
+  //  SerPriln(Neutro_yaw);
-  Serial.print(yaw_mid);
+  SerPri(yaw_mid);
 #endif
   delay(1000);
 
@@ -492,16 +517,16 @@ void loop(){
     log_yaw = ToDeg(yaw) * 10;
 
 #ifndef CONFIGURATOR    
-    Serial.print(log_roll);
+    SerPri(log_roll);
-    Serial.print(",");
+    SerPri(",");
-    Serial.print(log_pitch);
+    SerPri(log_pitch);
-    Serial.print(",");
+    SerPri(",");
-    Serial.print(log_yaw);
+    SerPri(log_yaw);
 
     //for (int i = 0; i < 6; i++)
     //{
-    //  Serial.print(AN[i]);
+    //  SerPri(AN[i]);
-    //  Serial.print(",");
+    //  SerPri(",");
     //}
 #endif
 
@@ -524,6 +549,28 @@ void loop(){
    
       command_rx_roll = (ch_roll-roll_mid) / 12.0;
       command_rx_pitch = (ch_pitch-pitch_mid) / 12.0;
+ 
+#ifdef AUTOMODE  
+      // New Automatic Stable <=> Acro switch. If pitch/roll stick is more than 60% from center, change to Acro    
+      if(command_rx_roll >= 30 || command_rx_roll <= -30 || 
+        command_rx_pitch >= 30 || command_rx_pitch <= -30 ) {
+       FL_mode = 1; 
+      } else FL_mode = 0;
+#endif
+
+      if(ch_aux2 > 1800) FL_mode = 1; // Force to Acro mode from radio
+      
+/*
+      // Debuging channels and fl_mode
+      SerPri(command_rx_roll);
+      comma();
+      SerPri(command_rx_pitch);
+      comma();
+      SerPri(FL_mode, DEC);
+      SerPriln();
+*/
+
+      
       //aux_float = (ch_yaw-Neutro_yaw) / 180.0;
       if (abs(ch_yaw-yaw_mid)<12)   // Take into account a bit of "dead zone" on yaw
         aux_float = 0.0;
@@ -540,11 +587,16 @@ void loop(){
       //K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2;
       K_aux = K_aux * 0.8 + ((ch_aux2-AUX_MID) / 300.0 + 1.7) * 0.2;   // /300 + 1.0
       if (K_aux < 0) K_aux = 0;
-      //Serial.print(",");
+      //SerPri(",");
-      //Serial.print(K_aux);
+      //SerPri(K_aux);
+
+
+
+    
+
 
       // We read the Quad Mode from Channel 5
-      if (ch_aux < 1200)
+      if (ch_aux > 1800)  // We really need to switch it ON from radio to activate GPS hold
       {
         AP_mode = 1;           // Position hold mode (GPS position control)
         digitalWrite(LED_Yellow,HIGH); // Yellow LED On
@@ -567,11 +619,11 @@ void loop(){
         target_longitude = GPS.Longitude;
 
 #ifndef CONFIGURATOR
-        Serial.println();
+        SerPriln();
-        Serial.print("* Target:");
+        SerPri("* Target:");
-        Serial.print(target_longitude);
+        SerPri(target_longitude);
-        Serial.print(",");
+        SerPri(",");
-        Serial.println(target_lattitude);
+        SerPriln(target_lattitude);
 #endif
         target_position=1;
         //target_sonar_altitude = sonar_value;
@@ -595,10 +647,10 @@ void loop(){
       GPS.NewData=0;  // We Reset the flag...
 
       //Output GPS data
-      //Serial.print(",");
+      //SerPri(",");
-      //Serial.print(GPS.Lattitude);
+      //SerPri(GPS.Lattitude);
-      //Serial.print(",");
+      //SerPri(",");
-      //Serial.print(GPS.Longitude);
+      //SerPri(GPS.Longitude);
 
       // Write GPS data to DataFlash log
       Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude, GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats);
@@ -617,15 +669,20 @@ void loop(){
         }
         else
         {
-          //Serial.print("NOFIX");
+          //SerPri("NOFIX");
           command_gps_roll=0;
           command_gps_pitch=0;
         }
       }
     }
 
+
+    
+
+
     // Control methodology selected using AUX2
-    if (ch_aux2 < 1200) {
+//    if (ch_aux2 < 1200) { 
+    if(FL_mode == 0) {        // Changed for variable
       gled_speed = 1200;
       Attitude_control_v3();
     }
@@ -714,7 +771,7 @@ void loop(){
     APM_RC.Force_Out2_Out3();
 
 #ifndef CONFIGURATOR
-    Serial.println();  // Line END 
+    SerPriln();  // Line END 
 #endif
   }
 #ifdef CONFIGURATOR

Arducopter/Functions.pde

@@ -27,23 +27,23 @@ void RadioCalibration() {
   long pitch_new = 0;
   long yaw_new = 0;
 
-  Serial.flush();
+  SerFlu();
-  Serial.println("Entering Radio Calibration mode");
+  SerPriln("Entering Radio Calibration mode");
-  Serial.println("Current channel MID values are:");
+  SerPriln("Current channel MID values are:");
-  Serial.print("ROLL: ");
+  SerPri("ROLL: ");
-  Serial.print(roll_mid);
+  SerPri(roll_mid);
-  Serial.print(" PITCH: ");
+  SerPri(" PITCH: ");
-  Serial.print(pitch_mid);
+  SerPri(pitch_mid);
-  Serial.print(" YAW: ");
+  SerPri(" YAW: ");
-  Serial.print(yaw_mid);
+  SerPri(yaw_mid);
-  Serial.println();
+  SerPriln();
-  Serial.println("Recalibrate Channel MID's [Y/N]?: ");
+  SerPriln("Recalibrate Channel MID's [Y/N]?: ");
   command_timer = millis();
 
   // Start counter loop and wait serial input. If not input for 5 seconds, return to normal mode
   while(millis() - command_timer < 5000) {
-    if (Serial.available()) {
+    if (SerAva()) {
-      queryType = Serial.read();
+      queryType = SerRea();
       if(queryType == 'y' || queryType == 'Y') {  
         Cmd_ok = TRUE;
         break;    
@@ -56,16 +56,16 @@ void RadioCalibration() {
   }
   if(Cmd_ok) {
     // We have a go. Let's do new calibration
-    Serial.println("Starting calibration run in 5 seconds. Place all sticks to their middle including trims");
+    SerPriln("Starting calibration run in 5 seconds. Place all sticks to their middle including trims");
     for(counter = 5; counter >= 0; counter --) {
       command_timer = millis();
       while(millis() - command_timer < 1000) {
       }
-      Serial.println(counter);
+      SerPriln(counter);
     }
     // Do actual calibration now
-    Serial.println("Measuring average channel values");
+    SerPriln("Measuring average channel values");
-    Serial.println("ROLL, PITCH, YAW");
+    SerPriln("ROLL, PITCH, YAW");
 
     counter = 0; // Reset counter for just in case. 
     command_timer = millis();
@@ -80,42 +80,42 @@ void RadioCalibration() {
         ch_aux = APM_RC.InputCh(4);
         ch_aux2 = APM_RC.InputCh(5);
 
-        Serial.print(ch_roll);
+        SerPri(ch_roll);
         comma();
-        Serial.print(ch_pitch);
+        SerPri(ch_pitch);
         comma();
-        Serial.print(ch_yaw);
+        SerPri(ch_yaw);
-        Serial.println();
+        SerPriln();
         roll_new += ch_roll;
         pitch_new += ch_pitch; 
         yaw_new += ch_yaw;
         counter++;
       }
     }
-    Serial.print("New samples received: ");
+    SerPri("New samples received: ");
-    Serial.println(counter);    
+    SerPriln(counter);    
     roll_new = roll_new / counter;
     pitch_new = pitch_new / counter;
     yaw_new = yaw_new / counter;
-    Serial.print("New values as: ");
+    SerPri("New values as: ");
-    Serial.print("ROLL: ");
+    SerPri("ROLL: ");
-    Serial.print(roll_new);
+    SerPri(roll_new);
-    Serial.print(" PITCH: ");
+    SerPri(" PITCH: ");
-    Serial.print(pitch_new);
+    SerPri(pitch_new);
-    Serial.print(" YAW: ");
+    SerPri(" YAW: ");
-    Serial.print(yaw_new);
+    SerPri(yaw_new);
-    Serial.println();
+    SerPriln();
-    Serial.println("Accept & Save values [Y/N]?: ");
+    SerPriln("Accept & Save values [Y/N]?: ");
     Cmd_ok = FALSE;
     while(millis() - command_timer < 5000) {
-      if (Serial.available()) {
+      if (SerAva()) {
-        queryType = Serial.read();
+        queryType = SerRea();
         if(queryType == 'y' || queryType == 'Y') { 
           Cmd_ok = TRUE;
           roll_mid = roll_new;
           pitch_mid = pitch_new;
           yaw_mid = yaw_new;
-          Serial.println("Values accepted, remember to save them to EEPROM with 'W' command");
+          SerPriln("Values accepted, remember to save them to EEPROM with 'W' command");
           break;    
         } 
         else {
@@ -127,12 +127,12 @@ void RadioCalibration() {
   } 
   if(queryType == 'n' || queryType == 'N') Cmd_ok = TRUE;
   if(Cmd_ok)
-    Serial.println("Returning normal mode...");
+    SerPriln("Returning normal mode...");
-  else Serial.println("Command timeout, returning normal mode....");
+  else SerPriln("Command timeout, returning normal mode....");
 }
 
 void comma() {
-  Serial.print(',');
+  SerPri(',');
 }
 
 #if BATTERY_EVENT == 1

Arducopter/Log.pde

@@ -91,21 +91,21 @@ void Log_Write_Radio(int ch1, int ch2, int ch3,int ch4, int ch5, int ch6)
 // Read a Sensor raw data packet
 void Log_Read_Sensor()
 {  
-  Serial.print("SENSOR:");
+  SerPri("SENSOR:");
-  Serial.print(DataFlash.ReadInt());  // GX
+  SerPri(DataFlash.ReadInt());  // GX
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());  // GY
+  SerPri(DataFlash.ReadInt());  // GY
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());  // GZ
+  SerPri(DataFlash.ReadInt());  // GZ
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());  // ACCX
+  SerPri(DataFlash.ReadInt());  // ACCX
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());  // ACCY
+  SerPri(DataFlash.ReadInt());  // ACCY
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());  // ACCZ
+  SerPri(DataFlash.ReadInt());  // ACCZ
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());  // AUX 
+  SerPri(DataFlash.ReadInt());  // AUX 
-  Serial.println();
+  SerPriln();
 }
 
 // Read an attitude packet
@@ -118,29 +118,29 @@ void Log_Read_Attitude()
   log_roll = DataFlash.ReadInt();
   log_pitch = DataFlash.ReadInt();
   log_yaw = DataFlash.ReadInt(); 
-  Serial.print("ATT:");
+  SerPri("ATT:");
-  Serial.print(log_roll);
+  SerPri(log_roll);
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(log_pitch);
+  SerPri(log_pitch);
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(log_yaw);
+  SerPri(log_yaw);
-  Serial.println();
+  SerPriln();
 }
 
 // Read a Sensor raw data packet
 void Log_Read_PID()
 {  
-  Serial.print("PID:");
+  SerPri("PID:");
-  Serial.print((int)DataFlash.ReadByte());  // NUM_PID
+  SerPri((int)DataFlash.ReadByte());  // NUM_PID
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());  // P
+  SerPri(DataFlash.ReadInt());  // P
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());  // I
+  SerPri(DataFlash.ReadInt());  // I
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());  // D
+  SerPri(DataFlash.ReadInt());  // D
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());  // output
+  SerPri(DataFlash.ReadInt());  // output
-  Serial.println();
+  SerPriln();
 }
 
 // Read a GPS packet
@@ -164,42 +164,42 @@ void Log_Read_GPS()
   log_Ground_Speed = DataFlash.ReadLong();
   log_Ground_Course = DataFlash.ReadLong();
 
-  Serial.print("GPS:");
+  SerPri("GPS:");
-  Serial.print(log_Time);
+  SerPri(log_Time);
-  Serial.print(",");
+  SerPri(",");
-  Serial.print((int)log_Fix);
+  SerPri((int)log_Fix);
-  Serial.print(",");
+  SerPri(",");
-  Serial.print((int)log_NumSats);
+  SerPri((int)log_NumSats);
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(log_Lattitude);
+  SerPri(log_Lattitude);
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(log_Longitude);
+  SerPri(log_Longitude);
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(log_Altitude);
+  SerPri(log_Altitude);
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(log_Ground_Speed);
+  SerPri(log_Ground_Speed);
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(log_Ground_Course);
+  SerPri(log_Ground_Course);
-  Serial.println();
+  SerPriln();
 
 }
 
 // Read an Radio packet
 void Log_Read_Radio()
 {  
-  Serial.print("RADIO:");
+  SerPri("RADIO:");
-  Serial.print(DataFlash.ReadInt());
+  SerPri(DataFlash.ReadInt());
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());
+  SerPri(DataFlash.ReadInt());
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());
+  SerPri(DataFlash.ReadInt());
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());
+  SerPri(DataFlash.ReadInt());
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());
+  SerPri(DataFlash.ReadInt());
-  Serial.print(",");
+  SerPri(",");
-  Serial.print(DataFlash.ReadInt());
+  SerPri(DataFlash.ReadInt());
-  Serial.println();
+  SerPriln();
 }
 
 // Read the DataFlash log memory : Packet Parser
@@ -247,8 +247,8 @@ void Log_Read(int start_page, int end_page)
             log_step++;
             break;
           default:
-            Serial.print("Error Reading Packet: ");
+            SerPri("Error Reading Packet: ");
-            Serial.print(packet_count); 
+            SerPri(packet_count); 
             log_step=0;   // Restart, we have a problem...
           }
         break;
@@ -256,12 +256,12 @@ void Log_Read(int start_page, int end_page)
         if(data==END_BYTE)
            packet_count++;
         else
-           Serial.println("Error Reading END_BYTE");
+           SerPriln("Error Reading END_BYTE");
         log_step=0;      // Restart sequence: new packet...        
       }
     }
-  Serial.print("Number of packets read: ");
+  SerPri("Number of packets read: ");
-  Serial.println(packet_count);
+  SerPriln(packet_count);
 }
 
 

Arducopter/SerialCom.pde

@@ -20,8 +20,8 @@
 
 void readSerialCommand() {
   // Check for serial message
-  if (Serial.available()) {
+  if (SerAva()) {
-    queryType = Serial.read();
+    queryType = SerRea();
     switch (queryType) {
     case 'A': // Stable PID
       KP_QUAD_ROLL = readFloatSerial();
@@ -114,114 +114,114 @@ void sendSerialTelemetry() {
   float aux_float[3]; // used for sensor calibration
   switch (queryType) {
   case '=': // Reserved debug command to view any variable from Serial Monitor
-/*    Serial.print("throttle =");
+/*    SerPri("throttle =");
-    Serial.println(ch_throttle);
+    SerPriln(ch_throttle);
-    Serial.print("control roll =");
+    SerPri("control roll =");
-    Serial.println(control_roll-CHANN_CENTER);
+    SerPriln(control_roll-CHANN_CENTER);
-    Serial.print("control pitch =");
+    SerPri("control pitch =");
-    Serial.println(control_pitch-CHANN_CENTER);
+    SerPriln(control_pitch-CHANN_CENTER);
-    Serial.print("control yaw =");
+    SerPri("control yaw =");
-    Serial.println(control_yaw-CHANN_CENTER);
+    SerPriln(control_yaw-CHANN_CENTER);
-    Serial.print("front left yaw =");
+    SerPri("front left yaw =");
-    Serial.println(frontMotor);
+    SerPriln(frontMotor);
-    Serial.print("front right yaw =");
+    SerPri("front right yaw =");
-    Serial.println(rightMotor);
+    SerPriln(rightMotor);
-    Serial.print("rear left yaw =");
+    SerPri("rear left yaw =");
-    Serial.println(leftMotor);
+    SerPriln(leftMotor);
-    Serial.print("rear right motor =");
+    SerPri("rear right motor =");
-    Serial.println(backMotor);
+    SerPriln(backMotor);
-    Serial.println();
+    SerPriln();
 
-    Serial.print("current roll rate =");
+    SerPri("current roll rate =");
-    Serial.println(read_adc(0));
+    SerPriln(read_adc(0));
-    Serial.print("current pitch rate =");
+    SerPri("current pitch rate =");
-    Serial.println(read_adc(1));
+    SerPriln(read_adc(1));
-    Serial.print("current yaw rate =");
+    SerPri("current yaw rate =");
-    Serial.println(read_adc(2));
+    SerPriln(read_adc(2));
-    Serial.print("command rx yaw =");
+    SerPri("command rx yaw =");
-    Serial.println(command_rx_yaw);
+    SerPriln(command_rx_yaw);
-    Serial.println(); 
+    SerPriln(); 
     queryType = 'X';*/
-    Serial.print(APM_RC.InputCh(0));
+    SerPri(APM_RC.InputCh(0));
     comma();
-    Serial.print(ch_roll_slope);
+    SerPri(ch_roll_slope);
     comma();
-    Serial.print(ch_roll_offset);
+    SerPri(ch_roll_offset);
     comma();
-    Serial.println(ch_roll);
+    SerPriln(ch_roll);
     break;
   case 'B': // Send roll, pitch and yaw PID values
-    Serial.print(KP_QUAD_ROLL, 3);
+    SerPri(KP_QUAD_ROLL, 3);
     comma();
-    Serial.print(KI_QUAD_ROLL, 3);
+    SerPri(KI_QUAD_ROLL, 3);
     comma();
-    Serial.print(STABLE_MODE_KP_RATE_ROLL, 3);
+    SerPri(STABLE_MODE_KP_RATE_ROLL, 3);
     comma();
-    Serial.print(KP_QUAD_PITCH, 3);
+    SerPri(KP_QUAD_PITCH, 3);
     comma();
-    Serial.print(KI_QUAD_PITCH, 3);
+    SerPri(KI_QUAD_PITCH, 3);
     comma();
-    Serial.print(STABLE_MODE_KP_RATE_PITCH, 3);
+    SerPri(STABLE_MODE_KP_RATE_PITCH, 3);
     comma();
-    Serial.print(KP_QUAD_YAW, 3);
+    SerPri(KP_QUAD_YAW, 3);
     comma();
-    Serial.print(KI_QUAD_YAW, 3);
+    SerPri(KI_QUAD_YAW, 3);
     comma();
-    Serial.print(STABLE_MODE_KP_RATE_YAW, 3);
+    SerPri(STABLE_MODE_KP_RATE_YAW, 3);
     comma();
-    Serial.print(STABLE_MODE_KP_RATE, 3);    // NOT USED NOW
+    SerPri(STABLE_MODE_KP_RATE, 3);    // NOT USED NOW
     comma();
-    Serial.println(MAGNETOMETER, 3);
+    SerPriln(MAGNETOMETER, 3);
     queryType = 'X';
     break;
   case 'D': // Send GPS PID
-    Serial.print(KP_GPS_ROLL, 3);
+    SerPri(KP_GPS_ROLL, 3);
     comma();
-    Serial.print(KI_GPS_ROLL, 3);
+    SerPri(KI_GPS_ROLL, 3);
     comma();
-    Serial.print(KD_GPS_ROLL, 3);
+    SerPri(KD_GPS_ROLL, 3);
     comma();
-    Serial.print(KP_GPS_PITCH, 3);
+    SerPri(KP_GPS_PITCH, 3);
     comma();
-    Serial.print(KI_GPS_PITCH, 3);
+    SerPri(KI_GPS_PITCH, 3);
     comma();
-    Serial.print(KD_GPS_PITCH, 3);
+    SerPri(KD_GPS_PITCH, 3);
     comma();
-    Serial.print(GPS_MAX_ANGLE, 3);
+    SerPri(GPS_MAX_ANGLE, 3);
     comma();
-    Serial.println(GEOG_CORRECTION_FACTOR, 3);
+    SerPriln(GEOG_CORRECTION_FACTOR, 3);
     queryType = 'X';
     break;
   case 'F': // Send altitude PID
-    Serial.print(KP_ALTITUDE, 3);
+    SerPri(KP_ALTITUDE, 3);
     comma();
-    Serial.print(KI_ALTITUDE, 3);
+    SerPri(KI_ALTITUDE, 3);
     comma();
-    Serial.println(KD_ALTITUDE, 3);
+    SerPriln(KD_ALTITUDE, 3);
     queryType = 'X';
     break;
   case 'H': // Send drift correction PID
-    Serial.print(Kp_ROLLPITCH, 4);
+    SerPri(Kp_ROLLPITCH, 4);
     comma();
-    Serial.print(Ki_ROLLPITCH, 7);
+    SerPri(Ki_ROLLPITCH, 7);
     comma();
-    Serial.print(Kp_YAW, 4);
+    SerPri(Kp_YAW, 4);
     comma();
-    Serial.println(Ki_YAW, 6);
+    SerPriln(Ki_YAW, 6);
     queryType = 'X';
     break;
   case 'J': // Send sensor offset
-    Serial.print(gyro_offset_roll);
+    SerPri(gyro_offset_roll);
     comma();
-    Serial.print(gyro_offset_pitch);
+    SerPri(gyro_offset_pitch);
     comma();
-    Serial.print(gyro_offset_yaw);
+    SerPri(gyro_offset_yaw);
     comma();
-    Serial.print(acc_offset_x);
+    SerPri(acc_offset_x);
     comma();
-    Serial.print(acc_offset_y);
+    SerPri(acc_offset_y);
     comma();
-    Serial.println(acc_offset_z);
+    SerPriln(acc_offset_z);
     AN_OFFSET[3] = acc_offset_x;
     AN_OFFSET[4] = acc_offset_y;
     AN_OFFSET[5] = acc_offset_z;
@@ -235,136 +235,136 @@ void sendSerialTelemetry() {
     queryType = 'X';
     break;
   case 'P': // Send rate control PID
-    Serial.print(Kp_RateRoll, 3);
+    SerPri(Kp_RateRoll, 3);
     comma();
-    Serial.print(Ki_RateRoll, 3);
+    SerPri(Ki_RateRoll, 3);
     comma();
-    Serial.print(Kd_RateRoll, 3);
+    SerPri(Kd_RateRoll, 3);
     comma();
-    Serial.print(Kp_RatePitch, 3);
+    SerPri(Kp_RatePitch, 3);
     comma();
-    Serial.print(Ki_RatePitch, 3);
+    SerPri(Ki_RatePitch, 3);
     comma();
-    Serial.print(Kd_RatePitch, 3);
+    SerPri(Kd_RatePitch, 3);
     comma();
-    Serial.print(Kp_RateYaw, 3);
+    SerPri(Kp_RateYaw, 3);
     comma();
-    Serial.print(Ki_RateYaw, 3);
+    SerPri(Ki_RateYaw, 3);
     comma();
-    Serial.print(Kd_RateYaw, 3);
+    SerPri(Kd_RateYaw, 3);
     comma();
-    Serial.println(xmitFactor, 3);
+    SerPriln(xmitFactor, 3);
     queryType = 'X';
     break;
   case 'Q': // Send sensor data
-    Serial.print(read_adc(0));
+    SerPri(read_adc(0));
     comma();
-    Serial.print(read_adc(1));
+    SerPri(read_adc(1));
     comma();
-    Serial.print(read_adc(2));
+    SerPri(read_adc(2));
     comma();
-    Serial.print(read_adc(4));
+    SerPri(read_adc(4));
     comma();
-    Serial.print(read_adc(3));
+    SerPri(read_adc(3));
     comma();
-    Serial.print(read_adc(5));
+    SerPri(read_adc(5));
     comma();
-    Serial.print(err_roll);
+    SerPri(err_roll);
     comma();
-    Serial.print(err_pitch);
+    SerPri(err_pitch);
     comma();
-    Serial.print(degrees(roll));
+    SerPri(degrees(roll));
     comma();
-    Serial.print(degrees(pitch));
+    SerPri(degrees(pitch));
     comma();
-    Serial.println(degrees(yaw));
+    SerPriln(degrees(yaw));
     break;
   case 'R': // Send raw sensor data
     break;
   case 'S': // Send all flight data
-    Serial.print(timer-timer_old);
+    SerPri(timer-timer_old);
     comma();
-    Serial.print(read_adc(0));
+    SerPri(read_adc(0));
     comma();
-    Serial.print(read_adc(1));
+    SerPri(read_adc(1));
     comma();
-    Serial.print(read_adc(2));
+    SerPri(read_adc(2));
     comma();
-    Serial.print(ch_throttle);
+    SerPri(ch_throttle);
     comma();
-    Serial.print(control_roll);
+    SerPri(control_roll);
     comma();
-    Serial.print(control_pitch);
+    SerPri(control_pitch);
     comma();
-    Serial.print(control_yaw);
+    SerPri(control_yaw);
     comma();
-    Serial.print(frontMotor); // Front Motor
+    SerPri(frontMotor); // Front Motor
     comma();
-    Serial.print(backMotor); // Back Motor
+    SerPri(backMotor); // Back Motor
     comma();
-    Serial.print(rightMotor); // Right Motor
+    SerPri(rightMotor); // Right Motor
     comma();
-    Serial.print(leftMotor); // Left Motor
+    SerPri(leftMotor); // Left Motor
     comma();
-    Serial.print(read_adc(4));
+    SerPri(read_adc(4));
     comma();
-    Serial.print(read_adc(3));
+    SerPri(read_adc(3));
     comma();
-    Serial.println(read_adc(5));
+    SerPriln(read_adc(5));
     break;
   case 'T': // Spare
     break;
   case 'U': // Send receiver values
-    Serial.print(ch_roll); // Aileron
+    SerPri(ch_roll); // Aileron
     comma();
-    Serial.print(ch_pitch); // Elevator
+    SerPri(ch_pitch); // Elevator
     comma();
-    Serial.print(ch_yaw); // Yaw
+    SerPri(ch_yaw); // Yaw
     comma();
-    Serial.print(ch_throttle); // Throttle
+    SerPri(ch_throttle); // Throttle
     comma();
-    Serial.print(ch_aux); // AUX1 (Mode)
+    SerPri(ch_aux); // AUX1 (Mode)
     comma();
-    Serial.print(ch_aux2); // AUX2 
+    SerPri(ch_aux2); // AUX2 
     comma();
-    Serial.print(roll_mid); // Roll MID value
+    SerPri(roll_mid); // Roll MID value
     comma();
-    Serial.print(pitch_mid); // Pitch MID value
+    SerPri(pitch_mid); // Pitch MID value
     comma();
-    Serial.println(yaw_mid); // Yaw MID Value
+    SerPriln(yaw_mid); // Yaw MID Value
     break;
   case 'V': // Spare
     break;
   case 'X': // Stop sending messages
     break;
   case '!': // Send flight software version
-    Serial.println(VER);
+    SerPriln(VER);
     queryType = 'X';
     break;
   case '2': // Send transmitter calibration values
-    Serial.print(ch_roll_slope);
+    SerPri(ch_roll_slope);
     comma();
-    Serial.print(ch_roll_offset);
+    SerPri(ch_roll_offset);
     comma();
-    Serial.print(ch_pitch_slope);
+    SerPri(ch_pitch_slope);
     comma();
-    Serial.print(ch_pitch_offset);
+    SerPri(ch_pitch_offset);
     comma();
-    Serial.print(ch_yaw_slope);
+    SerPri(ch_yaw_slope);
     comma();
-    Serial.print(ch_yaw_offset);
+    SerPri(ch_yaw_offset);
     comma();
-    Serial.print(ch_throttle_slope);
+    SerPri(ch_throttle_slope);
     comma();
-    Serial.print(ch_throttle_offset);
+    SerPri(ch_throttle_offset);
     comma();
-    Serial.print(ch_aux_slope);
+    SerPri(ch_aux_slope);
     comma();
-    Serial.print(ch_aux_offset);
+    SerPri(ch_aux_offset);
     comma();
-    Serial.print(ch_aux2_slope);
+    SerPri(ch_aux2_slope);
     comma();
-    Serial.println(ch_aux2_offset);
+    SerPriln(ch_aux2_offset);
     queryType = 'X';
   break;
   case '.': // Modify GPS settings
@@ -380,12 +380,12 @@ float readFloatSerial() {
   char data[128] = "";
 
   do {
-    if (Serial.available() == 0) {
+    if (SerAva() == 0) {
       delay(10);
       timeout++;
     }
     else {
-      data[index] = Serial.read();
+      data[index] = SerRea();
       timeout = 0;
       index++;
     }