From 35b754bbae46562cec7faa462a2d0af1608b893b Mon Sep 17 00:00:00 2001 From: jphelirc Date: Sun, 17 Oct 2010 17:34:17 +0000 Subject: [PATCH] Xbee/Ser0 telemetry ports are working again git-svn-id: https://arducopter.googlecode.com/svn/trunk@682 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArducopterNG/Arducopter.h | 10 +- ArducopterNG/ArducopterNG.pde | 199 ++++++++++++++++++++-------------- ArducopterNG/GCS.pde | 119 +++++++++++--------- ArducopterNG/System.pde | 12 +- 4 files changed, 199 insertions(+), 141 deletions(-) diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index 7d37697ac5..a419cde504 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -75,7 +75,8 @@ TODO: #define SERIAL2_BAUD 115200 #define SERIAL3_BAUD 115200 -#ifdef SerXbee // Xbee/Telemetry port +#ifdef SerXbee // Xbee/Telemetry port +//#define SerBau 115200 // Baud setting moved close next to port selection #define SerPri Serial3.print #define SerPrln Serial3.println #define SerRea Serial3.read @@ -86,11 +87,8 @@ TODO: #define SerPor "FTDI" #endif -#ifndef SerPri // If we don't have SerPri set yet, it means we have are using Serial0 -#define SerUSB -#endif - -#ifdef SerUSB // Defines for normal Serial0 port +#ifndef SerXbee // If we don't have SerXbee set, it means we have are using Serial0 +//#define SerBau 115200 // Baud setting moved close next to port selection #define SerPri Serial.print #define SerPrln Serial.println #define SerRea Serial.read diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index 4142869f9e..9f605f1a0a 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -6,9 +6,9 @@ File : ArducopterNG.pde Version : v1.0, 11 October 2010 Author(s): ArduCopter Team - Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz, - Jani Hirvinen, Ken McEwans, Roberto Navoni, - Sandro Benigno, Chris Anderson + Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz, + Jani Hirvinen, Ken McEwans, Roberto Navoni, + Sandro Benigno, Chris Anderson This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -22,7 +22,7 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . - + /* ********************************************************************** */ /* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */ /* Mounting position : RC connectors pointing backwards */ @@ -45,25 +45,41 @@ //#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 IsTEL // Do we have a telemetry connected, eg. XBee connected on Telemetry port -//#define IsAM // Do we have motormount LED's. AM = Atraction Mode +//LEGACY-jp #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 UseAirspeed + //#define UseBMP -//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it wired up!) + +//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!) #define CONFIGURATOR + +//////////////////// +// Telemetry + // Serial data, do we have FTDI cable or Xbee on Telemetry port as our primary command link -// If we are using normal FTDI/USB port as our telemetry/configuration, comment out next line +// If we are using normal FTDI/USB port as our telemetry/configuration, disable next //#define SerXbee -//#define Ser0 // FTDI/USB Port Either one -//#define Ser3 // Telemetry port +// Telemetry port speed, default is 115200 +#define SerBau 115200 + + + +////////////////////// +// Flight orientation // Frame build condiguration #define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration //#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms + + + + /* ************************************************************ */ /* **************** MAIN PROGRAM - INCLUDES ******************* */ /* ************************************************************ */ @@ -92,6 +108,7 @@ /* Software version */ #define VER 1.5 // Current software version (only numeric values) + /* ************************************************************ */ /* ************* MAIN PROGRAM - DECLARATIONS ****************** */ /* ************************************************************ */ @@ -107,9 +124,9 @@ unsigned long slowLoop = 0; /* **************** MAIN PROGRAM - SETUP ********************** */ /* ************************************************************ */ void setup() { - + APM_Init(); // APM Hardware initialization (in System.pde) - + mainLoop = millis(); // Initialize timers mediumLoop = mainLoop; GPS_timer = mainLoop; @@ -146,10 +163,10 @@ void loop() //float aux_float; currentTime = millis(); - + // Main loop at 200Hz (IMU + control) if ((currentTime-mainLoop) > 5) // 200Hz (every 5ms) - { + { G_Dt = (currentTime-mainLoop)*0.001; // Microseconds!!! mainLoop = currentTime; @@ -170,31 +187,31 @@ void loop() if (AP_mode == AP_NORMAL_MODE) // Normal mode Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw); else // Automatic mode : GPS position hold mode - Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw); - } + Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw); + } else { // ACRO Mode gled_speed = 400; Rate_control_v2(); // Reset yaw, so if we change to stable mode we continue with the actual yaw direction command_rx_yaw = ToDeg(yaw); - } + } // Send output commands to motor ESCs... motor_output(); - + // Autopilot mode functions if (AP_mode == AP_AUTOMATIC_MODE) - { + { if (target_position) - { + { if (GPS.NewData) // New GPS info? - { + { read_GPS_data(); Position_control(target_lattitude,target_longitude); // Call GPS position hold routine - } } + } else // First time we enter in GPS position hold we capture the target position as the actual position - { + { if (GPS.Fix){ // We need a GPS Fix to capture the actual position... target_lattitude = GPS.Lattitude; target_longitude = GPS.Longitude; @@ -203,68 +220,90 @@ void loop() target_baro_altitude = press_alt; Initial_Throttle = ch_throttle; Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde) - } + } command_gps_roll=0; command_gps_pitch=0; - } } + } else target_position=0; - } - - // Medium loop (about 60Hz) - if ((currentTime-mediumLoop)>=17){ - mediumLoop = currentTime; - GPS.Read(); // Read GPS data - // Each of the six cases executes at 10Hz - switch (medium_loopCounter){ - case 0: // Magnetometer reading (10Hz) - medium_loopCounter++; - slowLoop++; - #ifdef IsMAG - if (MAGNETOMETER == 1) { - APM_Compass.Read(); // Read magnetometer - APM_Compass.Calculate(roll,pitch); // Calculate heading - } - #endif - break; - case 1: // Barometer reading (2x10Hz = 20Hz) - medium_loopCounter++; - #ifdef UseBMP - if (APM_BMP085.Read()){ - read_baro(); - Baro_new_data = 1; - } - #endif - break; - case 2: // Send serial telemetry (10Hz) - medium_loopCounter++; - #ifdef CONFIGURATOR - sendSerialTelemetry(); - #endif - break; - case 3: // Read serial telemetry (10Hz) - medium_loopCounter++; - #ifdef CONFIGURATOR - readSerialCommand(); - #endif - break; - case 4: // second Barometer reading (2x10Hz = 20Hz) - medium_loopCounter++; - #ifdef UseBMP - if (APM_BMP085.Read()){ - read_baro(); - Baro_new_data = 1; - } - #endif - break; - case 5: // Battery monitor (10Hz) - medium_loopCounter=0; - #if BATTERY_EVENT == 1 - read_battery(); // Battery monitor - #endif - break; + } + + // Medium loop (about 60Hz) + if ((currentTime-mediumLoop)>=17){ + mediumLoop = currentTime; + GPS.Read(); // Read GPS data + // Each of the six cases executes at 10Hz + switch (medium_loopCounter){ + case 0: // Magnetometer reading (10Hz) + medium_loopCounter++; + slowLoop++; +#ifdef IsMAG + if (MAGNETOMETER == 1) { + APM_Compass.Read(); // Read magnetometer + APM_Compass.Calculate(roll,pitch); // Calculate heading } +#endif + break; + case 1: // Barometer reading (2x10Hz = 20Hz) + medium_loopCounter++; +#ifdef UseBMP + if (APM_BMP085.Read()){ + read_baro(); + Baro_new_data = 1; + } +#endif + break; + case 2: // Send serial telemetry (10Hz) + medium_loopCounter++; +#ifdef CONFIGURATOR + sendSerialTelemetry(); +#endif + break; + case 3: // Read serial telemetry (10Hz) + medium_loopCounter++; +#ifdef CONFIGURATOR + readSerialCommand(); +#endif + break; + case 4: // second Barometer reading (2x10Hz = 20Hz) + medium_loopCounter++; +#ifdef UseBMP + if (APM_BMP085.Read()){ + read_baro(); + Baro_new_data = 1; + } +#endif + break; + case 5: // Battery monitor (10Hz) + medium_loopCounter=0; +#if BATTERY_EVENT == 1 + read_battery(); // Battery monitor +#endif + break; } + } + + // AM and Mode status LED lights + if(millis() - gled_timer > gled_speed) { + gled_timer = millis(); + if(gled_status == HIGH) { + digitalWrite(LED_Green, LOW); +#ifdef IsAM + digitalWrite(RE_LED, LOW); +#endif + gled_status = LOW; +// SerPrln("L"); + } + else { + digitalWrite(LED_Green, HIGH); +#ifdef IsAM + if(motorArmed) digitalWrite(RE_LED, HIGH); +#endif + gled_status = HIGH; + } + } + } - + + diff --git a/ArducopterNG/GCS.pde b/ArducopterNG/GCS.pde index 4437feb873..ed2467b6d6 100644 --- a/ArducopterNG/GCS.pde +++ b/ArducopterNG/GCS.pde @@ -6,9 +6,9 @@ File : GCS.pde Version : v1.0, Aug 27, 2010 Author(s): ArduCopter Team - Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, - Jani Hirvinen, Ken McEwans, Roberto Navoni, - Sandro Benigno, Chris Anderson + Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, + Jani Hirvinen, Ken McEwans, Roberto Navoni, + Sandro Benigno, Chris Anderson This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -22,16 +22,16 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . - -* ************************************************************** * -ChangeLog: - - -* ************************************************************** * -TODO: - - -* ************************************************************** */ + + * ************************************************************** * + ChangeLog: + + + * ************************************************************** * + TODO: + + + * ************************************************************** */ // // Function : send_message() // @@ -43,10 +43,10 @@ TODO: void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 { - if(severity >= DEBUG_LEVEL){ - SerPri("MSG: "); - SerPrln(str); - } + if(severity >= DEBUG_LEVEL){ + SerPri("MSG: "); + SerPrln(str); + } } @@ -61,7 +61,7 @@ void send_message(byte severity, const char *str) // This is the instance of se void readSerialCommand() { // Check for serial message if (SerAva()) { - + queryType = SerRea(); switch (queryType) { case 'A': // Stable PID KP_QUAD_ROLL = readFloatSerial(); @@ -106,14 +106,14 @@ void readSerialCommand() { acc_offset_z = readFloatSerial(); break; case 'K': // Spare - break; + break; case 'M': // Receive debug motor commands frontMotor = readFloatSerial(); backMotor = readFloatSerial(); rightMotor = readFloatSerial(); leftMotor = readFloatSerial(); motorArmed = readFloatSerial(); - break; + break; case 'O': // Rate Control PID Kp_RateRoll = readFloatSerial(); Ki_RateRoll = readFloatSerial(); @@ -126,7 +126,7 @@ void readSerialCommand() { Kd_RateYaw = readFloatSerial(); xmitFactor = readFloatSerial(); break; - case 'W': // Write all user configurable values to EEPROM + case 'W': // Write all user configurable values to EEPROM writeUserConfig(); break; case 'Y': // Initialize EEPROM with default values @@ -145,7 +145,7 @@ void readSerialCommand() { ch_aux_offset = readFloatSerial(); ch_aux2_slope = readFloatSerial(); ch_aux2_offset = readFloatSerial(); - break; + break; } } } @@ -154,34 +154,34 @@ void sendSerialTelemetry() { float aux_float[3]; // used for sensor calibration switch (queryType) { case '=': // Reserved debug command to view any variable from Serial Monitor -/* SerPri(("throttle ="); - SerPrln(ch_throttle); - SerPri("control roll ="); - SerPrln(control_roll-CHANN_CENTER); - SerPri("control pitch ="); - SerPrln(control_pitch-CHANN_CENTER); - SerPri("control yaw ="); - SerPrln(control_yaw-CHANN_CENTER); - SerPri("front left yaw ="); - SerPrln(frontMotor); - SerPri("front right yaw ="); - SerPrln(rightMotor); - SerPri("rear left yaw ="); - SerPrln(leftMotor); - SerPri("rear right motor ="); - SerPrln(backMotor); - SerPrln(); - - SerPri("current roll rate ="); - SerPrln(read_adc(0)); - SerPri("current pitch rate ="); - SerPrln(read_adc(1)); - SerPri("current yaw rate ="); - SerPrln(read_adc(2)); - SerPri("command rx yaw ="); - SerPrln(command_rx_yaw); - SerPrln(); - queryType = 'X';*/ + /* SerPri(("throttle ="); + SerPrln(ch_throttle); + SerPri("control roll ="); + SerPrln(control_roll-CHANN_CENTER); + SerPri("control pitch ="); + SerPrln(control_pitch-CHANN_CENTER); + SerPri("control yaw ="); + SerPrln(control_yaw-CHANN_CENTER); + SerPri("front left yaw ="); + SerPrln(frontMotor); + SerPri("front right yaw ="); + SerPrln(rightMotor); + SerPri("rear left yaw ="); + SerPrln(leftMotor); + SerPri("rear right motor ="); + SerPrln(backMotor); + SerPrln(); + + SerPri("current roll rate ="); + SerPrln(read_adc(0)); + SerPri("current pitch rate ="); + SerPrln(read_adc(1)); + SerPri("current yaw rate ="); + SerPrln(read_adc(2)); + SerPri("command rx yaw ="); + SerPrln(command_rx_yaw); + SerPrln(); + queryType = 'X';*/ SerPri(APM_RC.InputCh(0)); comma(); SerPri(ch_roll_slope); @@ -268,7 +268,7 @@ void sendSerialTelemetry() { queryType = 'X'; break; case 'L': // Spare -// RadioCalibration(); + // RadioCalibration(); queryType = 'X'; break; case 'N': // Send magnetometer config @@ -351,6 +351,20 @@ void sendSerialTelemetry() { SerPri(read_adc(3)); comma(); SerPrln(read_adc(5)); +/* comma(); + SerPri(APM_Compass.Heading, 4); + comma(); + SerPri(APM_Compass.Heading_X, 4); + comma(); + SerPri(APM_Compass.Heading_Y, 4); + comma(); + SerPri(APM_Compass.Mag_X); + comma(); + SerPri(APM_Compass.Mag_Y); + comma(); + SerPri(APM_Compass.Mag_Z); + comma(); +*/ break; case 'T': // Spare break; @@ -406,7 +420,7 @@ void sendSerialTelemetry() { comma(); SerPrln(ch_aux2_offset); queryType = 'X'; - break; + break; case '.': // Modify GPS settings, print directly to GPS Port Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n"); break; @@ -438,3 +452,4 @@ float readFloatSerial() { while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128)); return atof(data); } + diff --git a/ArducopterNG/System.pde b/ArducopterNG/System.pde index 3f265dd92f..72332c4098 100644 --- a/ArducopterNG/System.pde +++ b/ArducopterNG/System.pde @@ -85,9 +85,14 @@ void APM_Init() { DataFlash.StartWrite(1); // Start a write session on page 1 - Serial.begin(115200); - //Serial.println("ArduCopter Quadcopter v1.0"); +// Serial.begin(115200); // Old mode serial begin, remove soon, by jp/17-10-10 + //Serial.println("ArduCopter Quadcopter v1.0"); + + // Proper Serial port/baud are defined on main .pde and then Arducopter.h with + // Choises of Xbee or + SerBeg(SerBau); + // 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) @@ -126,5 +131,6 @@ void APM_Init() { digitalWrite(RI_LED, HIGH); digitalWrite(LE_LED, HIGH); #endif + } - +