Some bug corrections on startup

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

ArducopterNG/ArduUser.h

@@ -37,8 +37,8 @@ TODO:
 // Safety & Security 
 
 // Arm & Disarm delays
-#define ARM_DELAY 200      // milliseconds of how long you need to keep rudder to max right for arming motors
+#define ARM_DELAY 50      // how long you need to keep rudder to max right for arming motors (units*0.02, 50=1second)
-#define DISARM_DELAY 100   // milliseconds of how long you need to keep rudder to max left for disarming motors
+#define DISARM_DELAY 25   // how long you need to keep rudder to max left for disarming motors
 
 /*************************************************************/
 // AM Mode & Flight information 
@@ -83,6 +83,8 @@ TODO:
 //Modes
 #define STABLE_MODE 0
 #define ACRO_MODE 1
+#define AP_NORMAL_MODE 0       // AP disabled => manual flight
+#define AP_AUTOMATIC_MODE 1    // AP Enabled  => Automatic mode (GPS position hold)
 
 //Axis
 #define ROLL 0

ArducopterNG/Arducopter.h

@@ -192,7 +192,7 @@ float command_rx_yaw_diff;
 int control_roll;           // PID control results
 int control_pitch;
 int control_yaw;
-float K_aux;
+//float K_aux;
 
 // Attitude PID controls
 float roll_I=0;
@@ -206,11 +206,9 @@ float yaw_D;
 float err_yaw;
 
 //Position control
-#ifdef IsGPS
 long target_longitude;
 long target_lattitude;
 byte target_position;
-#endif
 float gps_err_roll;
 float gps_err_roll_old;
 float gps_roll_D;
@@ -232,15 +230,8 @@ float altitude_I;
 float altitude_D;
 
 //Pressure Sensor variables
-#ifdef UseBMP
+long target_baro_altitude;
-unsigned long abs_press 	        = 0;    
+long 	press_alt			= 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

ArducopterNG/ArducopterNG.pde

@@ -80,14 +80,14 @@
 #include "Arducopter.h"
 #include "ArduUser.h"
 
-// GPS
+// GPS library (Include only one library)
 #include <GPS_MTK.h>		// ArduPilot MTK GPS Library
 //#include <GPS_IMU.h>		// ArduPilot IMU/SIM GPS Library
 //#include <GPS_UBLOX.h>	// ArduPilot Ublox GPS Library
 //#include <GPS_NMEA.h> 	// ArduPilot NMEA GPS library
 
 /* Software version */
-#define VER 0.1    // Current software version (only numeric values)
+#define VER 1.5    // Current software version (only numeric values)
 
 /* ************************************************************ */
 /* ************* MAIN PROGRAM - DECLARATIONS ****************** */
@@ -95,12 +95,10 @@
 
 byte flightMode;
 
-unsigned long currentTime, previousTime, deltaTime;
+unsigned long currentTime, previousTime;
 unsigned long mainLoop = 0;
-unsigned long sensorLoop = 0;
+unsigned long mediumLoop = 0;
-unsigned long controlLoop = 0;
+unsigned long slowLoop = 0;
-unsigned long radioLoop = 0;
-unsigned long motorLoop = 0;
 
 /* ************************************************************ */
 /* **************** MAIN PROGRAM - SETUP ********************** */
@@ -109,63 +107,54 @@ void setup() {
   
   APM_Init();    // APM Hardware initialization (in System.pde)
   
-  previousTime = millis();
+  mainLoop = millis();              // Initialize timers
+  mediumLoop = mainLoop;
+  GPS_timer = mainLoop;
   motorArmed = 0;
   Read_adc_raw();                   // Initialize ADC readings...
-  delay(20);
+  delay(10);
   digitalWrite(LED_Green,HIGH);     // Ready to go...  
 }
 
 
-
 /* ************************************************************ */
 /* ************** MAIN PROGRAM - MAIN LOOP ******************** */
 /* ************************************************************ */
 
-// fast rate
+// Sensor reading loop is inside APM_ADC and runs at 400Hz (based on Timer2 interrupt)
+
+// * fast rate loop => Main loop => 200Hz
 // read sensors
 // IMU : update attitude
 // motor control
-
+// Asyncronous task : read transmitter
-// medium rate
+// * medium rate loop (60Hz)
-// read transmitter
+// Asyncronous task : read GPS
+// * slow rate loop (10Hz)
 // magnetometer
-// barometer
+// barometer (20Hz)
-
-// slow rate
 // external command/telemetry
-// GPS
+// Battery monitor
 
 void loop()
 {
-  int aux;
+  //int aux;
-  int i;
+  //int i;
-  float aux_float;
+  //float aux_float;
 
   currentTime = millis();
-  //deltaTime = currentTime - previousTime;
-  //G_Dt = deltaTime / 1000.0;
-  //previousTime = currentTime;
   
-  // Sensor reading loop is inside APM_ADC and runs at 400Hz
   // Main loop at 200Hz (IMU + control)
-  if (currentTime > (mainLoop + 5))    // 200Hz (every 5ms)
+  if ((currentTime-mainLoop) > 5)    // 200Hz (every 5ms)
     {
-    G_Dt = (currentTime-mainLoop) / 1000.0;   // Microseconds!!!
+    G_Dt = (currentTime-mainLoop)*0.001;   // Microseconds!!!
     mainLoop = currentTime;
 
     //IMU DCM Algorithm
     Read_adc_raw();       // Read sensors raw data
     Matrix_update(); 
-    // Optimization: we don´t need to call this functions all the times
-    //if (IMU_cicle==0)
-    //  {
     Normalize();          
     Drift_correction();
-    //  IMU_cicle = 1;
-    //  }
-    //else
-    //  IMU_cicle = 0;
     Euler_angles();
 
     // Read radio values (if new data is available)
@@ -175,7 +164,7 @@ void loop()
     // Attitude control
     if(flightMode == STABLE_MODE) {    // STABLE Mode
       gled_speed = 1200;
-      if (AP_mode == 0)           // Normal mode
+      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);
@@ -190,31 +179,88 @@ void loop()
     // Send output commands to motor ESCs...
     motor_output();
     
-    // Performance optimization: Magnetometer sensor and pressure sensor are slowly to read (I2C)
+    // Autopilot mode functions
-    // so we read them at the end of the loop (all work is done in this loop run...)
+    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;
+          target_position=1;
+          //target_sonar_altitude = sonar_value;
+          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) {
-      if (MAG_counter > 20)  // Read compass data at 10Hz...
-      {
-        MAG_counter=0;
             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
-    #ifdef UseBMP
+          break;
-    #endif
+        case 2:  // Send serial telemetry (10Hz)
-    
+          medium_loopCounter++;
-    // Slow loop (10Hz)
-    if((currentTime-tlmTimer)>=100) {
-    //#if BATTERY_EVENT == 1
-    //  read_battery();         // Battery monitor
-    //#endif
           #ifdef CONFIGURATOR
-      readSerialCommand();
           sendSerialTelemetry();
           #endif
-      tlmTimer = currentTime;   
+          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;	
       }
     }
 }

ArducopterNG/Attitude.pde

@@ -167,7 +167,6 @@ void Rate_control_v2()
   previousYawRate = currentYawRate;
  
   // PID control
-  K_aux = KP_QUAD_YAW; // Comment this out if you want to use transmitter to adjust gain
   control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I; 
 }
 

ArducopterNG/Log.pde

@@ -277,7 +277,6 @@ void Log_Write_GPS(	long log_Time, long log_Lattitude, long log_Longitude, long
 	DataFlash.WriteLong(log_Ground_Speed);
 	DataFlash.WriteLong(log_Ground_Course);
 	DataFlash.WriteByte(END_BYTE);
-	DataFlash.WriteByte(END_BYTE);
 }
 #endif
 

ArducopterNG/Navigation.pde

@@ -32,3 +32,67 @@ TODO:
 - initial functions.
 
 * ************************************************************** */
+
+void read_GPS_data()
+{
+  GPS_timer_old=GPS_timer;   // Update GPS timer
+  GPS_timer = millis();
+  GPS_Dt = (GPS_timer-GPS_timer_old)*0.001;   // GPS_Dt
+  GPS.NewData=0;    // We Reset the flag...
+  
+  // Write GPS data to DataFlash log
+  Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude,GPS.Altitude,GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats);
+
+  //if (GPS.Fix >= 2)
+  if (GPS.Fix)
+    digitalWrite(LED_Red,HIGH);  // GPS Fix => RED LED
+  else
+    digitalWrite(LED_Red,LOW);
+}
+
+/* GPS based Position control */
+void Position_control(long lat_dest, long lon_dest)
+{
+  long Lon_diff;
+  long Lat_diff;
+
+  Lon_diff = lon_dest - GPS.Longitude;
+  Lat_diff = lat_dest - GPS.Lattitude;
+  
+  // ROLL
+  //Optimization : cos(yaw) = DCM_Matrix[0][0] ;  sin(yaw) = DCM_Matrix[1][0] 
+  gps_err_roll = (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[0][0] - (float)Lat_diff * DCM_Matrix[1][0];
+
+  gps_roll_D = (gps_err_roll-gps_err_roll_old) / GPS_Dt;
+  gps_err_roll_old = gps_err_roll;
+  
+  gps_roll_I += gps_err_roll * GPS_Dt;
+  gps_roll_I = constrain(gps_roll_I, -800, 800);
+
+  command_gps_roll = KP_GPS_ROLL * gps_err_roll + KD_GPS_ROLL * gps_roll_D + KI_GPS_ROLL * gps_roll_I;
+  command_gps_roll = constrain(command_gps_roll, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
+  
+  //Log_Write_PID(1,KP_GPS_ROLL*gps_err_roll*10,KI_GPS_ROLL*gps_roll_I*10,KD_GPS_ROLL*gps_roll_D*10,command_gps_roll*10);
+  
+  // PITCH
+  gps_err_pitch = -(float)Lat_diff * DCM_Matrix[0][0] - (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[1][0];
+
+  gps_pitch_D = (gps_err_pitch - gps_err_pitch_old) / GPS_Dt;
+  gps_err_pitch_old = gps_err_pitch;
+
+  gps_pitch_I += gps_err_pitch * GPS_Dt;
+  gps_pitch_I = constrain(gps_pitch_I, -800, 800);
+
+  command_gps_pitch = KP_GPS_PITCH * gps_err_pitch + KD_GPS_PITCH * gps_pitch_D + KI_GPS_PITCH * gps_pitch_I;
+  command_gps_pitch = constrain(command_gps_pitch, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
+
+  //Log_Write_PID(2,KP_GPS_PITCH*gps_err_pitch*10,KI_GPS_PITCH*gps_pitch_I*10,KD_GPS_PITCH*gps_pitch_D*10,command_gps_pitch*10);
+}
+
+void Reset_I_terms_navigation()
+{
+  altitude_I = 0;
+  gps_roll_I = 0;
+  gps_pitch_I = 0;  
+}
+

ArducopterNG/Radio.pde

@@ -48,7 +48,7 @@ void read_radio()
     ch_aux2 = APM_RC.InputCh(CH_6) * ch_aux2_slope + ch_aux2_offset;
     
     // Flight mode
-    if(ch_aux2 > 1800) 
+    if(ch_aux2 > 1200) 
       flightMode = ACRO_MODE;  // Force to Acro mode from radio
     else
       flightMode = STABLE_MODE;  // Stable mode (default)
@@ -57,12 +57,12 @@ void read_radio()
     if (flightMode == STABLE_MODE)
       {
       if(ch_aux > 1800)
-        AP_mode = 1;   // Automatic mode : GPS position hold mode + altitude hold
+        AP_mode = AP_AUTOMATIC_MODE;   // Automatic mode : GPS position hold mode + altitude hold
       else 
-        AP_mode = 0;   // Normal mode
+        AP_mode = AP_NORMAL_MODE;   // Normal mode
       }
       
-    if (flightMode==0)  // IN STABLE MODE we convert stick positions to absoulte angles
+    if (flightMode==STABLE_MODE)  // IN STABLE MODE we convert stick positions to absoulte angles
       {
       // In Stable mode stick position defines the desired angle in roll, pitch and yaw
       #ifdef FLIGHT_MODE_X
@@ -85,13 +85,50 @@ void read_radio()
       }
     
     // Write Radio data to DataFlash log
-    Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode);
+    Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,ch_aux,ch_aux2);
+    
+    // Motor arm logic
+    // Arm motor output : Throttle down and full yaw right for more than 2 seconds
+    if (ch_throttle < (MIN_THROTTLE + 100)) {
+      control_yaw = 0;
+      command_rx_yaw = ToDeg(yaw);
+      if (ch_yaw > 1850) {
+        if (Arming_counter > ARM_DELAY){
+          if(ch_throttle > 800) {
+          motorArmed = 1;
+          minThrottle = MIN_THROTTLE+60;  // A minimun value for mantain a bit if throttle
+          }
+        }
+        else
+          Arming_counter++;
+      }
+      else
+        Arming_counter=0;
+      // To Disarm motor output : Throttle down and full yaw left for more than 2 seconds
+      if (ch_yaw < 1150) {
+        if (Disarming_counter > DISARM_DELAY){
+          motorArmed = 0;
+          minThrottle = MIN_THROTTLE;
+        }
+        else
+          Disarming_counter++;
+      }
+      else
+        Disarming_counter=0;
+    }
+    else{
+      Arming_counter=0;
+      Disarming_counter=0;
+    }
 }
 
 
 // Send output commands to ESC´s
 void motor_output()
 {
+  if (ch_throttle < (MIN_THROTTLE + 100))  // If throttle is low we disable yaw (neccesary to arm/disarm motors safely)
+    control_yaw = 0;
+   
   // Quadcopter mix
   if (motorArmed == 1) 
     {   

ArducopterNG/Sensors.pde

@@ -81,3 +81,20 @@ void calibrateSensors(void) {
   for(gyro=GYROZ; gyro<=GYROY; gyro++)  
     AN_OFFSET[gyro]=aux_float[gyro];    // Update sensor OFFSETs from values read
 }
+
+#ifdef UseBMP
+void read_baro(void)
+{
+  float tempPresAlt;
+  
+  tempPresAlt = float(APM_BMP085.Press)/101325.0;
+  //tempPresAlt = pow(tempPresAlt, 0.190284);
+  //press_alt = (1.0 - tempPresAlt) * 145366.45;
+  tempPresAlt = pow(tempPresAlt, 0.190295);
+  if (press_alt==0)
+    press_alt = (1.0 - tempPresAlt) * 4433000;      // Altitude in cm
+  else
+    press_alt = press_alt*0.9 + ((1.0 - tempPresAlt) * 443300);  // Altitude in cm (filtered)
+}
+#endif
+

ArducopterNG/System.pde

@@ -43,10 +43,15 @@ void APM_Init() {
   pinMode(RELE_pin,OUTPUT);   // Rele output
   digitalWrite(RELE_pin,LOW);
  
-  //  delay(1000); // Wait until frame is not moving after initial power cord has connected
+  APM_RC.Init();             // APM Radio initialization
+  // RC channels Initialization (Quad motors)  
+  APM_RC.OutputCh(0,MIN_THROTTLE);  // Motors stoped
+  APM_RC.OutputCh(1,MIN_THROTTLE);
+  APM_RC.OutputCh(2,MIN_THROTTLE);
+  APM_RC.OutputCh(3,MIN_THROTTLE);
+  
   FullBlink(50,20);
   
-  APM_RC.Init();             // APM Radio initialization
   APM_ADC.Init();            // APM ADC library initialization
   DataFlash.Init();          // DataFlash log initialization