Main loop organization. NOT WORKING YET

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

ArducopterNG/ArducopterNG.pde

@@ -115,6 +115,7 @@ int sensorSign[6] = {1, 1, 1, 1, 1, 1}; // GYROZ, GYROX, GYROY, ACCELX, ACCELY,
 byte flightMode;
 
 unsigned long currentTime, previousTime, deltaTime;
+unsigned long mainLoop = 0;
 unsigned long sensorLoop = 0;
 unsigned long controlLoop = 0;
 unsigned long radioLoop = 0;
@@ -142,7 +143,7 @@ void setup() {
 
 // fast rate
 // read sensors
-// update attitude
+// IMU : update attitude
 // motor control
 
 // medium rate
@@ -154,69 +155,73 @@ void setup() {
 // external command/telemetry
 // GPS
 
+void loop()
+{
+  int aux;
+  int i;
+  float aux_float;
 
-void loop() {
   currentTime = millis();
-  deltaTime = currentTime - previousTime;
+  //deltaTime = currentTime - previousTime;
-  G_Dt = deltaTime / 1000.0;
+  //G_Dt = deltaTime / 1000.0;
-  previousTime = currentTime;
+  //previousTime = currentTime;
-  
+
-  // Read Sensors **************************************************
+  // Sensor reading loop is inside APM_ADC and runs at 400Hz
-  if (currentTime > sensorLoop + 2) { // 500Hz (every 2ms)
+  // Main loop at 200Hz (IMU + control)
-    for (channel = GYROZ; channel < LASTSENSOR; channel++) {
+  if (currentTime > (mainLoop + 5))    // 200Hz (every 5ms)
-      dataADC[channel] = readADC(channel); // defined in Sensors.pde
+    {
-    }
+    G_Dt = (currentTime-mainLoop) / 1000.0;   // Microseconds!!!
-    sensorLoop = currentTime;
+    mainLoop = currentTime;
-  }
+
-  
+    //IMU DCM Algorithm
-  // Update ArduCopter Control *************************************
+    Read_adc_raw();       // Read sensors raw data
-  if (currentTime > controlLoop + 5) { // 200Hz (every 5ms)
+    Matrix_update(); 
-    if(flightMode == STABLE) {        // Changed for variable
+    // 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)
+    read_radio();
+
+    // Attitude control
+    if(flightMode == STABLE) {    // STABLE Mode
       gled_speed = 1200;
-      Attitude_control_v3();
+      if (AP_mode == 0)           // 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);
     }
-    else {
+    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);
     }
-    controlLoop = currentTime;
-  }
-  
-    // Execute the fast loop
-    // ---------------------
-    // fast_loop();
-    // - PWM Updates
-    // - Stabilization
-    // - Altitude correction
-		
-  // Execute the medium loop 
-  // -----------------------
-  // medium_loop();
-  // - Radio read
-  // - GPS read
-  // - Drift correction
-  
 
+    // Send output commands to motors...
+    motor_output();
 
-		// Execute the slow loop 
+    // Performance optimization: Magnetometer sensor and pressure sensor are slowly to read (I2C)
-		// -----------------------
+    // so we read them at the end of the loop (all work is done in this loop run...)
-		// slow_loop();
+    #ifdef IsMAG
-                // - Battery usage
+    if (MAGNETOMETER == 1) {
-                // - GCS updates
+      if (MAG_counter > 20)  // Read compass data at 10Hz...
-                // - Garbage management
+      {
-
+        MAG_counter=0;
-		if (millis()- perf_mon_timer > 20000) {
+        APM_Compass.Read();     // Read magnetometer
-			if (mainLoop_count != 0) {
+        APM_Compass.Calculate(roll,pitch);  // Calculate heading
-	
+      }
-				//send_message(MSG_PERF_REPORT);
+    }
-				#if LOG_PM
+    #endif
-					Log_Write_Performance();
+    #ifdef UseBMP
-				#endif
+    #endif    
-				resetPerfData();
+    }
-			 }
-		}
-	}  
 }
-
+ 

ArducopterNG/Attitude.pde

@@ -56,48 +56,40 @@ TODO:
 // STABLE MODE
 // PI absolute angle control driving a P rate control
 // Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
-void Attitude_control_v3()
+void Attitude_control_v3(int command_roll, int command_pitch, int command_yaw)
 {
   float stable_roll,stable_pitch,stable_yaw;
   
   // ROLL CONTROL    
-  if (AP_mode==2)        // Normal Mode => Stabilization mode
+  err_roll = command_roll - ToDeg(roll);
-    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...
   
   roll_I += err_roll*G_Dt;
   roll_I = constrain(roll_I,-20,20);
 
   // PID absolute angle control
-  K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain
+  stable_roll = KP_QUAD_ROLL*err_roll + KI_QUAD_ROLL*roll_I;
-  stable_roll = K_aux*err_roll + KI_QUAD_ROLL*roll_I;
   
   // PD rate control (we use also the bias corrected gyro rates)
   err_roll = stable_roll - ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
   control_roll = STABLE_MODE_KP_RATE_ROLL*err_roll;
   
   // PITCH CONTROL
-  if (AP_mode==2)        // Normal mode => Stabilization mode
+  err_pitch = command_pitch - ToDeg(pitch);
-    err_pitch = command_rx_pitch - ToDeg(pitch);
-  else                   // GPS Position hold
-    err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch);  // Position Control
   err_pitch = constrain(err_pitch,-25,25);  // to limit max pitch command...
   
   pitch_I += err_pitch*G_Dt;
   pitch_I = constrain(pitch_I,-20,20);
  
   // PID absolute angle control
-  K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain
+  stable_pitch = KP_QUAD_PITCH*err_pitch + KI_QUAD_PITCH*pitch_I;
-  stable_pitch = K_aux*err_pitch + KI_QUAD_PITCH*pitch_I;
   
   // P rate control (we use also the bias corrected gyro rates)
   err_pitch = stable_pitch - ToDeg(Omega[1]);
   control_pitch = STABLE_MODE_KP_RATE_PITCH*err_pitch;
   
   // YAW CONTROL
-  err_yaw = command_rx_yaw - ToDeg(yaw);
+  err_yaw = command_yaw - ToDeg(yaw);
   if (err_yaw > 180)    // Normalize to -180,180
     err_yaw -= 360;
   else if(err_yaw < -180)

ArducopterNG/Log.pde

@@ -174,6 +174,21 @@ void Log_Write_PID(byte num_PID, int P, int I,int D, int output)
   DataFlash.WriteByte(END_BYTE);
 }
 
+// Write a Radio packet
+void Log_Write_Radio(int ch1, int ch2, int ch3,int ch4, int ch5, int ch6)
+{
+  DataFlash.WriteByte(HEAD_BYTE1);
+  DataFlash.WriteByte(HEAD_BYTE2);
+  DataFlash.WriteByte(LOG_RADIO_MSG);
+  DataFlash.WriteInt(ch1);
+  DataFlash.WriteInt(ch2);
+  DataFlash.WriteInt(ch3);
+  DataFlash.WriteInt(ch4);
+  DataFlash.WriteInt(ch5);
+  DataFlash.WriteInt(ch6);
+  DataFlash.WriteByte(END_BYTE);
+}
+
 #if LOG_PM
 // Write a performance monitoring packet. Total length : 19 bytes
 void Log_Write_Performance()
@@ -365,6 +380,24 @@ void Log_Read_PID()
   Serial.println();
 }
 
+// Read an Radio packet
+void Log_Read_Radio()
+{  
+  Serial.print("RADIO:");
+  Serial.print(DataFlash.ReadInt());
+  Serial.print(",");
+  Serial.print(DataFlash.ReadInt());
+  Serial.print(",");
+  Serial.print(DataFlash.ReadInt());
+  Serial.print(",");
+  Serial.print(DataFlash.ReadInt());
+  Serial.print(",");
+  Serial.print(DataFlash.ReadInt());
+  Serial.print(",");
+  Serial.print(DataFlash.ReadInt());
+  Serial.println();
+}
+
 // Read a mode packet
 void Log_Read_Mode()
 {	

ArducopterNG/Radio.pde

@@ -36,7 +36,7 @@ TODO:
 #define STICK_TO_ANGLE_FACTOR 12.0        // To convert stick position to absolute angles
 #define YAW_STICK_TO_ANGLE_FACTOR 150.0
 
-void Read_radio()
+void read_radio()
 {
   if (APM_RC.GetState() == 1)   // New radio frame?
     {
@@ -79,12 +79,11 @@ void Read_radio()
       #endif
 
       // YAW
-      if (abs(ch_yaw-yaw_mid)<8)   // Take into account a bit of "dead zone" on yaw
+      if (abs(ch_yaw-yaw_mid)>6)   // Take into account a bit of "dead zone" on yaw
-        aux_float = 0.0;
+        {
-      else
+        command_rx_yaw += (ch_yaw-yaw_mid) / YAW_STICK_TO_ANGLE_FACTOR;
-        aux_float = (ch_yaw-yaw_mid) / YAW_STICK_TO_ANGLE_FACTOR;
+        command_rx_yaw = Normalize_angle(command_rx_yaw);    // Normalize angle to [-180,180]
-      command_rx_yaw += aux_float;
+        }
-      command_rx_yaw = Normalize_angle(command_rx_yaw);    // Normalize angle to [-180,180]
       }
     
     // Write Radio data to DataFlash log
@@ -94,7 +93,7 @@ void Read_radio()
 
 
 // Send output commands to ESC´s
-void Motor_output()
+void motor_output()
 {
   // Quadcopter mix
   if (motorArmed == 1) 

ArducopterNG/Sensors.pde

@@ -25,6 +25,25 @@
 
 * ************************************************************** */
 
+/* ******* ADC functions ********************* */
+// Read all the ADC channles
+void Read_adc_raw(void)
+{
+  //int temp;
+  
+  for (int i=0;i<6;i++)
+    AN[i] = APM_ADC.Ch(sensors[i]);
+}
+
+// Returns an analog value with the offset
+int read_adc(int select)
+{
+  if (SENSOR_SIGN[select]<0)
+    return (AN_OFFSET[select]-AN[select]);
+  else
+    return (AN[select]-AN_OFFSET[select]);
+}
+
 int readADC(byte channel) {
   if (sensorSign[channel] < 0)
     return (zeroADC[channel] - APM_ADC.Ch(channel));
@@ -39,7 +58,7 @@ void calibrateSensors(void) {
     for (channel = GYROZ; channel < LASTSENSOR; channel++) {
       rawADC[channel] = APM_ADC.Ch(channel);
       zeroADC[channel] = (zeroADC[channel] * 0.8) + (rawADC[channel] * 0.2);
-      Log_Write_Sensor(rawADC[GYROZ], rawADC[GYROX], rawADC[GYROY], rawADC[ACCELX], rawADC[ACCELY], rawADC[ACCELZ], receiverData[THROTTLE]);
+      //Log_Write_Sensor(rawADC[GYROZ], rawADC[GYROX], rawADC[GYROY], rawADC[ACCELX], rawADC[ACCELY], rawADC[ACCELZ], receiverData[THROTTLE]);
     }
     delay(5);
     // Runnings lights effect to let user know that we are taking mesurements