cleaning up code and adding functions from APM main code

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

Arducopter/ArduCopter.h

@@ -27,6 +27,12 @@
 // User configurable settings are on UserConfig.h
 /*******************************************************************/
 
+/**************************************************************/
+// Special features that might disapear in future releases
+
+//#define jpframe  // This is only Jani's special frame, you should never use unless you know what you are doing
+                 // As default this should be always checked off.
+
 
 /* APM Hardware definitions */
 #define LED_Yellow 36
@@ -48,12 +54,16 @@
 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};
 
+#ifndef jpframe
+int SENSOR_SIGN[]={
+  1, -1, -1,    -1, 1, 1,     -1, -1, -1}; 
+ //{-1,1,-1,1,-1,1,-1,-1,-1};
+#else
+int SENSOR_SIGN[]={
+  1, -1, 1,    -1, 1, 1,     -1, -1, -1}; 
+ //{-1,1,-1,1,-1,1,-1,-1,-1};
+#endif
 
 /* APM Hardware definitions, END */
 
@@ -115,30 +125,18 @@ float yaw=0;
 unsigned int counter = 0;
 
 float DCM_Matrix[3][3]= {
-  {
+  { 1,0,0 },
-    1,0,0  }
+  { 0,1,0 },
-  ,{
+  { 0,0,1 }}; 
-    0,1,0  }
-  ,{
-    0,0,1  }
-}; 
 float Update_Matrix[3][3]={
-  {
+  { 0,1,2 },
-    0,1,2  }
+  { 3,4,5 },
-  ,{
+  { 6,7,8 }}; //Gyros here
-    3,4,5  }
-  ,{
-    6,7,8  }
-}; //Gyros here
 
 float Temporary_Matrix[3][3]={
-  {
+  { 0,0,0 },
-    0,0,0  }
+  { 0,0,0 },
-  ,{
+  { 0,0,0 }};
-    0,0,0  }
-  ,{
-    0,0,0  }
-};
 
 // GPS variables
 float speed_3d=0;
@@ -200,6 +198,30 @@ float command_altitude;
 float altitude_I;
 float altitude_D;
 
+//Pressure Sensor variables
+#ifdef UseBMP
+unsigned long abs_press 	        = 0;    
+unsigned long abs_press_filt            = 0;
+unsigned long abs_press_gnd             = 0;
+int 	ground_temperature	        = 0;    // 
+int 	temp_unfilt			= 0;
+long 	ground_alt			= 0;	// Ground altitude from gps at startup in centimeters
+long 	press_alt			= 0;	// Pressure altitude 
+#endif
+
+
+#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
+
+#define AIRSPEED_PIN 1		// Need to correct value
+#define BATTERY_PIN 1		// Need to correct value
+#define RELAY_PIN 47
+#define LOW_VOLTAGE	11.4    // Pack voltage at which to trigger alarm
+#define INPUT_VOLTAGE 5.2	// (Volts) voltage your power regulator is feeding your ArduPilot to have an accurate pressure and battery level readings. (you need a multimeter to measure and set this of course)
+#define VOLT_DIV_RATIO 1.0	//  Voltage divider ratio set with thru-hole resistor (see manual)
+
+float 	battery_voltage 	= LOW_VOLTAGE * 1.05;		// Battery Voltage, initialized above threshold for filter
+
+
 // Sonar variables
 int Sonar_value=0;
 #define SonarToCm(x) (x*1.26)   // Sonar raw value to centimeters

Arducopter/Arducopter.pde

@@ -30,6 +30,11 @@
  GEAR OFF = Flight Assist (Stable Mode)
  
  **** LED Feedback ****
+ Bootup Sequence:
+ 1) A, B, C LED's blinking rapidly while waiting ESCs to bootup and initial shake to end from connecting battery
+ 2) A, B, C LED's have running light while calibrating Gyro/Acc's
+ 3) Green LED Solid after initialization finished
+ 
  Green LED On = APM Initialization Finished
  Yellow LED On = GPS Hold Mode
  Yellow LED Off = Flight Assist Mode (No GPS)
@@ -38,6 +43,7 @@
  
  Green LED blink slow = Motors armed, Stable mode
  Green LED blink rapid = Motors armed, Acro mode 
+
 */
 
 /* User definable modules */
@@ -51,6 +57,18 @@
 
 #define CONFIGURATOR  // Do se use Configurator or normal text output over serial link
 
+
+
+/**********************************************/
+// Not in use yet, starting to work with battery monitors and pressure sensors. 
+// Added 19-08-2010
+
+//#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!)
+
+/**********************************************/
+
 /* User definable modules - END */
 
 // Frame build condiguration
@@ -67,6 +85,9 @@
 #include <APM_RC.h>
 #include <DataFlash.h>
 #include <APM_Compass.h>
+#ifdef UseBMP
+#include <APM_BMP085.h>
+#endif
 
 //#include <GPS_NMEA.h>   // General NMEA GPS 
 #include <GPS_MTK.h>      // MediaTEK DIY Drones GPS. 
@@ -77,7 +98,8 @@
 #include "ArduCopter.h"
 #include "UserConfig.h"
 
-#define SWVER 1.31    // Current software version (only numeric values)
+/* Software version */
+#define VER 1.32    // Current software version (only numeric values)
 
 
 /* ***************************************************************************** */
@@ -85,7 +107,7 @@
 /* ***************************************************************************** */
 
 // Normal users does not need to edit anything below these lines. If you have
-// need, go and change them in FrameConfig.h
+// need, go and change them in UserConfig.h
 
 /* ************************************************************ */
 // STABLE MODE
@@ -234,7 +256,7 @@ int channel_filter(int ch, int ch_old)
 /* ************************************************************ */
 void setup()
 {
-  int i;
+  int i, j;
   float aux_float[3];
 
   pinMode(LED_Yellow,OUTPUT); //Yellow LED A  (PC1)
@@ -246,7 +268,17 @@ void setup()
   pinMode(RELE_pin,OUTPUT);   // Rele output
   digitalWrite(RELE_pin,LOW);
 
-  delay(1000); // Wait until frame is not moving after initial power cord has connected
+  //  delay(1000); // Wait until frame is not moving after initial power cord has connected
+  for(i = 0; i <= 50; i++) {
+    digitalWrite(LED_Green, HIGH);
+    digitalWrite(LED_Yellow, HIGH);
+    digitalWrite(LED_Red, HIGH);
+    delay(20);
+    digitalWrite(LED_Green, LOW);
+    digitalWrite(LED_Yellow, LOW);
+    digitalWrite(LED_Red, LOW);
+    delay(20);
+  }
 
   APM_RC.Init();             // APM Radio initialization
   APM_ADC.Init();            // APM ADC library initialization
@@ -269,7 +301,6 @@ void setup()
   if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
   if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
 
-
   // RC channels Initialization (Quad motors)  
   APM_RC.OutputCh(0,MIN_THROTTLE);  // Motors stoped
   APM_RC.OutputCh(1,MIN_THROTTLE);
@@ -306,6 +337,7 @@ void setup()
   aux_float[1] = gyro_offset_pitch;
   aux_float[2] = gyro_offset_yaw;
 
+  j = 0;
   // Take the gyro offset values
   for(i=0;i<300;i++)
   {
@@ -319,7 +351,30 @@ void setup()
     //Serial.println();
     Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
     delay(10);
+    
+    // Runnings lights effect to let user know that we are taking mesurements
+    if(j == 0) {
+      digitalWrite(LED_Green, HIGH);
+      digitalWrite(LED_Yellow, LOW);
+      digitalWrite(LED_Red, LOW);
     } 
+    else if (j == 1) {
+      digitalWrite(LED_Green, LOW);
+      digitalWrite(LED_Yellow, HIGH);
+      digitalWrite(LED_Red, LOW);
+    } 
+    else {
+      digitalWrite(LED_Green, LOW);
+      digitalWrite(LED_Yellow, LOW);
+      digitalWrite(LED_Red, HIGH);
+    }
+    if((i % 5) == 0) j++;
+    if(j >= 3) j = 0;
+  }
+  digitalWrite(LED_Green, LOW);
+  digitalWrite(LED_Yellow, LOW);
+  digitalWrite(LED_Red, LOW);
+
   for(int y=0; y<=2; y++)   
     AN_OFFSET[y]=aux_float[y];
 
@@ -330,7 +385,6 @@ void setup()
     Serial.print("AN[]:");
     Serial.println(AN_OFFSET[i]);
   }
-
   Serial.print("Yaw neutral value:");
   //  Serial.println(Neutro_yaw);
   Serial.print(yaw_mid);
@@ -367,9 +421,11 @@ void setup()
   Read_adc_raw();        // Initialize ADC readings...
   delay(20);
 
+#ifdef IsAM
   // Switch Left & Right lights on
   digitalWrite(RI_LED, HIGH);
   digitalWrite(LE_LED, HIGH); 
+#endif
 
   motorArmed = 0;
   digitalWrite(LED_Green,HIGH);     // Ready to go...
@@ -384,12 +440,12 @@ void loop(){
   int aux;
   int i;
   float aux_float;
+
   //Log variables
   int log_roll;
   int log_pitch;
   int log_yaw;
 
-
   if((millis()-timer)>=10)   // Main loop 100Hz
   {
     counter++;
@@ -409,12 +465,13 @@ void loop(){
       }
     }
 #endif    
+
     Matrix_update(); 
     Normalize();
     Drift_correction();
     Euler_angles();
-    // *****************
 
+    // *****************
     // Output data
     log_roll = ToDeg(roll) * 10;
     log_pitch = ToDeg(pitch) * 10;
@@ -604,8 +661,9 @@ void loop(){
     // Quadcopter mix
     // Ask Jose if we still need this IF statement, and if we want to do an ESC calibration
     if (motorArmed == 1) {   
+#ifdef IsAM
       digitalWrite(FR_LED, HIGH);    // AM-Mode
-     
+#endif
 #ifdef FLIGHT_MODE_+
       rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000);
       leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000);
@@ -620,7 +678,9 @@ void loop(){
 #endif
     }
     if (motorArmed == 0) {
+#ifdef IsAM
       digitalWrite(FR_LED, LOW);    // AM-Mode
+#endif
       digitalWrite(LED_Green,HIGH); // Ready LED on
 
       rightMotor = MIN_THROTTLE;
@@ -674,7 +734,10 @@ void loop(){
     } 
   }
 
+} // End of void loop()
+
+
+// END of Arducopter.pde
 
 
-}
 

Arducopter/Functions.pde

@@ -135,8 +135,14 @@ void comma() {
   Serial.print(',');
 }
 
-
+#if BATTERY_EVENT == 1
-
+void low_battery_event(void)
+{
+//	send_message(SEVERITY_HIGH,"Low Battery!");
+//	set_mode(RTL);
+//	throttle_cruise = THROTTLE_CRUISE;
+}
+#endif
 
 
 

Arducopter/SerialCom.pde

@@ -375,7 +375,7 @@ void sendSerialTelemetry() {
   case 'X': // Stop sending messages
     break;
   case '!': // Send flight software version
-    Serial.println(SWVER);
+    Serial.println(VER);
     queryType = 'X';
     break;
   case '.': // Modify GPS settings

Arducopter/UserConfig.h

@@ -26,12 +26,13 @@ TODO:
 ************************************************************* */
 
                  
+
 /*************************************************************/
 // Safety & Security 
 
 // Arm & Disarm delays
-#define ARM_DELAY 200
+#define ARM_DELAY 200      // milliseconds of how long you need to keep rudder to max right for arming motors
-#define DISARM_DELAY 100
+#define DISARM_DELAY 100   // milliseconds of how long you need to keep rudder to max left for disarming motors
 
 
 /*************************************************************/
@@ -57,10 +58,10 @@ TODO:
 
 //#define RADIO_TEST_MODE
 
-#define ROLL_MID 1478    // Radio Roll channel mid value
+#define ROLL_MID 1500    // Radio Roll channel mid value
-#define PITCH_MID 1483    // Radio Pitch channel mid value
+#define PITCH_MID 1500    // Radio Pitch channel mid value
 #define YAW_MID 1500    // Radio Yaw channel mid value
-#define THROTTLE_MID 1502    // Radio Throttle channel mid value
+#define THROTTLE_MID 1505    // Radio Throttle channel mid value
 #define AUX_MID 1500
 
 #define CHANN_CENTER 1500       // Channel center, legacy
@@ -68,25 +69,25 @@ TODO:
 
 // Following variables stored in EEPROM
 float KP_QUAD_ROLL;
-float KD_QUAD_ROLL;
 float KI_QUAD_ROLL;
+float KD_QUAD_ROLL;
 float KP_QUAD_PITCH;
-float KD_QUAD_PITCH;
 float KI_QUAD_PITCH;
+float KD_QUAD_PITCH;
 float KP_QUAD_YAW;
-float KD_QUAD_YAW;
 float KI_QUAD_YAW;
+float KD_QUAD_YAW;
 float STABLE_MODE_KP_RATE;
 float KP_GPS_ROLL;
-float KD_GPS_ROLL;
 float KI_GPS_ROLL;
+float KD_GPS_ROLL;
 float KP_GPS_PITCH;
-float KD_GPS_PITCH;
 float KI_GPS_PITCH;
+float KD_GPS_PITCH;
 float GPS_MAX_ANGLE;
 float KP_ALTITUDE;
-float KD_ALTITUDE;
 float KI_ALTITUDE;
+float KD_ALTITUDE;
 int acc_offset_x;
 int acc_offset_y;
 int acc_offset_z;
@@ -126,25 +127,25 @@ float ch_aux2_offset = 0;
 // when a "Default EEPROM Value" command is sent through serial interface
 void setUserConfig() {
   KP_QUAD_ROLL = 1.8;
-  KD_QUAD_ROLL = 0.4; //0.48
   KI_QUAD_ROLL = 0.30; //0.4
+  KD_QUAD_ROLL = 0.4; //0.48
   KP_QUAD_PITCH = 1.8;
-  KD_QUAD_PITCH = 0.4; //0.48
   KI_QUAD_PITCH = 0.30; //0.4
+  KD_QUAD_PITCH = 0.4; //0.48
   KP_QUAD_YAW = 3.6;
-  KD_QUAD_YAW = 1.2;
   KI_QUAD_YAW = 0.15;
+  KD_QUAD_YAW = 1.2;
   STABLE_MODE_KP_RATE = 0.2;  // New param for stable mode
-  KP_GPS_ROLL = 0.012;
+  KP_GPS_ROLL = 0.02;
-  KD_GPS_ROLL = 0.005;
+  KI_GPS_ROLL = 0.008;
-  KI_GPS_ROLL = 0.004;
+  KD_GPS_ROLL = 0.006;
-  KP_GPS_PITCH = 0.012;
+  KP_GPS_PITCH = 0.02;
-  KD_GPS_PITCH = 0.005;
+  KI_GPS_PITCH = 0.008;
-  KI_GPS_PITCH = 0.004;
+  KD_GPS_PITCH = 0.006;
-  GPS_MAX_ANGLE = 10;
+  GPS_MAX_ANGLE = 18;
   KP_ALTITUDE = 0.8;
-  KD_ALTITUDE = 0.2;
   KI_ALTITUDE = 0.2;
+  KD_ALTITUDE = 0.2;
   acc_offset_x = 2073;
   acc_offset_y = 2056;
   acc_offset_z = 2010;
@@ -186,25 +187,25 @@ void setUserConfig() {
 
 // EEPROM storage addresses
 #define KP_QUAD_ROLL_ADR 0
-#define KD_QUAD_ROLL_ADR 4
 #define KI_QUAD_ROLL_ADR 8
+#define KD_QUAD_ROLL_ADR 4
 #define KP_QUAD_PITCH_ADR 12
-#define KD_QUAD_PITCH_ADR 16
 #define KI_QUAD_PITCH_ADR 20
+#define KD_QUAD_PITCH_ADR 16
 #define KP_QUAD_YAW_ADR 24
-#define KD_QUAD_YAW_ADR 28
 #define KI_QUAD_YAW_ADR 32
+#define KD_QUAD_YAW_ADR 28
 #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 KD_GPS_ROLL_ADR 44
 #define KP_GPS_PITCH_ADR 52
-#define KD_GPS_PITCH_ADR 56
 #define KI_GPS_PITCH_ADR 60
+#define KD_GPS_PITCH_ADR 56
 #define GPS_MAX_ANGLE_ADR 64
 #define KP_ALTITUDE_ADR 68
-#define KD_ALTITUDE_ADR 72
 #define KI_ALTITUDE_ADR 76
+#define KD_ALTITUDE_ADR 72
 #define acc_offset_x_ADR 80
 #define acc_offset_y_ADR 84
 #define acc_offset_z_ADR 88
@@ -268,25 +269,25 @@ void writeEEPROM(float value, int address) {
 
 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);
+  KD_QUAD_ROLL = readEEPROM(KD_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);
+  KD_QUAD_PITCH = readEEPROM(KD_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);
+  KD_QUAD_YAW = readEEPROM(KD_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);
+  KD_GPS_ROLL = readEEPROM(KD_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);
+  KD_GPS_PITCH = readEEPROM(KD_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);
+  KD_ALTITUDE = readEEPROM(KD_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);