Main loop organization. NOT WORKING YET

git-svn-id: https://arducopter.googlecode.com/svn/trunk@653 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-10-11 14:12:44 +00:00
parent cca79ba0db
commit af74cb32c5
5 changed files with 126 additions and 78 deletions

View File

@ -115,6 +115,7 @@ int sensorSign[6] = {1, 1, 1, 1, 1, 1}; // GYROZ, GYROX, GYROY, ACCELX, ACCELY,
byte flightMode; byte flightMode;
unsigned long currentTime, previousTime, deltaTime; unsigned long currentTime, previousTime, deltaTime;
unsigned long mainLoop = 0;
unsigned long sensorLoop = 0; unsigned long sensorLoop = 0;
unsigned long controlLoop = 0; unsigned long controlLoop = 0;
unsigned long radioLoop = 0; unsigned long radioLoop = 0;
@ -142,7 +143,7 @@ void setup() {
// fast rate // fast rate
// read sensors // read sensors
// update attitude // IMU : update attitude
// motor control // motor control
// medium rate // medium rate
@ -154,69 +155,73 @@ void setup() {
// external command/telemetry // external command/telemetry
// GPS // GPS
void loop()
{
int aux;
int i;
float aux_float;
void loop() {
currentTime = millis(); 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;
}
// Update ArduCopter Control ************************************* //IMU DCM Algorithm
if (currentTime > controlLoop + 5) { // 200Hz (every 5ms) Read_adc_raw(); // Read sensors raw data
if(flightMode == STABLE) { // Changed for variable 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; 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; gled_speed = 400;
Rate_control_v2(); Rate_control_v2();
// Reset yaw, so if we change to stable mode we continue with the actual yaw direction // Reset yaw, so if we change to stable mode we continue with the actual yaw direction
command_rx_yaw = ToDeg(yaw); command_rx_yaw = ToDeg(yaw);
} }
controlLoop = currentTime;
}
// Execute the fast loop // Send output commands to motors...
// --------------------- motor_output();
// fast_loop();
// - PWM Updates
// - Stabilization
// - Altitude correction
// Execute the medium 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...)
// medium_loop(); #ifdef IsMAG
// - Radio read if (MAGNETOMETER == 1) {
// - GPS read if (MAG_counter > 20) // Read compass data at 10Hz...
// - Drift correction {
MAG_counter=0;
APM_Compass.Read(); // Read magnetometer
APM_Compass.Calculate(roll,pitch); // Calculate heading
// Execute the slow loop }
// ----------------------- }
// slow_loop(); #endif
// - Battery usage #ifdef UseBMP
// - GCS updates #endif
// - 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();
}
}
}
} }

View File

@ -56,48 +56,40 @@ TODO:
// STABLE MODE // STABLE MODE
// PI absolute angle control driving a P rate control // PI absolute angle control driving a P rate control
// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands // 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; float stable_roll,stable_pitch,stable_yaw;
// ROLL CONTROL // 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... err_roll = constrain(err_roll,-25,25); // to limit max roll command...
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);
// PID absolute angle control // 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) // 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 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; control_roll = STABLE_MODE_KP_RATE_ROLL*err_roll;
// PITCH CONTROL // 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... err_pitch = constrain(err_pitch,-25,25); // to limit max pitch command...
pitch_I += err_pitch*G_Dt; pitch_I += err_pitch*G_Dt;
pitch_I = constrain(pitch_I,-20,20); pitch_I = constrain(pitch_I,-20,20);
// PID absolute angle control // 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) // P rate control (we use also the bias corrected gyro rates)
err_pitch = stable_pitch - ToDeg(Omega[1]); err_pitch = stable_pitch - ToDeg(Omega[1]);
control_pitch = STABLE_MODE_KP_RATE_PITCH*err_pitch; control_pitch = STABLE_MODE_KP_RATE_PITCH*err_pitch;
// YAW CONTROL // YAW CONTROL
err_yaw = command_rx_yaw - ToDeg(yaw); err_yaw = command_yaw - ToDeg(yaw);
if (err_yaw > 180) // Normalize to -180,180 if (err_yaw > 180) // Normalize to -180,180
err_yaw -= 360; err_yaw -= 360;
else if(err_yaw < -180) else if(err_yaw < -180)

View File

@ -174,6 +174,21 @@ void Log_Write_PID(byte num_PID, int P, int I,int D, int output)
DataFlash.WriteByte(END_BYTE); 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 #if LOG_PM
// Write a performance monitoring packet. Total length : 19 bytes // Write a performance monitoring packet. Total length : 19 bytes
void Log_Write_Performance() void Log_Write_Performance()
@ -365,6 +380,24 @@ void Log_Read_PID()
Serial.println(); 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 // Read a mode packet
void Log_Read_Mode() void Log_Read_Mode()
{ {

View File

@ -36,7 +36,7 @@ TODO:
#define STICK_TO_ANGLE_FACTOR 12.0 // To convert stick position to absolute angles #define STICK_TO_ANGLE_FACTOR 12.0 // To convert stick position to absolute angles
#define YAW_STICK_TO_ANGLE_FACTOR 150.0 #define YAW_STICK_TO_ANGLE_FACTOR 150.0
void Read_radio() void read_radio()
{ {
if (APM_RC.GetState() == 1) // New radio frame? if (APM_RC.GetState() == 1) // New radio frame?
{ {
@ -79,12 +79,11 @@ void Read_radio()
#endif #endif
// YAW // 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 // Write Radio data to DataFlash log
@ -94,7 +93,7 @@ void Read_radio()
// Send output commands to ESC´s // Send output commands to ESC´s
void Motor_output() void motor_output()
{ {
// Quadcopter mix // Quadcopter mix
if (motorArmed == 1) if (motorArmed == 1)

View File

@ -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) { int readADC(byte channel) {
if (sensorSign[channel] < 0) if (sensorSign[channel] < 0)
return (zeroADC[channel] - APM_ADC.Ch(channel)); return (zeroADC[channel] - APM_ADC.Ch(channel));
@ -39,7 +58,7 @@ void calibrateSensors(void) {
for (channel = GYROZ; channel < LASTSENSOR; channel++) { for (channel = GYROZ; channel < LASTSENSOR; channel++) {
rawADC[channel] = APM_ADC.Ch(channel); rawADC[channel] = APM_ADC.Ch(channel);
zeroADC[channel] = (zeroADC[channel] * 0.8) + (rawADC[channel] * 0.2); 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); delay(5);
// Runnings lights effect to let user know that we are taking mesurements // Runnings lights effect to let user know that we are taking mesurements