Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
ArducopterNG/ArducopterNG.pde
View File

@ -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;
//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;
}
// 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;
// Update ArduCopter Control *************************************
if (currentTime > controlLoop + 5) { // 200Hz (every 5ms)
if(flightMode == STABLE) { // Changed for variable
//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;
// Send output commands to motors...
motor_output();
// 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
}
}
// Execute the fast loop
// ---------------------
// fast_loop();
// - PWM Updates
// - Stabilization
// - Altitude correction
// Execute the medium loop
// -----------------------
// medium_loop();
// - Radio read
// - GPS read
// - Drift correction
// 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();
}
}
#ifdef UseBMP
#endif
}
}

20
ArducopterNG/Attitude.pde
View File

@ -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)

33
ArducopterNG/Log.pde
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);
}
// 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()
{

13
ArducopterNG/Radio.pde
View File

@ -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,13 +79,12 @@ 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;
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
Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode);
@ -94,7 +93,7 @@ void Read_radio()
// Send output commands to ESC´s
void Motor_output()
void motor_output()
{
// Quadcopter mix
if (motorArmed == 1)

21
ArducopterNG/Sensors.pde
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) {
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