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
+
}
-
+