Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Xbee/Ser0 telemetry ports are working again

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@682 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-10-17 17:34:17 +00:00
parent 455e315a1a
commit 35b754bbae
4 changed files with 199 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArducopterNG/Arducopter.h
View File

@ -75,7 +75,8 @@ TODO:
#define SERIAL2_BAUD 115200 #define SERIAL2_BAUD 115200
#define SERIAL3_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 SerPri Serial3.print
#define SerPrln Serial3.println #define SerPrln Serial3.println
#define SerRea Serial3.read #define SerRea Serial3.read
@ -86,11 +87,8 @@ TODO:
#define SerPor "FTDI" #define SerPor "FTDI"
#endif #endif
#ifndef SerPri // If we don't have SerPri set yet, it means we have are using Serial0 #ifndef SerXbee // If we don't have SerXbee set, it means we have are using Serial0
#define SerUSB //#define SerBau 115200 // Baud setting moved close next to port selection
#endif
#ifdef SerUSB // Defines for normal Serial0 port
#define SerPri Serial.print #define SerPri Serial.print
#define SerPrln Serial.println #define SerPrln Serial.println
#define SerRea Serial.read #define SerRea Serial.read

199
ArducopterNG/ArducopterNG.pde
View File

@ -6,9 +6,9 @@
File : ArducopterNG.pde File : ArducopterNG.pde
Version : v1.0, 11 October 2010 Version : v1.0, 11 October 2010
Author(s): ArduCopter Team Author(s): ArduCopter Team
Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz, Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni, Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson Sandro Benigno, Chris Anderson
This program is free software: you can redistribute it and/or modify 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 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 You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
/* ********************************************************************** */ /* ********************************************************************** */
/* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */ /* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */
/* Mounting position : RC connectors pointing backwards */ /* Mounting position : RC connectors pointing backwards */
@ -45,25 +45,41 @@
//#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 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 //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 IsAM // Do we have motormount LED's. AM = Atraction Mode
//#define UseAirspeed //#define UseAirspeed
//#define UseBMP //#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 #define CONFIGURATOR
////////////////////
// Telemetry
// Serial data, do we have FTDI cable or Xbee on Telemetry port as our primary command link // 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 SerXbee
//#define Ser0 // FTDI/USB Port Either one // Telemetry port speed, default is 115200
//#define Ser3 // Telemetry port #define SerBau 115200
//////////////////////
// Flight orientation
// Frame build condiguration // Frame build condiguration
#define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration #define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration
//#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms //#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms
/* ************************************************************ */ /* ************************************************************ */
/* **************** MAIN PROGRAM - INCLUDES ******************* */ /* **************** MAIN PROGRAM - INCLUDES ******************* */
/* ************************************************************ */ /* ************************************************************ */
@ -92,6 +108,7 @@
/* Software version */ /* Software version */
#define VER 1.5 // Current software version (only numeric values) #define VER 1.5 // Current software version (only numeric values)
/* ************************************************************ */ /* ************************************************************ */
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */ /* ************* MAIN PROGRAM - DECLARATIONS ****************** */
/* ************************************************************ */ /* ************************************************************ */
@ -107,9 +124,9 @@ unsigned long slowLoop = 0;
/* **************** MAIN PROGRAM - SETUP ********************** */ /* **************** MAIN PROGRAM - SETUP ********************** */
/* ************************************************************ */ /* ************************************************************ */
void setup() { void setup() {
APM_Init(); // APM Hardware initialization (in System.pde) APM_Init(); // APM Hardware initialization (in System.pde)
mainLoop = millis(); // Initialize timers mainLoop = millis(); // Initialize timers
mediumLoop = mainLoop; mediumLoop = mainLoop;
GPS_timer = mainLoop; GPS_timer = mainLoop;
@ -146,10 +163,10 @@ void loop()
//float aux_float; //float aux_float;
currentTime = millis(); currentTime = millis();
// Main loop at 200Hz (IMU + control) // Main loop at 200Hz (IMU + control)
if ((currentTime-mainLoop) > 5) // 200Hz (every 5ms) if ((currentTime-mainLoop) > 5) // 200Hz (every 5ms)
{ {
G_Dt = (currentTime-mainLoop)*0.001; // Microseconds!!! G_Dt = (currentTime-mainLoop)*0.001; // Microseconds!!!
mainLoop = currentTime; mainLoop = currentTime;
@ -170,31 +187,31 @@ void loop()
if (AP_mode == AP_NORMAL_MODE) // Normal mode if (AP_mode == AP_NORMAL_MODE) // Normal mode
Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw); Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw);
else // Automatic mode : GPS position hold mode 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 else { // ACRO Mode
gled_speed = 400; gled_speed = 400;
Rate_control_v2(); Rate_control_v2();
// Reset yaw, so if we change to stable mode we continue with the actual yaw direction // Reset yaw, so if we change to stable mode we continue with the actual yaw direction
command_rx_yaw = ToDeg(yaw); command_rx_yaw = ToDeg(yaw);
} }
// Send output commands to motor ESCs... // Send output commands to motor ESCs...
motor_output(); motor_output();
// Autopilot mode functions // Autopilot mode functions
if (AP_mode == AP_AUTOMATIC_MODE) if (AP_mode == AP_AUTOMATIC_MODE)
{ {
if (target_position) if (target_position)
{ {
if (GPS.NewData) // New GPS info? if (GPS.NewData) // New GPS info?
{ {
read_GPS_data(); read_GPS_data();
Position_control(target_lattitude,target_longitude); // Call GPS position hold routine 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 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... if (GPS.Fix){ // We need a GPS Fix to capture the actual position...
target_lattitude = GPS.Lattitude; target_lattitude = GPS.Lattitude;
target_longitude = GPS.Longitude; target_longitude = GPS.Longitude;
@ -203,68 +220,90 @@ void loop()
target_baro_altitude = press_alt; target_baro_altitude = press_alt;
Initial_Throttle = ch_throttle; Initial_Throttle = ch_throttle;
Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde) Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde)
} }
command_gps_roll=0; command_gps_roll=0;
command_gps_pitch=0; command_gps_pitch=0;
}
} }
}
else else
target_position=0; target_position=0;
} }
// Medium loop (about 60Hz) // Medium loop (about 60Hz)
if ((currentTime-mediumLoop)>=17){ if ((currentTime-mediumLoop)>=17){
mediumLoop = currentTime; mediumLoop = currentTime;
GPS.Read(); // Read GPS data GPS.Read(); // Read GPS data
// Each of the six cases executes at 10Hz // Each of the six cases executes at 10Hz
switch (medium_loopCounter){ switch (medium_loopCounter){
case 0: // Magnetometer reading (10Hz) case 0: // Magnetometer reading (10Hz)
medium_loopCounter++; medium_loopCounter++;
slowLoop++; slowLoop++;
#ifdef IsMAG #ifdef IsMAG
if (MAGNETOMETER == 1) { if (MAGNETOMETER == 1) {
APM_Compass.Read(); // Read magnetometer APM_Compass.Read(); // Read magnetometer
APM_Compass.Calculate(roll,pitch); // Calculate heading 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;
} }
#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;
}
}
} }

119
ArducopterNG/GCS.pde
View File

@ -6,9 +6,9 @@
File : GCS.pde File : GCS.pde
Version : v1.0, Aug 27, 2010 Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni, Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson Sandro Benigno, Chris Anderson
This program is free software: you can redistribute it and/or modify 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 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 You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
* ************************************************************** * * ************************************************************** *
ChangeLog: ChangeLog:
* ************************************************************** * * ************************************************************** *
TODO: TODO:
* ************************************************************** */ * ************************************************************** */
// //
// Function : send_message() // 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 void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05
{ {
if(severity >= DEBUG_LEVEL){ if(severity >= DEBUG_LEVEL){
SerPri("MSG: "); SerPri("MSG: ");
SerPrln(str); SerPrln(str);
} }
} }
@ -61,7 +61,7 @@ void send_message(byte severity, const char *str) // This is the instance of se
void readSerialCommand() { void readSerialCommand() {
// Check for serial message // Check for serial message
if (SerAva()) { if (SerAva()) {
queryType = SerRea();
switch (queryType) { switch (queryType) {
case 'A': // Stable PID case 'A': // Stable PID
KP_QUAD_ROLL = readFloatSerial(); KP_QUAD_ROLL = readFloatSerial();
@ -106,14 +106,14 @@ void readSerialCommand() {
acc_offset_z = readFloatSerial(); acc_offset_z = readFloatSerial();
break; break;
case 'K': // Spare case 'K': // Spare
break; break;
case 'M': // Receive debug motor commands case 'M': // Receive debug motor commands
frontMotor = readFloatSerial(); frontMotor = readFloatSerial();
backMotor = readFloatSerial(); backMotor = readFloatSerial();
rightMotor = readFloatSerial(); rightMotor = readFloatSerial();
leftMotor = readFloatSerial(); leftMotor = readFloatSerial();
motorArmed = readFloatSerial(); motorArmed = readFloatSerial();
break; break;
case 'O': // Rate Control PID case 'O': // Rate Control PID
Kp_RateRoll = readFloatSerial(); Kp_RateRoll = readFloatSerial();
Ki_RateRoll = readFloatSerial(); Ki_RateRoll = readFloatSerial();
@ -126,7 +126,7 @@ void readSerialCommand() {
Kd_RateYaw = readFloatSerial(); Kd_RateYaw = readFloatSerial();
xmitFactor = readFloatSerial(); xmitFactor = readFloatSerial();
break; break;
case 'W': // Write all user configurable values to EEPROM case 'W': // Write all user configurable values to EEPROM
writeUserConfig(); writeUserConfig();
break; break;
case 'Y': // Initialize EEPROM with default values case 'Y': // Initialize EEPROM with default values
@ -145,7 +145,7 @@ void readSerialCommand() {
ch_aux_offset = readFloatSerial(); ch_aux_offset = readFloatSerial();
ch_aux2_slope = readFloatSerial(); ch_aux2_slope = readFloatSerial();
ch_aux2_offset = readFloatSerial(); ch_aux2_offset = readFloatSerial();
break; break;
} }
} }
} }
@ -154,34 +154,34 @@ void sendSerialTelemetry() {
float aux_float[3]; // used for sensor calibration float aux_float[3]; // used for sensor calibration
switch (queryType) { switch (queryType) {
case '=': // Reserved debug command to view any variable from Serial Monitor case '=': // Reserved debug command to view any variable from Serial Monitor
/* SerPri(("throttle ="); /* SerPri(("throttle =");
SerPrln(ch_throttle); SerPrln(ch_throttle);
SerPri("control roll ="); SerPri("control roll =");
SerPrln(control_roll-CHANN_CENTER); SerPrln(control_roll-CHANN_CENTER);
SerPri("control pitch ="); SerPri("control pitch =");
SerPrln(control_pitch-CHANN_CENTER); SerPrln(control_pitch-CHANN_CENTER);
SerPri("control yaw ="); SerPri("control yaw =");
SerPrln(control_yaw-CHANN_CENTER); SerPrln(control_yaw-CHANN_CENTER);
SerPri("front left yaw ="); SerPri("front left yaw =");
SerPrln(frontMotor); SerPrln(frontMotor);
SerPri("front right yaw ="); SerPri("front right yaw =");
SerPrln(rightMotor); SerPrln(rightMotor);
SerPri("rear left yaw ="); SerPri("rear left yaw =");
SerPrln(leftMotor); SerPrln(leftMotor);
SerPri("rear right motor ="); SerPri("rear right motor =");
SerPrln(backMotor); SerPrln(backMotor);
SerPrln(); SerPrln();
SerPri("current roll rate ="); SerPri("current roll rate =");
SerPrln(read_adc(0)); SerPrln(read_adc(0));
SerPri("current pitch rate ="); SerPri("current pitch rate =");
SerPrln(read_adc(1)); SerPrln(read_adc(1));
SerPri("current yaw rate ="); SerPri("current yaw rate =");
SerPrln(read_adc(2)); SerPrln(read_adc(2));
SerPri("command rx yaw ="); SerPri("command rx yaw =");
SerPrln(command_rx_yaw); SerPrln(command_rx_yaw);
SerPrln(); SerPrln();
queryType = 'X';*/ queryType = 'X';*/
SerPri(APM_RC.InputCh(0)); SerPri(APM_RC.InputCh(0));
comma(); comma();
SerPri(ch_roll_slope); SerPri(ch_roll_slope);
@ -268,7 +268,7 @@ void sendSerialTelemetry() {
queryType = 'X'; queryType = 'X';
break; break;
case 'L': // Spare case 'L': // Spare
// RadioCalibration(); // RadioCalibration();
queryType = 'X'; queryType = 'X';
break; break;
case 'N': // Send magnetometer config case 'N': // Send magnetometer config
@ -351,6 +351,20 @@ void sendSerialTelemetry() {
SerPri(read_adc(3)); SerPri(read_adc(3));
comma(); comma();
SerPrln(read_adc(5)); 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; break;
case 'T': // Spare case 'T': // Spare
break; break;
@ -406,7 +420,7 @@ void sendSerialTelemetry() {
comma(); comma();
SerPrln(ch_aux2_offset); SerPrln(ch_aux2_offset);
queryType = 'X'; queryType = 'X';
break; break;
case '.': // Modify GPS settings, print directly to GPS Port case '.': // Modify GPS settings, print directly to GPS Port
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n"); Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
break; break;
@ -438,3 +452,4 @@ float readFloatSerial() {
while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128)); while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
return atof(data); return atof(data);
} }

12
ArducopterNG/System.pde
View File

@ -85,9 +85,14 @@ void APM_Init() {
DataFlash.StartWrite(1); // Start a write session on page 1 DataFlash.StartWrite(1); // Start a write session on page 1
Serial.begin(115200); // Serial.begin(115200); // Old mode serial begin, remove soon, by jp/17-10-10
//Serial.println("ArduCopter Quadcopter v1.0");
//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) // Check if we enable the DataFlash log Read Mode (switch)
// If we press switch 1 at startup we read the Dataflash eeprom // If we press switch 1 at startup we read the Dataflash eeprom
while (digitalRead(SW1_pin)==0) while (digitalRead(SW1_pin)==0)
@ -126,5 +131,6 @@ void APM_Init() {
digitalWrite(RI_LED, HIGH); digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH); digitalWrite(LE_LED, HIGH);
#endif #endif
} }