From 837c18392213145a696ecca2c3c926173c497e7d Mon Sep 17 00:00:00 2001 From: jphelirc Date: Thu, 9 Sep 2010 14:56:38 +0000 Subject: [PATCH] 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 | 4 +- Arducopter/Arducopter.pde | 137 +++++++++++++++------ Arducopter/Functions.pde | 76 ++++++------ Arducopter/Log.pde | 138 ++++++++++----------- Arducopter/SerialCom.pde | 250 +++++++++++++++++++------------------- 5 files changed, 332 insertions(+), 273 deletions(-) diff --git a/Arducopter/ArduCopter.h b/Arducopter/ArduCopter.h index 8657b8f552..32b133071a 100644 --- a/Arducopter/ArduCopter.h +++ b/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; @@ -248,4 +250,4 @@ long tlmTimer = 0; // Arming/Disarming uint8_t Arming_counter=0; -uint8_t Disarming_counter=0; +uint8_t Disarming_counter=0; diff --git a/Arducopter/Arducopter.pde b/Arducopter/Arducopter.pde index 659ce0dcd3..4e1a88f922 100644 --- a/Arducopter/Arducopter.pde +++ b/Arducopter/Arducopter.pde @@ -34,7 +34,7 @@ 1) A, B, C LED's blinking rapidly while waiting ESCs to bootup and initial shake to end from connecting battery 2) A, B, C LED's have running light while calibrating Gyro/Acc's 3) Green LED Solid after initialization finished - + Green LED On = APM Initialization Finished Yellow LED On = GPS Hold Mode Yellow LED Off = Flight Assist Mode (No GPS) @@ -49,11 +49,13 @@ /* User definable modules */ // Comment out with // modules that you are not using -#define IsGPS // Do we have a GPS connected +#define IsGPS // Do we have a GPS connected //#define IsNEWMTEK// Do we have MTEK with new firmware -#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator +#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 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); - Serial.begin(115200); - //Serial.println(); - //Serial.println("ArduCopter Quadcopter v1.0"); + SerBeg(SerBau); // Initialize SerialXX.port, IsXBEE define declares which port +#ifndef CONFIGURATOR + SerPri("ArduCopter Quadcopter v"); + 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]); - //Serial.print(","); + //SerPri(AN[y]); + //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[]:"); - Serial.println(AN_OFFSET[i]); + SerPri("AN[]:"); + SerPriln(AN_OFFSET[i]); } - Serial.print("Yaw neutral value:"); - // Serial.println(Neutro_yaw); - Serial.print(yaw_mid); + SerPri("Yaw neutral value:"); + // SerPriln(Neutro_yaw); + SerPri(yaw_mid); #endif delay(1000); @@ -492,16 +517,16 @@ void loop(){ log_yaw = ToDeg(yaw) * 10; #ifndef CONFIGURATOR - Serial.print(log_roll); - Serial.print(","); - Serial.print(log_pitch); - Serial.print(","); - Serial.print(log_yaw); + SerPri(log_roll); + SerPri(","); + SerPri(log_pitch); + SerPri(","); + SerPri(log_yaw); //for (int i = 0; i < 6; i++) //{ - // Serial.print(AN[i]); - // Serial.print(","); + // SerPri(AN[i]); + // 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(","); - //Serial.print(K_aux); + //SerPri(","); + //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(); - Serial.print("* Target:"); - Serial.print(target_longitude); - Serial.print(","); - Serial.println(target_lattitude); + SerPriln(); + SerPri("* Target:"); + SerPri(target_longitude); + SerPri(","); + 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(","); - //Serial.print(GPS.Lattitude); - //Serial.print(","); - //Serial.print(GPS.Longitude); + //SerPri(","); + //SerPri(GPS.Lattitude); + //SerPri(","); + //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 @@ -749,4 +806,4 @@ void loop(){ // END of Arducopter.pde - + diff --git a/Arducopter/Functions.pde b/Arducopter/Functions.pde index 2797d15752..acf737a054 100644 --- a/Arducopter/Functions.pde +++ b/Arducopter/Functions.pde @@ -27,23 +27,23 @@ void RadioCalibration() { long pitch_new = 0; long yaw_new = 0; - Serial.flush(); - Serial.println("Entering Radio Calibration mode"); - Serial.println("Current channel MID values are:"); - Serial.print("ROLL: "); - Serial.print(roll_mid); - Serial.print(" PITCH: "); - Serial.print(pitch_mid); - Serial.print(" YAW: "); - Serial.print(yaw_mid); - Serial.println(); - Serial.println("Recalibrate Channel MID's [Y/N]?: "); + SerFlu(); + SerPriln("Entering Radio Calibration mode"); + SerPriln("Current channel MID values are:"); + SerPri("ROLL: "); + SerPri(roll_mid); + SerPri(" PITCH: "); + SerPri(pitch_mid); + SerPri(" YAW: "); + SerPri(yaw_mid); + SerPriln(); + 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()) { - queryType = Serial.read(); + if (SerAva()) { + 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"); - Serial.println("ROLL, PITCH, YAW"); + SerPriln("Measuring average channel values"); + 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); - Serial.println(); + SerPri(ch_yaw); + SerPriln(); roll_new += ch_roll; pitch_new += ch_pitch; yaw_new += ch_yaw; counter++; } } - Serial.print("New samples received: "); - Serial.println(counter); + SerPri("New samples received: "); + SerPriln(counter); roll_new = roll_new / counter; pitch_new = pitch_new / counter; yaw_new = yaw_new / counter; - Serial.print("New values as: "); - Serial.print("ROLL: "); - Serial.print(roll_new); - Serial.print(" PITCH: "); - Serial.print(pitch_new); - Serial.print(" YAW: "); - Serial.print(yaw_new); - Serial.println(); - Serial.println("Accept & Save values [Y/N]?: "); + SerPri("New values as: "); + SerPri("ROLL: "); + SerPri(roll_new); + SerPri(" PITCH: "); + SerPri(pitch_new); + SerPri(" YAW: "); + SerPri(yaw_new); + SerPriln(); + SerPriln("Accept & Save values [Y/N]?: "); Cmd_ok = FALSE; while(millis() - command_timer < 5000) { - if (Serial.available()) { - queryType = Serial.read(); + if (SerAva()) { + 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..."); - else Serial.println("Command timeout, returning normal mode...."); + SerPriln("Returning normal mode..."); + else SerPriln("Command timeout, returning normal mode...."); } void comma() { - Serial.print(','); + SerPri(','); } #if BATTERY_EVENT == 1 diff --git a/Arducopter/Log.pde b/Arducopter/Log.pde index 364a2b50ed..c3db699ea3 100644 --- a/Arducopter/Log.pde +++ b/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:"); - Serial.print(DataFlash.ReadInt()); // GX - Serial.print(","); - Serial.print(DataFlash.ReadInt()); // GY - Serial.print(","); - Serial.print(DataFlash.ReadInt()); // GZ - Serial.print(","); - Serial.print(DataFlash.ReadInt()); // ACCX - Serial.print(","); - Serial.print(DataFlash.ReadInt()); // ACCY - Serial.print(","); - Serial.print(DataFlash.ReadInt()); // ACCZ - Serial.print(","); - Serial.print(DataFlash.ReadInt()); // AUX - Serial.println(); + SerPri("SENSOR:"); + SerPri(DataFlash.ReadInt()); // GX + SerPri(","); + SerPri(DataFlash.ReadInt()); // GY + SerPri(","); + SerPri(DataFlash.ReadInt()); // GZ + SerPri(","); + SerPri(DataFlash.ReadInt()); // ACCX + SerPri(","); + SerPri(DataFlash.ReadInt()); // ACCY + SerPri(","); + SerPri(DataFlash.ReadInt()); // ACCZ + SerPri(","); + SerPri(DataFlash.ReadInt()); // AUX + 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:"); - Serial.print(log_roll); - Serial.print(","); - Serial.print(log_pitch); - Serial.print(","); - Serial.print(log_yaw); - Serial.println(); + SerPri("ATT:"); + SerPri(log_roll); + SerPri(","); + SerPri(log_pitch); + SerPri(","); + SerPri(log_yaw); + SerPriln(); } // Read a Sensor raw data packet void Log_Read_PID() { - Serial.print("PID:"); - Serial.print((int)DataFlash.ReadByte()); // NUM_PID - Serial.print(","); - Serial.print(DataFlash.ReadInt()); // P - Serial.print(","); - Serial.print(DataFlash.ReadInt()); // I - Serial.print(","); - Serial.print(DataFlash.ReadInt()); // D - Serial.print(","); - Serial.print(DataFlash.ReadInt()); // output - Serial.println(); + SerPri("PID:"); + SerPri((int)DataFlash.ReadByte()); // NUM_PID + SerPri(","); + SerPri(DataFlash.ReadInt()); // P + SerPri(","); + SerPri(DataFlash.ReadInt()); // I + SerPri(","); + SerPri(DataFlash.ReadInt()); // D + SerPri(","); + SerPri(DataFlash.ReadInt()); // output + 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:"); - Serial.print(log_Time); - Serial.print(","); - Serial.print((int)log_Fix); - Serial.print(","); - Serial.print((int)log_NumSats); - Serial.print(","); - Serial.print(log_Lattitude); - Serial.print(","); - Serial.print(log_Longitude); - Serial.print(","); - Serial.print(log_Altitude); - Serial.print(","); - Serial.print(log_Ground_Speed); - Serial.print(","); - Serial.print(log_Ground_Course); - Serial.println(); + SerPri("GPS:"); + SerPri(log_Time); + SerPri(","); + SerPri((int)log_Fix); + SerPri(","); + SerPri((int)log_NumSats); + SerPri(","); + SerPri(log_Lattitude); + SerPri(","); + SerPri(log_Longitude); + SerPri(","); + SerPri(log_Altitude); + SerPri(","); + SerPri(log_Ground_Speed); + SerPri(","); + SerPri(log_Ground_Course); + SerPriln(); } // Read an Radio packet void Log_Read_Radio() { - Serial.print("RADIO:"); - Serial.print(DataFlash.ReadInt()); - Serial.print(","); - Serial.print(DataFlash.ReadInt()); - Serial.print(","); - Serial.print(DataFlash.ReadInt()); - Serial.print(","); - Serial.print(DataFlash.ReadInt()); - Serial.print(","); - Serial.print(DataFlash.ReadInt()); - Serial.print(","); - Serial.print(DataFlash.ReadInt()); - Serial.println(); + SerPri("RADIO:"); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPri(DataFlash.ReadInt()); + 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: "); - Serial.print(packet_count); + SerPri("Error Reading Packet: "); + 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: "); - Serial.println(packet_count); + SerPri("Number of packets read: "); + SerPriln(packet_count); } - + diff --git a/Arducopter/SerialCom.pde b/Arducopter/SerialCom.pde index 1d3ef91b78..0862766327 100644 --- a/Arducopter/SerialCom.pde +++ b/Arducopter/SerialCom.pde @@ -20,8 +20,8 @@ void readSerialCommand() { // Check for serial message - if (Serial.available()) { - queryType = Serial.read(); + if (SerAva()) { + 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 ="); - Serial.println(ch_throttle); - Serial.print("control roll ="); - Serial.println(control_roll-CHANN_CENTER); - Serial.print("control pitch ="); - Serial.println(control_pitch-CHANN_CENTER); - Serial.print("control yaw ="); - Serial.println(control_yaw-CHANN_CENTER); - Serial.print("front left yaw ="); - Serial.println(frontMotor); - Serial.print("front right yaw ="); - Serial.println(rightMotor); - Serial.print("rear left yaw ="); - Serial.println(leftMotor); - Serial.print("rear right motor ="); - Serial.println(backMotor); - Serial.println(); +/* SerPri("throttle ="); + SerPriln(ch_throttle); + SerPri("control roll ="); + SerPriln(control_roll-CHANN_CENTER); + SerPri("control pitch ="); + SerPriln(control_pitch-CHANN_CENTER); + SerPri("control yaw ="); + SerPriln(control_yaw-CHANN_CENTER); + SerPri("front left yaw ="); + SerPriln(frontMotor); + SerPri("front right yaw ="); + SerPriln(rightMotor); + SerPri("rear left yaw ="); + SerPriln(leftMotor); + SerPri("rear right motor ="); + SerPriln(backMotor); + SerPriln(); - Serial.print("current roll rate ="); - Serial.println(read_adc(0)); - Serial.print("current pitch rate ="); - Serial.println(read_adc(1)); - Serial.print("current yaw rate ="); - Serial.println(read_adc(2)); - Serial.print("command rx yaw ="); - Serial.println(command_rx_yaw); - Serial.println(); + SerPri("current roll rate ="); + SerPriln(read_adc(0)); + SerPri("current pitch rate ="); + SerPriln(read_adc(1)); + SerPri("current yaw rate ="); + SerPriln(read_adc(2)); + SerPri("command rx yaw ="); + SerPriln(command_rx_yaw); + 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,16 +380,16 @@ 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++; } } while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128)); return atof(data); -} +}