From af74cb32c55c5f3f14a489cdd33a9ec5b8802a84 Mon Sep 17 00:00:00 2001 From: jjulio1234 Date: Mon, 11 Oct 2010 14:12:44 +0000 Subject: [PATCH] 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 ++++++++++++++++++---------------- ArducopterNG/Attitude.pde | 20 ++---- ArducopterNG/Log.pde | 33 ++++++++++ ArducopterNG/Radio.pde | 15 +++-- ArducopterNG/Sensors.pde | 21 ++++++- 5 files changed, 126 insertions(+), 78 deletions(-) diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index 2d27c8fc30..f3966f5c50 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/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; - G_Dt = deltaTime / 1000.0; - previousTime = currentTime; - - // Read Sensors ************************************************** - if (currentTime > sensorLoop + 2) { // 500Hz (every 2ms) - for (channel = GYROZ; channel < LASTSENSOR; channel++) { - dataADC[channel] = readADC(channel); // defined in Sensors.pde - } - sensorLoop = currentTime; - } - - // Update ArduCopter Control ************************************* - if (currentTime > controlLoop + 5) { // 200Hz (every 5ms) - if(flightMode == STABLE) { // Changed for variable + //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) + { + G_Dt = (currentTime-mainLoop) / 1000.0; // 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) + 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 - // ----------------------- - // slow_loop(); - // - Battery usage - // - GCS updates - // - Garbage management - - if (millis()- perf_mon_timer > 20000) { - if (mainLoop_count != 0) { - - //send_message(MSG_PERF_REPORT); - #if LOG_PM - Log_Write_Performance(); - #endif - resetPerfData(); - } - } - } + // 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...) + #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 + #ifdef UseBMP + #endif + } } - + diff --git a/ArducopterNG/Attitude.pde b/ArducopterNG/Attitude.pde index f9621e5003..5fda61f6d0 100644 --- a/ArducopterNG/Attitude.pde +++ b/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_rx_roll - ToDeg(roll); - else - err_roll = (command_rx_roll + command_gps_roll) - ToDeg(roll); // Position control + err_roll = command_roll - ToDeg(roll); 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 = K_aux*err_roll + KI_QUAD_ROLL*roll_I; + stable_roll = KP_QUAD_ROLL*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_rx_pitch - ToDeg(pitch); - else // GPS Position hold - err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch); // Position Control + err_pitch = command_pitch - ToDeg(pitch); 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 = K_aux*err_pitch + KI_QUAD_PITCH*pitch_I; + stable_pitch = KP_QUAD_PITCH*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) diff --git a/ArducopterNG/Log.pde b/ArducopterNG/Log.pde index 2348ea4522..f2ad639599 100644 --- a/ArducopterNG/Log.pde +++ b/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() { diff --git a/ArducopterNG/Radio.pde b/ArducopterNG/Radio.pde index 448185e435..58910f481c 100644 --- a/ArducopterNG/Radio.pde +++ b/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 - aux_float = 0.0; - else - aux_float = (ch_yaw-yaw_mid) / YAW_STICK_TO_ANGLE_FACTOR; - command_rx_yaw += aux_float; - command_rx_yaw = Normalize_angle(command_rx_yaw); // Normalize angle to [-180,180] + if (abs(ch_yaw-yaw_mid)>6) // Take into account a bit of "dead zone" on yaw + { + command_rx_yaw += (ch_yaw-yaw_mid) / YAW_STICK_TO_ANGLE_FACTOR; + 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) diff --git a/ArducopterNG/Sensors.pde b/ArducopterNG/Sensors.pde index f20749de37..16242a0996 100644 --- a/ArducopterNG/Sensors.pde +++ b/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