Rearranging variables and new header files

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

Arducopter/ArduCopter.h

@@ -0,0 +1,399 @@
+/*
+  ArduCopter 1.3 - Aug 2010
+  www.ArduCopter.com
+  Copyright (c) 2010. All rights reserved.
+  An Open Source Arduino based multicopter.
+ 
+  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 
+  the Free Software Foundation, either version 3 of the License, or 
+  (at your option) any later version. 
+
+  This program is distributed in the hope that it will be useful, 
+  but WITHOUT ANY WARRANTY; without even the implied warranty of 
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
+  GNU General Public License for more details. 
+
+  You should have received a copy of the GNU General Public License 
+  along with this program. If not, see <http://www.gnu.org/licenses/>. 
+*/
+
+#include "WProgram.h"
+
+/*******************************************************************/
+// ArduPilot Mega specific hardware and software settings
+// 
+// DO NOT EDIT unless you are absolytely sure of your doings. 
+// User configurable settings are on UserConfig.h
+/*******************************************************************/
+
+
+/* APM Hardware definitions */
+#define LED_Yellow 36
+#define LED_Red 35
+#define LED_Green 37
+#define RELE_pin 47
+#define SW1_pin 41
+#define SW2_pin 40
+
+//#define VDIV1 AN1
+//#define VDIV2 AN2
+//#define VDIV3 AN3
+//#define VDIV4 AN4
+
+//#define AN5
+//#define AN6
+
+// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
+uint8_t sensors[6] = {1, 2, 0, 4, 5, 6};  // For ArduPilot Mega Sensor Shield Hardware
+
+// Sensor: GYROX, GYROY, GYROZ,   ACCELX, ACCELY, ACCELZ,     MAGX, MAGY, MAGZ
+int SENSOR_SIGN[]={
+  1, -1, -1,         // GYROX, GYROY, GYROZ
+ -1, 1, 1,           // ACCELX, ACCELY, ACCELZ
+ -1, -1, -1};        // MAGNETOX, MAGNETOY, MAGNETOZ
+ //{-1,1,-1,1,-1,1,-1,-1,-1};
+
+
+/* APM Hardware definitions, END */
+
+/* General definitions */
+
+#define TRUE 1
+#define FALSE 0
+#define ON 1
+#define OFF 0
+
+
+// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step
+// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
+// Tested value : 408
+#define GRAVITY 408 //this equivalent to 1G in the raw data coming from the accelerometer 
+#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
+
+#define ToRad(x) (x*0.01745329252)  // *pi/180
+#define ToDeg(x) (x*57.2957795131)  // *180/pi
+
+// IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4
+// Tested values : 
+#define Gyro_Gain_X 0.4  //X axis Gyro gain
+#define Gyro_Gain_Y 0.41 //Y axis Gyro gain
+#define Gyro_Gain_Z 0.41 //Z axis Gyro gain
+#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
+
+/*For debugging purposes*/
+#define OUTPUTMODE 1  //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data
+
+int AN[6]; //array that store the 6 ADC channels
+int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers
+int gyro_temp;
+
+
+float G_Dt=0.02;    // Integration time for the gyros (DCM algorithm)
+float Accel_Vector[3]= {0, 0, 0}; //Store the acceleration in a vector
+float Accel_Vector_unfiltered[3]= {0, 0, 0}; //Store the acceleration in a vector
+float Gyro_Vector[3]= {0, 0, 0};//Store the gyros rutn rate in a vector
+float Omega_Vector[3]= {0, 0, 0}; //Corrected Gyro_Vector data
+float Omega_P[3]= {0, 0, 0};//Omega Proportional correction
+float Omega_I[3]= {0, 0, 0};//Omega Integrator
+float Omega[3]= {0, 0, 0};
+//float Accel_magnitude;
+//float Accel_weight;
+
+float errorRollPitch[3]= {0, 0, 0};
+float errorYaw[3]= {0, 0, 0};
+float errorCourse=0;
+float COGX=0; //Course overground X axis
+float COGY=1; //Course overground Y axis
+
+float roll=0;
+float pitch=0;
+float yaw=0;
+
+unsigned int counter=0;
+
+float DCM_Matrix[3][3]= {
+  {
+    1,0,0  }
+  ,{
+    0,1,0  }
+  ,{
+    0,0,1  }
+}; 
+float Update_Matrix[3][3]={
+  {
+    0,1,2  }
+  ,{
+    3,4,5  }
+  ,{
+    6,7,8  }
+}; //Gyros here
+
+float Temporary_Matrix[3][3]={
+  {
+    0,0,0  }
+  ,{
+    0,0,0  }
+  ,{
+    0,0,0  }
+};
+
+// GPS variables
+float speed_3d=0;
+int GPS_ground_speed=0;
+
+long timer=0; //general porpuse timer 
+long timer_old;
+
+// Attitude control variables
+float command_rx_roll=0;        // User commands
+float command_rx_roll_old;
+float command_rx_roll_diff;
+float command_rx_pitch=0;
+float command_rx_pitch_old;
+float command_rx_pitch_diff;
+float command_rx_yaw=0;
+float command_rx_yaw_diff;
+int control_roll;           // PID control results
+int control_pitch;
+int control_yaw;
+float K_aux;
+
+// Attitude PID controls
+float roll_I=0;
+float roll_D;
+float err_roll;
+float pitch_I=0;
+float pitch_D;
+float err_pitch;
+float yaw_I=0;
+float yaw_D;
+float err_yaw;
+
+//Position control
+long target_longitude;
+long target_lattitude;
+byte target_position;
+float gps_err_roll;
+float gps_err_roll_old;
+float gps_roll_D;
+float gps_roll_I=0;
+float gps_err_pitch;
+float gps_err_pitch_old;
+float gps_pitch_D;
+float gps_pitch_I=0;
+float command_gps_roll;
+float command_gps_pitch;
+
+//Altitude control
+int Initial_Throttle;
+int target_sonar_altitude;
+int err_altitude;
+int err_altitude_old;
+float command_altitude;
+float altitude_I;
+float altitude_D;
+
+// Sonar variables
+int Sonar_value=0;
+#define SonarToCm(x) (x*1.26)   // Sonar raw value to centimeters
+int Sonar_Counter=0;
+
+// AP_mode : 1=> Position hold  2=>Stabilization assist mode (normal mode)
+byte AP_mode = 2;  
+
+// Mode LED timers and variables, used to blink LED_Green
+byte gled_status = HIGH;
+long gled_timer;
+int gled_speed;
+
+long t0;
+int num_iter;
+float aux_debug;
+
+// Radio definitions
+int Neutro_yaw;
+int ch_roll;
+int ch_pitch;
+int ch_throttle;
+int ch_yaw;
+int ch_aux;
+int ch_aux2;
+
+int frontMotor;
+int backMotor;
+int leftMotor;
+int rightMotor;
+byte motorArmed = 0;
+int minThrottle = 0;
+
+// Serial communication
+char queryType;
+long tlmTimer = 0;
+
+// Arming/Disarming
+uint8_t Arming_counter=0;
+uint8_t Disarming_counter=0;
+
+
+
+/*****************************************************/
+// APM Specific Memory variables
+
+// Following variables stored in EEPROM
+float KP_QUAD_ROLL;
+float KD_QUAD_ROLL;
+float KI_QUAD_ROLL;
+float KP_QUAD_PITCH;
+float KD_QUAD_PITCH;
+float KI_QUAD_PITCH;
+float KP_QUAD_YAW;
+float KD_QUAD_YAW;
+float KI_QUAD_YAW;
+float STABLE_MODE_KP_RATE;
+float KP_GPS_ROLL;
+float KD_GPS_ROLL;
+float KI_GPS_ROLL;
+float KP_GPS_PITCH;
+float KD_GPS_PITCH;
+float KI_GPS_PITCH;
+float GPS_MAX_ANGLE;
+float KP_ALTITUDE;
+float KD_ALTITUDE;
+float KI_ALTITUDE;
+int acc_offset_x;
+int acc_offset_y;
+int acc_offset_z;
+int gyro_offset_roll;
+int gyro_offset_pitch;
+int gyro_offset_yaw;
+float Kp_ROLLPITCH;
+float Ki_ROLLPITCH;
+float Kp_YAW;
+float Ki_YAW;
+float GEOG_CORRECTION_FACTOR;
+int MAGNETOMETER;
+float Kp_RateRoll;
+float Ki_RateRoll;
+float Kd_RateRoll;
+float Kp_RatePitch;
+float Ki_RatePitch;
+float Kd_RatePitch;
+float Kp_RateYaw;
+float Ki_RateYaw;
+float Kd_RateYaw;
+float xmitFactor;
+
+// EEPROM storage addresses
+#define KP_QUAD_ROLL_ADR 0
+#define KD_QUAD_ROLL_ADR 4
+#define KI_QUAD_ROLL_ADR 8
+#define KP_QUAD_PITCH_ADR 12
+#define KD_QUAD_PITCH_ADR 16
+#define KI_QUAD_PITCH_ADR 20
+#define KP_QUAD_YAW_ADR 24
+#define KD_QUAD_YAW_ADR 28
+#define KI_QUAD_YAW_ADR 32
+#define STABLE_MODE_KP_RATE_ADR 36
+#define KP_GPS_ROLL_ADR 40
+#define KD_GPS_ROLL_ADR 44
+#define KI_GPS_ROLL_ADR 48
+#define KP_GPS_PITCH_ADR 52
+#define KD_GPS_PITCH_ADR 56
+#define KI_GPS_PITCH_ADR 60
+#define GPS_MAX_ANGLE_ADR 64
+#define KP_ALTITUDE_ADR 68
+#define KD_ALTITUDE_ADR 72
+#define KI_ALTITUDE_ADR 76
+#define acc_offset_x_ADR 80
+#define acc_offset_y_ADR 84
+#define acc_offset_z_ADR 88
+#define gyro_offset_roll_ADR 92
+#define gyro_offset_pitch_ADR 96
+#define gyro_offset_yaw_ADR 100
+#define Kp_ROLLPITCH_ADR 104
+#define Ki_ROLLPITCH_ADR 108
+#define Kp_YAW_ADR 112
+#define Ki_YAW_ADR 116
+#define GEOG_CORRECTION_FACTOR_ADR 120
+#define MAGNETOMETER_ADR 124
+#define XMITFACTOR_ADR 128
+#define KP_RATEROLL_ADR 132
+#define KI_RATEROLL_ADR 136
+#define KD_RATEROLL_ADR 140
+#define KP_RATEPITCH_ADR 144
+#define KI_RATEPITCH_ADR 148
+#define KD_RATEPITCH_ADR 152
+#define KP_RATEYAW_ADR 156
+#define KI_RATEYAW_ADR 160
+#define KD_RATEYAW_ADR 164
+
+// Utilities for writing and reading from the EEPROM
+float readEEPROM(int address) {
+  union floatStore {
+    byte floatByte[4];
+    float floatVal;
+  } floatOut;
+  
+  for (int i = 0; i < 4; i++) 
+    floatOut.floatByte[i] = EEPROM.read(address + i);
+  return floatOut.floatVal;
+}
+
+void writeEEPROM(float value, int address) {
+  union floatStore {
+    byte floatByte[4];
+    float floatVal;
+  } floatIn;
+  
+  floatIn.floatVal = value;
+  for (int i = 0; i < 4; i++) 
+    EEPROM.write(address + i, floatIn.floatByte[i]);
+}
+
+void readUserConfig() {
+  KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR);
+  KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR);
+  KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR);
+  KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR);
+  KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR);
+  KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR);
+  KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
+  KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
+  KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR);
+  STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR);
+  KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
+  KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
+  KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);
+  KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR);
+  KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR);
+  KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR);
+  GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR);
+  KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR);
+  KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
+  KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR);
+  acc_offset_x = readEEPROM(acc_offset_x_ADR);
+  acc_offset_y = readEEPROM(acc_offset_y_ADR);
+  acc_offset_z = readEEPROM(acc_offset_z_ADR);
+  gyro_offset_roll = readEEPROM(gyro_offset_roll_ADR);
+  gyro_offset_pitch = readEEPROM(gyro_offset_pitch_ADR);
+  gyro_offset_yaw = readEEPROM(gyro_offset_yaw_ADR);
+  Kp_ROLLPITCH = readEEPROM(Kp_ROLLPITCH_ADR);
+  Ki_ROLLPITCH = readEEPROM(Ki_ROLLPITCH_ADR);
+  Kp_YAW = readEEPROM(Kp_YAW_ADR);
+  Ki_YAW = readEEPROM(Ki_YAW_ADR);
+  GEOG_CORRECTION_FACTOR = readEEPROM(GEOG_CORRECTION_FACTOR_ADR);
+  MAGNETOMETER = readEEPROM(MAGNETOMETER_ADR);
+  Kp_RateRoll = readEEPROM(KP_RATEROLL_ADR);
+  Ki_RateRoll = readEEPROM(KI_RATEROLL_ADR);
+  Kd_RateRoll = readEEPROM(KD_RATEROLL_ADR);
+  Kp_RatePitch = readEEPROM(KP_RATEPITCH_ADR);
+  Ki_RatePitch = readEEPROM(KI_RATEPITCH_ADR);
+  Kd_RatePitch = readEEPROM(KD_RATEPITCH_ADR);
+  Kp_RateYaw = readEEPROM(KP_RATEYAW_ADR);
+  Ki_RateYaw = readEEPROM(KI_RATEYAW_ADR);
+  Kd_RateYaw = readEEPROM(KD_RATEYAW_ADR);
+  xmitFactor = readEEPROM(XMITFACTOR_ADR);
+}

Arducopter/Arducopter.pde

@@ -1,5 +1,5 @@
 /* ********************************************************************** */
-/*                    ArduCopter Quadcopter code                          */
+/* j                   ArduCopter Quadcopter code                          */
 /*                                                                        */
 /* Quadcopter code from AeroQuad project and ArduIMU quadcopter project   */
 /* IMU DCM code from Diydrones.com                                        */
@@ -33,242 +33,60 @@
  Green LED On = APM Initialization Finished
  Yellow LED On = GPS Hold Mode
  Yellow LED Off = Flight Assist Mode (No GPS)
- Red LED On = GPS Fix
+ Red LED On = GPS Fix, 2D or 3D
  Red LED Off = No GPS Fix
- */
+
+ Green LED blink slow = Motors armed, Stable mode
+ Green LED blink rapid = Motors armed, Acro mode
+ 
+*/
+
+/* User definable modules */
+
+// Comment out with // modules that you are not using
+#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
+
+#define CONFIGURATOR  // Do se use Configurator or normal text output over serial link
+
+/* User definable modules - END */
+
+// 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
+
+
+/* ****************************************************************************** */
+/* ****************************** Includes ************************************** */
+/* ****************************************************************************** */
 
 #include <Wire.h>
 #include <APM_ADC.h>
 #include <APM_RC.h>
 #include <DataFlash.h>
 #include <APM_Compass.h>
-// Put your GPS library here:
+
-//#include <GPS_NMEA.h>  // MTK GPS
+//#include <GPS_NMEA.h>   // General NMEA GPS 
-#include <GPS_MTK.h>
+#include <GPS_MTK.h>      // MediaTEK DIY Drones GPS. 
-//#include <GPS_UBLOX.h>
+//#include <GPS_UBLOX.h>  // uBlox GPS
 
 // EEPROM storage for user configurable values
 #include <EEPROM.h>
-#include "UserSettings.h"
+#include "ArduCopter.h"
+#include "UserConfig.h"
 
-/* APM Hardware definitions */
+#define SWVER 1.31    // Current software version (only numeric values)
-#define LED_Yellow 36
-#define LED_Red 35
-#define LED_Green 37
-#define RELE_pin 47
-#define SW1_pin 41
-#define SW2_pin 40
-/* *** */
 
-/* AM PIN Definitions */
-/* Can be changed in future to AN extension ports */
-
-#define FR_LED 3  // Mega PE4 pin
-#define RE_LED 2  // Mega PE5 pin
-#define RI_LED 7  // Mega PH4 pin
-#define LE_LED 8  // Mega PH5 pin
-
-/* AM PIN Definitions - END */
 
 /* ***************************************************************************** */
-/*  CONFIGURATION PART                                                           */
+/* ************************ CONFIGURATION PART ********************************* */
 /* ***************************************************************************** */
 
-// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step
+// Normal users does not need to edit anything below these lines. If you have
-// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
+// need, go and change them in FrameConfig.h
-// Tested value : 408
-#define GRAVITY 408 //this equivalent to 1G in the raw data coming from the accelerometer 
-#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
-
-#define ToRad(x) (x*0.01745329252)  // *pi/180
-#define ToDeg(x) (x*57.2957795131)  // *180/pi
-
-// IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4
-// Tested values : 
-#define Gyro_Gain_X 0.4  //X axis Gyro gain
-#define Gyro_Gain_Y 0.41 //Y axis Gyro gain
-#define Gyro_Gain_Z 0.41 //Z axis Gyro gain
-#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
-#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
-#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
-
-/*For debugging purposes*/
-#define OUTPUTMODE 1  //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data
-
-//Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
-uint8_t sensors[6] = {
-  1,2,0,4,5,6};  // For ArduPilot Mega Sensor Shield Hardware
-
-//Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
-int SENSOR_SIGN[]={
-  1,-1,-1,-1,1,1,-1,-1,-1};  //{-1,1,-1,1,-1,1,-1,-1,-1};
-
-int AN[6]; //array that store the 6 ADC channels
-int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers
-int gyro_temp;
-
-
-float G_Dt=0.02;    // Integration time for the gyros (DCM algorithm)
-
-float Accel_Vector[3]= {
-  0,0,0}; //Store the acceleration in a vector
-float Accel_Vector_unfiltered[3]= {
-  0,0,0}; //Store the acceleration in a vector
-//float Accel_magnitude;
-//float Accel_weight;
-float Gyro_Vector[3]= {
-  0,0,0};//Store the gyros rutn rate in a vector
-float Omega_Vector[3]= {
-  0,0,0}; //Corrected Gyro_Vector data
-float Omega_P[3]= {
-  0,0,0};//Omega Proportional correction
-float Omega_I[3]= {
-  0,0,0};//Omega Integrator
-float Omega[3]= {
-  0,0,0};
-
-float errorRollPitch[3]= {
-  0,0,0};
-float errorYaw[3]= {
-  0,0,0};
-float errorCourse=0;
-float COGX=0; //Course overground X axis
-float COGY=1; //Course overground Y axis
-
-float roll=0;
-float pitch=0;
-float yaw=0;
-
-unsigned int counter=0;
-
-float DCM_Matrix[3][3]= {
-  {
-    1,0,0  }
-  ,{
-    0,1,0  }
-  ,{
-    0,0,1  }
-}; 
-float Update_Matrix[3][3]={
-  {
-    0,1,2  }
-  ,{
-    3,4,5  }
-  ,{
-    6,7,8  }
-}; //Gyros here
-
-float Temporary_Matrix[3][3]={
-  {
-    0,0,0  }
-  ,{
-    0,0,0  }
-  ,{
-    0,0,0  }
-};
-
-// GPS variables
-float speed_3d=0;
-int GPS_ground_speed=0;
-
-long timer=0; //general porpuse timer 
-long timer_old;
-
-// Attitude control variables
-float command_rx_roll=0;        // User commands
-float command_rx_roll_old;
-float command_rx_roll_diff;
-float command_rx_pitch=0;
-float command_rx_pitch_old;
-float command_rx_pitch_diff;
-float command_rx_yaw=0;
-float command_rx_yaw_diff;
-int control_roll;           // PID control results
-int control_pitch;
-int control_yaw;
-float K_aux;
-
-// Attitude PID controls
-float roll_I=0;
-float roll_D;
-float err_roll;
-float pitch_I=0;
-float pitch_D;
-float err_pitch;
-float yaw_I=0;
-float yaw_D;
-float err_yaw;
-
-//Position control
-long target_longitude;
-long target_lattitude;
-byte target_position;
-float gps_err_roll;
-float gps_err_roll_old;
-float gps_roll_D;
-float gps_roll_I=0;
-float gps_err_pitch;
-float gps_err_pitch_old;
-float gps_pitch_D;
-float gps_pitch_I=0;
-float command_gps_roll;
-float command_gps_pitch;
-
-//Altitude control
-int Initial_Throttle;
-int target_sonar_altitude;
-int err_altitude;
-int err_altitude_old;
-float command_altitude;
-float altitude_I;
-float altitude_D;
-
-// Sonar variables
-int Sonar_value=0;
-#define SonarToCm(x) (x*1.26)   // Sonar raw value to centimeters
-int Sonar_Counter=0;
-
-// AP_mode : 1=> Position hold  2=>Stabilization assist mode (normal mode)
-byte AP_mode = 2;  
-
-// Mode LED timers and variables, used to blink LED_Green
-byte gled_status = HIGH;
-long gled_timer;
-int gled_speed;
-
-long t0;
-int num_iter;
-float aux_debug;
-
-// Radio definitions
-int Neutro_yaw;
-int ch_roll;
-int ch_pitch;
-int ch_throttle;
-int ch_yaw;
-int ch_aux;
-int ch_aux2;
-#define CHANN_CENTER 1500
-#define MIN_THROTTLE 1040       // Throttle pulse width at minimun...
-
-// Motor variables
-#define FLIGHT_MODE_+
-//#define FLIGHT_MODE_X
-int frontMotor;
-int backMotor;
-int leftMotor;
-int rightMotor;
-byte motorArmed = 0;
-int minThrottle = 0;
-
-// Serial communication
-#define CONFIGURATOR
-char queryType;
-long tlmTimer = 0;
-
-// Arming/Disarming
-uint8_t Arming_counter=0;
-uint8_t Disarming_counter=0;
 
 /* ************************************************************ */
 /* Altitude control... (based on sonar) */
@@ -276,10 +94,10 @@ void Altitude_control(int target_sonar_altitude)
 {
   err_altitude_old = err_altitude;
   err_altitude = target_sonar_altitude - Sonar_value;  
-  altitude_D = (float)(err_altitude-err_altitude_old)/G_Dt;
+  altitude_D = (float)(err_altitude - err_altitude_old) / G_Dt;
-  altitude_I += (float)err_altitude*G_Dt;
+  altitude_I += (float)err_altitude * G_Dt;
-  altitude_I = constrain(altitude_I,-100,100);
+  altitude_I = constrain(altitude_I, -100, 100);
-  command_altitude = Initial_Throttle + KP_ALTITUDE*err_altitude + KD_ALTITUDE*altitude_D + KI_ALTITUDE*altitude_I;
+  command_altitude = Initial_Throttle + KP_ALTITUDE * err_altitude + KD_ALTITUDE * altitude_D + KI_ALTITUDE * altitude_I;
 }
 
 /* ************************************************************ */
@@ -320,6 +138,8 @@ void Position_control(long lat_dest, long lon_dest)
   command_gps_pitch = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
 }
 
+
+
 /* ************************************************************ */
 // STABLE MODE
 // ROLL, PITCH and YAW PID controls... 
@@ -332,19 +152,19 @@ void Attitude_control_v2()
   float pitch_rate;
 
   // ROLL CONTROL    
-  if (AP_mode==2)        // Normal Mode => Stabilization mode
+  if (AP_mode == 2)        // Normal Mode => Stabilization mode
     err_roll = command_rx_roll - ToDeg(roll);
   else
     err_roll = (command_rx_roll + command_gps_roll) - ToDeg(roll);  // Position control
 
-    err_roll = constrain(err_roll,-25,25);  // to limit max roll command...
+    err_roll = constrain(err_roll, -25, 25);  // to limit max roll command...
 
   // New control term...
   roll_rate = ToDeg(Omega[0]);  // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
-  err_roll_rate = ((ch_roll-1500)>>1) - roll_rate;
+  err_roll_rate = ((ch_roll - ROLL_MID) >> 1) - roll_rate;
 
-  roll_I += err_roll*G_Dt;
+  roll_I += err_roll * G_Dt;
-  roll_I = constrain(roll_I,-20,20);
+  roll_I = constrain(roll_I, -20, 20);
   // D term implementation => two parts: gyro part and command part
   // To have a better (faster) response we can use the Gyro reading directly for the Derivative term...
   // We also add a part that takes into account the command from user (stick) to make the system more responsive to user inputs
@@ -352,8 +172,7 @@ void Attitude_control_v2()
 
   // PID control
   K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain
-  control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I + STABLE_MODE_KP_RATE*err_roll_rate; 
+  control_roll = K_aux * err_roll + KD_QUAD_ROLL * roll_D + KI_QUAD_ROLL * roll_I + STABLE_MODE_KP_RATE * err_roll_rate;  
-  ; 
 
   // PITCH CONTROL
   if (AP_mode==2)        // Normal mode => Stabilization mode
@@ -365,7 +184,7 @@ void Attitude_control_v2()
 
   // New control term...
   pitch_rate = ToDeg(Omega[1]);
-  err_pitch_rate = ((ch_pitch-1500)>>1) - pitch_rate;
+  err_pitch_rate = ((ch_pitch - PITCH_MID) >> 1) - pitch_rate;
 
   pitch_I += err_pitch*G_Dt;
   pitch_I = constrain(pitch_I,-20,20);
@@ -374,7 +193,7 @@ void Attitude_control_v2()
 
   // PID control
   K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain
-  control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I + STABLE_MODE_KP_RATE*err_pitch_rate; 
+  control_pitch = K_aux * err_pitch + KD_QUAD_PITCH * pitch_D + KI_QUAD_PITCH * pitch_I + STABLE_MODE_KP_RATE * err_pitch_rate; 
 
   // YAW CONTROL
   err_yaw = command_rx_yaw - ToDeg(yaw);
@@ -383,14 +202,14 @@ void Attitude_control_v2()
   else if(err_yaw < -180)
     err_yaw += 360;
 
-  err_yaw = constrain(err_yaw,-60,60);  // to limit max yaw command...
+  err_yaw = constrain(err_yaw, -60, 60);  // to limit max yaw command...
 
-  yaw_I += err_yaw*G_Dt;
+  yaw_I += err_yaw * G_Dt;
-  yaw_I = constrain(yaw_I,-20,20);
+  yaw_I = constrain(yaw_I, -20, 20);
   yaw_D = - ToDeg(Omega[2]);
 
   // PID control
-  control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I;
+  control_yaw = KP_QUAD_YAW * err_yaw + KD_QUAD_YAW * yaw_D + KI_QUAD_YAW * yaw_I;
 }
 
 // ACRO MODE
@@ -402,20 +221,20 @@ void Rate_control()
   // ROLL CONTROL
   currentRollRate = read_adc(0);      // I need a positive sign here
 
-  err_roll = ((ch_roll-1500) * xmitFactor) - currentRollRate;
+  err_roll = ((ch_roll - ROLL_MID) * xmitFactor) - currentRollRate;
 
-  roll_I += err_roll*G_Dt;
+  roll_I += err_roll * G_Dt;
-  roll_I = constrain(roll_I,-20,20);
+  roll_I = constrain(roll_I, -20, 20);
 
   roll_D = currentRollRate - previousRollRate;
   previousRollRate = currentRollRate;
 
   // PID control
-  control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I; 
+  control_roll = Kp_RateRoll * err_roll + Kd_RateRoll * roll_D + Ki_RateRoll * roll_I; 
 
   // PITCH CONTROL
   currentPitchRate = read_adc(1);
-  err_pitch = ((ch_pitch-1500) * xmitFactor) - currentPitchRate;
+  err_pitch = ((ch_pitch - PITCH_MID) * xmitFactor) - currentPitchRate;
 
   pitch_I += err_pitch*G_Dt;
   pitch_I = constrain(pitch_I,-20,20);
@@ -428,7 +247,7 @@ void Rate_control()
 
   // YAW CONTROL
   currentYawRate = read_adc(2);
-  err_yaw = ((ch_yaw-1500)* xmitFactor) - currentYawRate;
+  err_yaw = ((ch_yaw - YAW_MID) * xmitFactor) - currentYawRate;
 
   yaw_I += err_yaw*G_Dt;
   yaw_I = constrain(yaw_I,-20,20);
@@ -459,12 +278,13 @@ int channel_filter(int ch, int ch_old)
     if (diff_ch_old>40)    
       return(ch_old+40);
   }
-  return((ch+ch_old)>>1);   // Small filtering
+  return((ch + ch_old) >> 1);   // Small filtering
   //return(ch);
 }
 
-
+/* ************************************************************ */
-/* ****** SETUP ********************************************************************* */
+/* **************** MAIN PROGRAM - SETUP ********************** */
+/* ************************************************************ */
 void setup()
 {
   int i;
@@ -474,17 +294,26 @@ void setup()
   pinMode(LED_Red,OUTPUT);    //Red LED B     (PC2)
   pinMode(LED_Green,OUTPUT);  //Green LED C   (PC0)
 
-  pinMode(SW1_pin,INPUT);  //Switch SW1 (pin PG0)
+  pinMode(SW1_pin,INPUT);     //Switch SW1 (pin PG0)
 
-  pinMode(RELE_pin,OUTPUT);      // Rele output
+  pinMode(RELE_pin,OUTPUT);   // Rele output
   digitalWrite(RELE_pin,LOW);
 
-  delay(250);
+  delay(1000); // Wait until frame is not moving after initial power cord has connected
 
-  APM_RC.Init();    // APM Radio initialization
+  APM_RC.Init();             // APM Radio initialization
-  APM_ADC.Init();   // APM ADC library initialization
+  APM_ADC.Init();            // APM ADC library initialization
-  DataFlash.Init(); // DataFlash log initialization
+  DataFlash.Init();          // DataFlash log initialization
-  GPS.Init();       // GPS Initialization
+
+#ifdef IsGPS  
+  GPS.Init();                // GPS Initialization
+#ifdef IsNEWMTEK  
+  delay(250);
+  // DIY Drones MTEK GPS needs binary sentences activated if you upgraded to latest firmware.
+  // If your GPS shows solid blue but LED C (Red) does not go on, your GPS is on NMEA mode
+  Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n"); 
+#endif
+#endif
 
   readUserConfig(); // Load user configurable items from EEPROM
 
@@ -513,7 +342,6 @@ void setup()
     delay(30000);
   }
 
-  //delay(3000);
 
   Read_adc_raw();
   delay(20);
@@ -594,7 +422,10 @@ void setup()
   digitalWrite(LED_Green,HIGH);     // Ready to go...
 }
 
-/* ***** MAIN LOOP ***** */
+
+/* ************************************************************ */
+/* ************** MAIN PROGRAM - MAIN LOOP ******************** */
+/* ************************************************************ */
 void loop(){
 
   int aux;
@@ -615,6 +446,7 @@ void loop(){
 
     // IMU DCM Algorithm
     Read_adc_raw();
+#ifdef IsMAG
     if (MAGNETOMETER == 1) {
       if (counter > 10)  // Read compass data at 10Hz... (10 loop runs)
       {
@@ -623,6 +455,7 @@ void loop(){
         APM_Compass.Calculate(roll,pitch);  // Calculate heading
       }
     }
+#endif    
     Matrix_update(); 
     Normalize();
     Drift_correction();
@@ -630,9 +463,9 @@ void loop(){
     // *****************
 
     // Output data
-    log_roll = ToDeg(roll)*10;
+    log_roll = ToDeg(roll) * 10;
-    log_pitch = ToDeg(pitch)*10;
+    log_pitch = ToDeg(pitch) * 10;
-    log_yaw = ToDeg(yaw)*10;
+    log_yaw = ToDeg(yaw) * 10;
 
 #ifndef CONFIGURATOR    
     Serial.print(log_roll);
@@ -657,19 +490,19 @@ void loop(){
     {
       // Commands from radio Rx... 
       // Stick position defines the desired angle in roll, pitch and yaw
-      ch_roll = channel_filter(APM_RC.InputCh(0),ch_roll);
+      ch_roll = channel_filter(APM_RC.InputCh(0), ch_roll);
-      ch_pitch = channel_filter(APM_RC.InputCh(1),ch_pitch);
+      ch_pitch = channel_filter(APM_RC.InputCh(1), ch_pitch);
-      ch_throttle = channel_filter(APM_RC.InputCh(2),ch_throttle);
+      ch_throttle = channel_filter(APM_RC.InputCh(2), ch_throttle);
-      ch_yaw = channel_filter(APM_RC.InputCh(3),ch_yaw);
+      ch_yaw = channel_filter(APM_RC.InputCh(3), ch_yaw);
       ch_aux = APM_RC.InputCh(4);
       ch_aux2 = APM_RC.InputCh(5);
       command_rx_roll_old = command_rx_roll;
-      command_rx_roll = (ch_roll-CHANN_CENTER)/12.0;
+      command_rx_roll = (ch_roll-CHANN_CENTER) / 12.0;
-      command_rx_roll_diff = command_rx_roll-command_rx_roll_old;
+      command_rx_roll_diff = command_rx_roll - command_rx_roll_old;
       command_rx_pitch_old = command_rx_pitch;
-      command_rx_pitch = (ch_pitch-CHANN_CENTER)/12.0;
+      command_rx_pitch = (ch_pitch-CHANN_CENTER) / 12.0;
-      command_rx_pitch_diff = command_rx_pitch-command_rx_pitch_old;
+      command_rx_pitch_diff = command_rx_pitch - command_rx_pitch_old;
-      aux_float = (ch_yaw-Neutro_yaw)/180.0;
+      aux_float = (ch_yaw-Neutro_yaw) / 180.0;
       command_rx_yaw += aux_float;
       command_rx_yaw_diff = aux_float;
       if (command_rx_yaw > 180)         // Normalize yaw to -180,180 degrees
@@ -680,7 +513,7 @@ void loop(){
       // Read through comments in Attitude_control() if you wish to use transmitter to adjust P gains
       // I use K_aux (channel 6) to adjust gains linked to a knob in the radio... [not used now]
       //K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2;
-      K_aux = K_aux*0.8 + ((ch_aux2-1500)/300.0 + 1.7)*0.2;   // /300 + 1.0
+      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;
@@ -703,12 +536,14 @@ void loop(){
       Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode);
     }  // END new radio data
 
+
     if (AP_mode==1)  // Position Control
     {
       if (target_position==0)   // If this is the first time we switch to Position control, actual position is our target position
       {
         target_lattitude = GPS.Lattitude;
         target_longitude = GPS.Longitude;
+
 #ifndef CONFIGURATOR
         Serial.println();
         Serial.print("* Target:");
@@ -743,14 +578,14 @@ void loop(){
       // 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);
 
-      if (GPS.Fix)
+      if (GPS.Fix >= 2)
         digitalWrite(LED_Red,HIGH);  // GPS Fix => Blue LED
       else
         digitalWrite(LED_Red,LOW);
 
       if (AP_mode==1)
       {
-        if ((target_position==1)&&(GPS.Fix))
+        if ((target_position==1) && (GPS.Fix))
         {
           Position_control(target_lattitude,target_longitude);  // Call position hold routine
         }
@@ -783,7 +618,7 @@ void loop(){
       command_rx_yaw = ToDeg(yaw);
       command_rx_yaw_diff = 0;
       if (ch_yaw > 1800) {
-        if (Arming_counter>240){
+        if (Arming_counter > ARM_DELAY){
           motorArmed = 1;
           minThrottle = 1100;
         }
@@ -794,7 +629,7 @@ void loop(){
         Arming_counter=0;
       // To Disarm motor output : Throttle down and full yaw left for more than 2 seconds
       if (ch_yaw < 1200) {
-        if (Disarming_counter>240){
+        if (Disarming_counter > DISARM_DELAY){
           motorArmed = 0;
           minThrottle = MIN_THROTTLE;
         }
@@ -846,6 +681,7 @@ void loop(){
     APM_RC.OutputCh(1, leftMotor);    // Left motor
     APM_RC.OutputCh(2, frontMotor);   // Front motor
     APM_RC.OutputCh(3, backMotor);   // Back motor     
+    
     // InstantPWM
     APM_RC.Force_Out0_Out1();
     APM_RC.Force_Out2_Out3();
@@ -862,17 +698,21 @@ void loop(){
   }
 #endif
 
-  // AM and Mode lights
+  // 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;
     } 
     else {
       digitalWrite(LED_Green, HIGH);
+#ifdef IsAM
       if(motorArmed) digitalWrite(RE_LED, HIGH);
+#endif
       gled_status = HIGH;
     } 
   }

Arducopter/DCM.pde

@@ -1,3 +1,24 @@
+/*
+ ArduCopter v1.3 - Aug 2010
+ www.ArduCopter.com
+ Copyright (c) 2010.  All rights reserved.
+ An Open Source Arduino based multicopter.
+ 
+ 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 
+ the Free Software Foundation, either version 3 of the License, or 
+ (at your option) any later version. 
+ 
+ This program is distributed in the hope that it will be useful, 
+ but WITHOUT ANY WARRANTY; without even the implied warranty of 
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
+ GNU General Public License for more details. 
+ 
+ You should have received a copy of the GNU General Public License 
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+ 
+ 
 /* ******* ADC functions ********************* */
 // Read all the ADC channles
 void Read_adc_raw(void)

Arducopter/SerialCom.pde

@@ -1,22 +1,22 @@
 /*
   ArduCopter v1.2 - June 2010
-  www.ArduCopter.com
+ www.ArduCopter.com
-  Copyright (c) 2010.  All rights reserved.
+ Copyright (c) 2010.  All rights reserved.
-  An Open Source Arduino based multicopter.
+ An Open Source Arduino based multicopter.
  
-  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 
-  the Free Software Foundation, either version 3 of the License, or 
+ the Free Software Foundation, either version 3 of the License, or 
-  (at your option) any later version. 
+ (at your option) any later version. 
  
-  This program is distributed in the hope that it will be useful, 
+ This program is distributed in the hope that it will be useful, 
-  but WITHOUT ANY WARRANTY; without even the implied warranty of 
+ but WITHOUT ANY WARRANTY; without even the implied warranty of 
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
-  GNU General Public License for more details. 
+ GNU General Public License for more details. 
  
-  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/>.
-*/
+ */
 
 void readSerialCommand() {
   // Check for serial message
@@ -182,19 +182,32 @@ 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("throttle =");
-    Serial.print("control roll =");Serial.println(control_roll-CHANN_CENTER);
+    Serial.println(ch_throttle);
-    Serial.print("control pitch =");Serial.println(control_pitch-CHANN_CENTER);
+    Serial.print("control roll =");
-    Serial.print("control yaw =");Serial.println(control_yaw-CHANN_CENTER);
+    Serial.println(control_roll-CHANN_CENTER);
-    Serial.print("front left yaw =");Serial.println(frontMotor);
+    Serial.print("control pitch =");
-    Serial.print("front right yaw =");Serial.println(rightMotor);
+    Serial.println(control_pitch-CHANN_CENTER);
-    Serial.print("rear left yaw =");Serial.println(leftMotor);
+    Serial.print("control yaw =");
-    Serial.print("rear right motor =");Serial.println(backMotor);Serial.println();
+    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();
 
-    Serial.print("current roll rate =");Serial.println(read_adc(0));
+    Serial.print("current roll rate =");
-    Serial.print("current pitch rate =");Serial.println(read_adc(1));
+    Serial.println(read_adc(0));
-    Serial.print("current yaw rate =");Serial.println(read_adc(2));
+    Serial.print("current pitch rate =");
-    Serial.print("command rx yaw =");Serial.println(command_rx_yaw);
+    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(); 
     queryType = 'X';
     break;
@@ -379,9 +392,12 @@ void sendSerialTelemetry() {
   case 'X': // Stop sending messages
     break;
   case '!': // Send flight software version
-    Serial.println("1.2");
+    Serial.println(SWVER);
     queryType = 'X';
     break;
+  case '.': // Modify GPS settings
+    Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
+    break;
   }
 }
 
@@ -401,10 +417,12 @@ float readFloatSerial() {
       timeout = 0;
       index++;
     }
-  }  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);
 }
 
 void comma() {
   Serial.print(',');
 }
+

Arducopter/UserConfig.h

@@ -0,0 +1,70 @@
+/*
+  ArduCopter v1.3 - Aug 2010
+ www.ArduCopter.com
+ Copyright (c) 2010.  All rights reserved.
+ An Open Source Arduino based multicopter.
+ 
+ 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 
+ the Free Software Foundation, either version 3 of the License, or 
+ (at your option) any later version. 
+ 
+ This program is distributed in the hope that it will be useful, 
+ but WITHOUT ANY WARRANTY; without even the implied warranty of 
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
+ GNU General Public License for more details. 
+ 
+ You should have received a copy of the GNU General Public License 
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+ 
+/* *************************************************************
+TODO:
+
+  - move all user definable variables from main pde to here
+  - comment variables properly
+
+
+************************************************************* */
+
+
+/*************************************************************/
+// Safety & Security 
+
+// Arm & Disarm delays
+#define ARM_DELAY 200
+#define DISARM_DELAY 100
+
+
+/*************************************************************/
+// AM Mode & Flight information 
+
+/* AM PIN Definitions */
+/* Will be moved in future to AN extension ports */
+/* due need to have PWM pins free for sonars and servos */
+
+#define FR_LED 3  // Mega PE4 pin, OUT7
+#define RE_LED 2  // Mega PE5 pin, OUT6
+#define RI_LED 7  // Mega PH4 pin, OUT5
+#define LE_LED 8  // Mega PH5 pin, OUT4
+
+/* AM PIN Definitions - END */
+
+
+/*************************************************************/
+// Radio related definitions
+
+// If you don't know these values, you can activate RADIO_TEST_MODE below
+// and check your mid values
+
+//#define RADIO_TEST_MODE
+
+#define ROLL_MID 1500    // Radio Roll channel mid value
+#define PITCH_MID 1500    // Radio Pitch channel mid value
+#define YAW_MID 1500    // Radio Yaw channel mid value
+#define THROTTLE_MID 1500    // Radio Throttle channel mid value
+#define AUX_MID 1500
+
+#define CHANN_CENTER 1500       // Channel center, legacy
+#define MIN_THROTTLE 1040       // Throttle pulse width at minimun...
+

Arducopter/UserSettings.h

@@ -1,177 +0,0 @@
-/*
-  ArduCopter v1.2
-  www.ArduCopter.com
-  Copyright (c) 2010. All rights reserved.
-  An Open Source Arduino based multicopter.
- 
-  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 
-  the Free Software Foundation, either version 3 of the License, or 
-  (at your option) any later version. 
-
-  This program is distributed in the hope that it will be useful, 
-  but WITHOUT ANY WARRANTY; without even the implied warranty of 
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
-  GNU General Public License for more details. 
-
-  You should have received a copy of the GNU General Public License 
-  along with this program. If not, see <http://www.gnu.org/licenses/>. 
-*/
-
-#include "WProgram.h"
-
-// Following variables stored in EEPROM
-float KP_QUAD_ROLL;
-float KD_QUAD_ROLL;
-float KI_QUAD_ROLL;
-float KP_QUAD_PITCH;
-float KD_QUAD_PITCH;
-float KI_QUAD_PITCH;
-float KP_QUAD_YAW;
-float KD_QUAD_YAW;
-float KI_QUAD_YAW;
-float STABLE_MODE_KP_RATE;
-float KP_GPS_ROLL;
-float KD_GPS_ROLL;
-float KI_GPS_ROLL;
-float KP_GPS_PITCH;
-float KD_GPS_PITCH;
-float KI_GPS_PITCH;
-float GPS_MAX_ANGLE;
-float KP_ALTITUDE;
-float KD_ALTITUDE;
-float KI_ALTITUDE;
-int acc_offset_x;
-int acc_offset_y;
-int acc_offset_z;
-int gyro_offset_roll;
-int gyro_offset_pitch;
-int gyro_offset_yaw;
-float Kp_ROLLPITCH;
-float Ki_ROLLPITCH;
-float Kp_YAW;
-float Ki_YAW;
-float GEOG_CORRECTION_FACTOR;
-int MAGNETOMETER;
-float Kp_RateRoll;
-float Ki_RateRoll;
-float Kd_RateRoll;
-float Kp_RatePitch;
-float Ki_RatePitch;
-float Kd_RatePitch;
-float Kp_RateYaw;
-float Ki_RateYaw;
-float Kd_RateYaw;
-float xmitFactor;
-
-// EEPROM storage addresses
-#define KP_QUAD_ROLL_ADR 0
-#define KD_QUAD_ROLL_ADR 4
-#define KI_QUAD_ROLL_ADR 8
-#define KP_QUAD_PITCH_ADR 12
-#define KD_QUAD_PITCH_ADR 16
-#define KI_QUAD_PITCH_ADR 20
-#define KP_QUAD_YAW_ADR 24
-#define KD_QUAD_YAW_ADR 28
-#define KI_QUAD_YAW_ADR 32
-#define STABLE_MODE_KP_RATE_ADR 36
-#define KP_GPS_ROLL_ADR 40
-#define KD_GPS_ROLL_ADR 44
-#define KI_GPS_ROLL_ADR 48
-#define KP_GPS_PITCH_ADR 52
-#define KD_GPS_PITCH_ADR 56
-#define KI_GPS_PITCH_ADR 60
-#define GPS_MAX_ANGLE_ADR 64
-#define KP_ALTITUDE_ADR 68
-#define KD_ALTITUDE_ADR 72
-#define KI_ALTITUDE_ADR 76
-#define acc_offset_x_ADR 80
-#define acc_offset_y_ADR 84
-#define acc_offset_z_ADR 88
-#define gyro_offset_roll_ADR 92
-#define gyro_offset_pitch_ADR 96
-#define gyro_offset_yaw_ADR 100
-#define Kp_ROLLPITCH_ADR 104
-#define Ki_ROLLPITCH_ADR 108
-#define Kp_YAW_ADR 112
-#define Ki_YAW_ADR 116
-#define GEOG_CORRECTION_FACTOR_ADR 120
-#define MAGNETOMETER_ADR 124
-#define XMITFACTOR_ADR 128
-#define KP_RATEROLL_ADR 132
-#define KI_RATEROLL_ADR 136
-#define KD_RATEROLL_ADR 140
-#define KP_RATEPITCH_ADR 144
-#define KI_RATEPITCH_ADR 148
-#define KD_RATEPITCH_ADR 152
-#define KP_RATEYAW_ADR 156
-#define KI_RATEYAW_ADR 160
-#define KD_RATEYAW_ADR 164
-
-// Utilities for writing and reading from the EEPROM
-float readEEPROM(int address) {
-  union floatStore {
-    byte floatByte[4];
-    float floatVal;
-  } floatOut;
-  
-  for (int i = 0; i < 4; i++) 
-    floatOut.floatByte[i] = EEPROM.read(address + i);
-  return floatOut.floatVal;
-}
-
-void writeEEPROM(float value, int address) {
-  union floatStore {
-    byte floatByte[4];
-    float floatVal;
-  } floatIn;
-  
-  floatIn.floatVal = value;
-  for (int i = 0; i < 4; i++) 
-    EEPROM.write(address + i, floatIn.floatByte[i]);
-}
-
-void readUserConfig() {
-  KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR);
-  KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR);
-  KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR);
-  KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR);
-  KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR);
-  KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR);
-  KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
-  KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
-  KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR);
-  STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR);
-  KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
-  KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
-  KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);
-  KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR);
-  KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR);
-  KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR);
-  GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR);
-  KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR);
-  KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
-  KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR);
-  acc_offset_x = readEEPROM(acc_offset_x_ADR);
-  acc_offset_y = readEEPROM(acc_offset_y_ADR);
-  acc_offset_z = readEEPROM(acc_offset_z_ADR);
-  gyro_offset_roll = readEEPROM(gyro_offset_roll_ADR);
-  gyro_offset_pitch = readEEPROM(gyro_offset_pitch_ADR);
-  gyro_offset_yaw = readEEPROM(gyro_offset_yaw_ADR);
-  Kp_ROLLPITCH = readEEPROM(Kp_ROLLPITCH_ADR);
-  Ki_ROLLPITCH = readEEPROM(Ki_ROLLPITCH_ADR);
-  Kp_YAW = readEEPROM(Kp_YAW_ADR);
-  Ki_YAW = readEEPROM(Ki_YAW_ADR);
-  GEOG_CORRECTION_FACTOR = readEEPROM(GEOG_CORRECTION_FACTOR_ADR);
-  MAGNETOMETER = readEEPROM(MAGNETOMETER_ADR);
-  Kp_RateRoll = readEEPROM(KP_RATEROLL_ADR);
-  Ki_RateRoll = readEEPROM(KI_RATEROLL_ADR);
-  Kd_RateRoll = readEEPROM(KD_RATEROLL_ADR);
-  Kp_RatePitch = readEEPROM(KP_RATEPITCH_ADR);
-  Ki_RatePitch = readEEPROM(KI_RATEPITCH_ADR);
-  Kd_RatePitch = readEEPROM(KD_RATEPITCH_ADR);
-  Kp_RateYaw = readEEPROM(KP_RATEYAW_ADR);
-  Ki_RateYaw = readEEPROM(KI_RATEYAW_ADR);
-  Kd_RateYaw = readEEPROM(KD_RATEYAW_ADR);
-  xmitFactor = readEEPROM(XMITFACTOR_ADR);
-}