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

AM Mode, Green LED optimization 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@75 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-08-12 02:36:57 +00:00
parent 84fb1f015d
commit 2fdceb1b78
1 changed files with 263 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

454
Arducopter/Arducopter.pde
View File

@ -24,18 +24,18 @@
/* /*
**** Switch Functions ***** **** Switch Functions *****
AUX1 ON = Stable Mode AUX1 ON = Stable Mode
AUX1 OFF = Acro Mode AUX1 OFF = Acro Mode
GEAR ON = GPS Hold GEAR ON = GPS Hold
GEAR OFF = Flight Assist (Stable Mode) GEAR OFF = Flight Assist (Stable Mode)
**** LED Feedback **** **** LED Feedback ****
Green LED On = APM Initialization Finished Green LED On = APM Initialization Finished
Yellow LED On = GPS Hold Mode Yellow LED On = GPS Hold Mode
Yellow LED Off = Flight Assist Mode (No GPS) Yellow LED Off = Flight Assist Mode (No GPS)
Red LED On = GPS Fix Red LED On = GPS Fix
Red LED Off = No GPS Fix Red LED Off = No GPS Fix
*/ */
#include <Wire.h> #include <Wire.h>
#include <APM_ADC.h> #include <APM_ADC.h>
@ -60,6 +60,16 @@ Red LED Off = No GPS Fix
#define SW2_pin 40 #define SW2_pin 40
/* *** */ /* *** */
/* AM PIN Definitions */
/* Can be changed in future to AN extension ports */
#define FR_LED 3 // Mega PE4 pin
#define RE_LED 2 // Mega PE5 pin
#define RI_LED 7 // Mega PH4 pin
#define LE_LED 8 // Mega PH5 pin
/* AM PIN Definitions - END */
/* ***************************************************************************** */ /* ***************************************************************************** */
/* CONFIGURATION PART */ /* CONFIGURATION PART */
/* ***************************************************************************** */ /* ***************************************************************************** */
@ -86,10 +96,12 @@ Red LED Off = No GPS Fix
#define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data #define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data
//Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
uint8_t sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware uint8_t sensors[6] = {
1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware
//Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
int SENSOR_SIGN[]={1,-1,-1,-1,1,1,-1,-1,-1}; //{-1,1,-1,1,-1,1,-1,-1,-1}; int SENSOR_SIGN[]={
1,-1,-1,-1,1,1,-1,-1,-1}; //{-1,1,-1,1,-1,1,-1,-1,-1};
int AN[6]; //array that store the 6 ADC channels int AN[6]; //array that store the 6 ADC channels
int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers
@ -98,18 +110,27 @@ int gyro_temp;
float G_Dt=0.02; // Integration time for the gyros (DCM algorithm) float G_Dt=0.02; // Integration time for the gyros (DCM algorithm)
float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector float Accel_Vector[3]= {
float Accel_Vector_unfiltered[3]= {0,0,0}; //Store the acceleration in a vector 0,0,0}; //Store the acceleration in a vector
float Accel_Vector_unfiltered[3]= {
0,0,0}; //Store the acceleration in a vector
//float Accel_magnitude; //float Accel_magnitude;
//float Accel_weight; //float Accel_weight;
float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector float Gyro_Vector[3]= {
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data 0,0,0};//Store the gyros rutn rate in a vector
float Omega_P[3]= {0,0,0};//Omega Proportional correction float Omega_Vector[3]= {
float Omega_I[3]= {0,0,0};//Omega Integrator 0,0,0}; //Corrected Gyro_Vector data
float Omega[3]= {0,0,0}; float Omega_P[3]= {
0,0,0};//Omega Proportional correction
float Omega_I[3]= {
0,0,0};//Omega Integrator
float Omega[3]= {
0,0,0};
float errorRollPitch[3]= {0,0,0}; float errorRollPitch[3]= {
float errorYaw[3]= {0,0,0}; 0,0,0};
float errorYaw[3]= {
0,0,0};
float errorCourse=0; float errorCourse=0;
float COGX=0; //Course overground X axis float COGX=0; //Course overground X axis
float COGY=1; //Course overground Y axis float COGY=1; //Course overground Y axis
@ -121,16 +142,29 @@ float yaw=0;
unsigned int counter=0; unsigned int counter=0;
float DCM_Matrix[3][3]= { float DCM_Matrix[3][3]= {
{1,0,0} {
,{0,1,0} 1,0,0 }
,{0,0,1} ,{
0,1,0 }
,{
0,0,1 }
}; };
float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here float Update_Matrix[3][3]={
{
0,1,2 }
,{
3,4,5 }
,{
6,7,8 }
}; //Gyros here
float Temporary_Matrix[3][3]={ float Temporary_Matrix[3][3]={
{0,0,0} {
,{0,0,0} 0,0,0 }
,{0,0,0} ,{
0,0,0 }
,{
0,0,0 }
}; };
// GPS variables // GPS variables
@ -195,7 +229,12 @@ int Sonar_value=0;
int Sonar_Counter=0; int Sonar_Counter=0;
// AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode) // AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode)
byte AP_mode = 2; byte AP_mode = 2;
// Mode LED timers and variables, used to blink LED_Green
byte gled_status = HIGH;
long gled_timer;
int gled_speed;
long t0; long t0;
int num_iter; int num_iter;
@ -251,32 +290,32 @@ void Position_control(long lat_dest, long lon_dest)
long Lat_diff; long Lat_diff;
float gps_err_roll; float gps_err_roll;
float gps_err_pitch; float gps_err_pitch;
Lon_diff = lon_dest - GPS.Longitude; Lon_diff = lon_dest - GPS.Longitude;
Lat_diff = lat_dest - GPS.Lattitude; Lat_diff = lat_dest - GPS.Lattitude;
// ROLL // ROLL
gps_err_roll_old = gps_err_roll; gps_err_roll_old = gps_err_roll;
//Optimization : cos(yaw) = DCM_Matrix[0][0] ; sin(yaw) = DCM_Matrix[1][0] //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_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)/G_Dt; gps_roll_D = (gps_err_roll-gps_err_roll_old)/G_Dt;
gps_roll_I += gps_err_roll*G_Dt; gps_roll_I += gps_err_roll*G_Dt;
gps_roll_I = constrain(gps_roll_I,-500,500); gps_roll_I = constrain(gps_roll_I,-500,500);
command_gps_roll = KP_GPS_ROLL*gps_err_roll + KD_GPS_ROLL*gps_roll_D + KI_GPS_ROLL*gps_roll_I; 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 command_gps_roll = constrain(command_gps_roll,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
// PITCH // PITCH
gps_err_pitch_old = gps_err_pitch; gps_err_pitch_old = gps_err_pitch;
gps_err_pitch = -(float)Lat_diff*DCM_Matrix[0][0]- (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[1][0]; 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)/G_Dt; gps_pitch_D = (gps_err_pitch-gps_err_pitch_old)/G_Dt;
gps_pitch_I += gps_err_pitch*G_Dt; gps_pitch_I += gps_err_pitch*G_Dt;
gps_pitch_I = constrain(gps_pitch_I,-500,500); gps_pitch_I = constrain(gps_pitch_I,-500,500);
command_gps_pitch = KP_GPS_PITCH*gps_err_pitch + KD_GPS_PITCH*gps_pitch_D + KI_GPS_PITCH*gps_pitch_I; 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 command_gps_pitch = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
} }
@ -291,64 +330,65 @@ void Attitude_control_v2()
float err_pitch_rate; float err_pitch_rate;
float roll_rate; float roll_rate;
float pitch_rate; float pitch_rate;
// ROLL CONTROL // ROLL CONTROL
if (AP_mode==2) // Normal Mode => Stabilization mode if (AP_mode==2) // Normal Mode => Stabilization mode
err_roll = command_rx_roll - ToDeg(roll); err_roll = command_rx_roll - ToDeg(roll);
else else
err_roll = (command_rx_roll + command_gps_roll) - ToDeg(roll); // Position control 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...
// New control term... // New control term...
roll_rate = ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected roll_rate = ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
err_roll_rate = ((ch_roll-1500)>>1) - roll_rate; err_roll_rate = ((ch_roll-1500)>>1) - roll_rate;
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);
// D term implementation => two parts: gyro part and command part // D term implementation => two parts: gyro part and command part
// To have a better (faster) response we can use the Gyro reading directly for the Derivative term... // To have a better (faster) response we can use the Gyro reading directly for the Derivative term...
// We also add a part that takes into account the command from user (stick) to make the system more responsive to user inputs // We also add a part that takes into account the command from user (stick) to make the system more responsive to user inputs
roll_D = - roll_rate; // Take into account Angular velocity of the stick (command) roll_D = - roll_rate; // Take into account Angular velocity of the stick (command)
// PID control // PID control
K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain
control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I + STABLE_MODE_KP_RATE*err_roll_rate; ; control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I + STABLE_MODE_KP_RATE*err_roll_rate;
;
// PITCH CONTROL // PITCH CONTROL
if (AP_mode==2) // Normal mode => Stabilization mode if (AP_mode==2) // Normal mode => Stabilization mode
err_pitch = command_rx_pitch - ToDeg(pitch); err_pitch = command_rx_pitch - ToDeg(pitch);
else else
err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch); // Position Control 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...
// New control term... // New control term...
pitch_rate = ToDeg(Omega[1]); pitch_rate = ToDeg(Omega[1]);
err_pitch_rate = ((ch_pitch-1500)>>1) - pitch_rate; err_pitch_rate = ((ch_pitch-1500)>>1) - pitch_rate;
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);
// D term // D term
pitch_D = - pitch_rate; pitch_D = - pitch_rate;
// PID control // PID control
K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain
control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I + STABLE_MODE_KP_RATE*err_pitch_rate; control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I + STABLE_MODE_KP_RATE*err_pitch_rate;
// YAW CONTROL // YAW CONTROL
err_yaw = command_rx_yaw - ToDeg(yaw); err_yaw = command_rx_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)
err_yaw += 360; err_yaw += 360;
err_yaw = constrain(err_yaw,-60,60); // to limit max yaw command... err_yaw = constrain(err_yaw,-60,60); // to limit max yaw command...
yaw_I += err_yaw*G_Dt; yaw_I += err_yaw*G_Dt;
yaw_I = constrain(yaw_I,-20,20); yaw_I = constrain(yaw_I,-20,20);
yaw_D = - ToDeg(Omega[2]); yaw_D = - ToDeg(Omega[2]);
// PID control // PID control
control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I; control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I;
} }
@ -358,44 +398,44 @@ void Rate_control()
{ {
static float previousRollRate, previousPitchRate, previousYawRate; static float previousRollRate, previousPitchRate, previousYawRate;
float currentRollRate, currentPitchRate, currentYawRate; float currentRollRate, currentPitchRate, currentYawRate;
// ROLL CONTROL // ROLL CONTROL
currentRollRate = read_adc(0); // I need a positive sign here currentRollRate = read_adc(0); // I need a positive sign here
err_roll = ((ch_roll-1500) * xmitFactor) - currentRollRate; err_roll = ((ch_roll-1500) * xmitFactor) - currentRollRate;
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);
roll_D = currentRollRate - previousRollRate; roll_D = currentRollRate - previousRollRate;
previousRollRate = currentRollRate; previousRollRate = currentRollRate;
// PID control // PID control
control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I; control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I;
// PITCH CONTROL // PITCH CONTROL
currentPitchRate = read_adc(1); currentPitchRate = read_adc(1);
err_pitch = ((ch_pitch-1500) * xmitFactor) - currentPitchRate; err_pitch = ((ch_pitch-1500) * xmitFactor) - currentPitchRate;
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);
pitch_D = currentPitchRate - previousPitchRate; pitch_D = currentPitchRate - previousPitchRate;
previousPitchRate = currentPitchRate; previousPitchRate = currentPitchRate;
// PID control // PID control
control_pitch = Kp_RatePitch*err_pitch + Kd_RatePitch*pitch_D + Ki_RatePitch*pitch_I; control_pitch = Kp_RatePitch*err_pitch + Kd_RatePitch*pitch_D + Ki_RatePitch*pitch_I;
// YAW CONTROL // YAW CONTROL
currentYawRate = read_adc(2); currentYawRate = read_adc(2);
err_yaw = ((ch_yaw-1500)* xmitFactor) - currentYawRate; err_yaw = ((ch_yaw-1500)* xmitFactor) - currentYawRate;
yaw_I += err_yaw*G_Dt; yaw_I += err_yaw*G_Dt;
yaw_I = constrain(yaw_I,-20,20); yaw_I = constrain(yaw_I,-20,20);
yaw_D = currentYawRate - previousYawRate; yaw_D = currentYawRate - previousYawRate;
previousYawRate = currentYawRate; previousYawRate = currentYawRate;
// PID control // PID control
K_aux = KP_QUAD_YAW; // Comment this out if you want to use transmitter to adjust gain 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; control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I;
@ -405,20 +445,20 @@ void Rate_control()
int channel_filter(int ch, int ch_old) int channel_filter(int ch, int ch_old)
{ {
int diff_ch_old; int diff_ch_old;
if (ch_old==0) // ch_old not initialized if (ch_old==0) // ch_old not initialized
return(ch); return(ch);
diff_ch_old = ch - ch_old; // Difference with old reading diff_ch_old = ch - ch_old; // Difference with old reading
if (diff_ch_old<0) if (diff_ch_old<0)
{ {
if (diff_ch_old<-40) if (diff_ch_old<-40)
return(ch_old-40); // We limit the max difference between readings return(ch_old-40); // We limit the max difference between readings
} }
else else
{ {
if (diff_ch_old>40) if (diff_ch_old>40)
return(ch_old+40); return(ch_old+40);
} }
return((ch+ch_old)>>1); // Small filtering return((ch+ch_old)>>1); // Small filtering
//return(ch); //return(ch);
} }
@ -429,23 +469,23 @@ void setup()
{ {
int i; int i;
float aux_float[3]; float aux_float[3];
pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1) pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1)
pinMode(LED_Red,OUTPUT); //Red LED B (PC2) pinMode(LED_Red,OUTPUT); //Red LED B (PC2)
pinMode(LED_Green,OUTPUT); //Green LED C (PC0) pinMode(LED_Green,OUTPUT); //Green LED C (PC0)
pinMode(SW1_pin,INPUT); //Switch SW1 (pin PG0) pinMode(SW1_pin,INPUT); //Switch SW1 (pin PG0)
pinMode(RELE_pin,OUTPUT); // Rele output pinMode(RELE_pin,OUTPUT); // Rele output
digitalWrite(RELE_pin,LOW); digitalWrite(RELE_pin,LOW);
delay(250); delay(250);
APM_RC.Init(); // APM Radio initialization APM_RC.Init(); // APM Radio initialization
APM_ADC.Init(); // APM ADC library initialization APM_ADC.Init(); // APM ADC library initialization
DataFlash.Init(); // DataFlash log initialization DataFlash.Init(); // DataFlash log initialization
GPS.Init(); // GPS Initialization GPS.Init(); // GPS Initialization
readUserConfig(); // Load user configurable items from EEPROM readUserConfig(); // Load user configurable items from EEPROM
// RC channels Initialization (Quad motors) // RC channels Initialization (Quad motors)
@ -453,31 +493,31 @@ void setup()
APM_RC.OutputCh(1,MIN_THROTTLE); APM_RC.OutputCh(1,MIN_THROTTLE);
APM_RC.OutputCh(2,MIN_THROTTLE); APM_RC.OutputCh(2,MIN_THROTTLE);
APM_RC.OutputCh(3,MIN_THROTTLE); APM_RC.OutputCh(3,MIN_THROTTLE);
if (MAGNETOMETER == 1) if (MAGNETOMETER == 1)
APM_Compass.Init(); // I2C initialization APM_Compass.Init(); // I2C initialization
DataFlash.StartWrite(1); // Start a write session on page 1 DataFlash.StartWrite(1); // Start a write session on page 1
//Serial.begin(57600); //Serial.begin(57600);
Serial.begin(115200); Serial.begin(115200);
//Serial.println(); //Serial.println();
//Serial.println("ArduCopter Quadcopter v1.0"); //Serial.println("ArduCopter Quadcopter v1.0");
// Check if we enable the DataFlash log Read Mode (switch) // Check if we enable the DataFlash log Read Mode (switch)
// If we press switch 1 at startup we read the Dataflash eeprom // If we press switch 1 at startup we read the Dataflash eeprom
while (digitalRead(SW1_pin)==0) while (digitalRead(SW1_pin)==0)
{ {
Serial.println("Entering Log Read Mode..."); Serial.println("Entering Log Read Mode...");
Log_Read(1,1000); Log_Read(1,1000);
delay(30000); delay(30000);
} }
//delay(3000); //delay(3000);
Read_adc_raw(); Read_adc_raw();
delay(20); delay(20);
// Offset values for accels and gyros... // Offset values for accels and gyros...
AN_OFFSET[3] = acc_offset_x; AN_OFFSET[3] = acc_offset_x;
AN_OFFSET[4] = acc_offset_y; AN_OFFSET[4] = acc_offset_y;
@ -485,73 +525,78 @@ void setup()
aux_float[0] = gyro_offset_roll; aux_float[0] = gyro_offset_roll;
aux_float[1] = gyro_offset_pitch; aux_float[1] = gyro_offset_pitch;
aux_float[2] = gyro_offset_yaw; aux_float[2] = gyro_offset_yaw;
// Take the gyro offset values // Take the gyro offset values
for(i=0;i<300;i++) for(i=0;i<300;i++)
{ {
Read_adc_raw(); Read_adc_raw();
for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset. for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset.
{ {
aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2; aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2;
//Serial.print(AN[y]); //Serial.print(AN[y]);
//Serial.print(","); //Serial.print(",");
} }
//Serial.println(); //Serial.println();
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle); Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
delay(10); delay(10);
} }
for(int y=0; y<=2; y++) for(int y=0; y<=2; y++)
AN_OFFSET[y]=aux_float[y]; AN_OFFSET[y]=aux_float[y];
Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value
#ifndef CONFIGURATOR #ifndef CONFIGURATOR
for(i=0;i<6;i++) for(i=0;i<6;i++)
{ {
Serial.print("AN[]:"); Serial.print("AN[]:");
Serial.println(AN_OFFSET[i]); Serial.println(AN_OFFSET[i]);
} }
Serial.print("Yaw neutral value:"); Serial.print("Yaw neutral value:");
Serial.println(Neutro_yaw); Serial.println(Neutro_yaw);
#endif #endif
#if (RADIO_TEST_MODE) // RADIO TEST MODE TO TEST RADIO CHANNELS #if (RADIO_TEST_MODE) // RADIO TEST MODE TO TEST RADIO CHANNELS
while(1) while(1)
{ {
if (APM_RC.GetState()==1) if (APM_RC.GetState()==1)
{ {
Serial.print("AIL:"); Serial.print("AIL:");
Serial.print(APM_RC.InputCh(0)); Serial.print(APM_RC.InputCh(0));
Serial.print("ELE:"); Serial.print("ELE:");
Serial.print(APM_RC.InputCh(1)); Serial.print(APM_RC.InputCh(1));
Serial.print("THR:"); Serial.print("THR:");
Serial.print(APM_RC.InputCh(2)); Serial.print(APM_RC.InputCh(2));
Serial.print("YAW:"); Serial.print("YAW:");
Serial.print(APM_RC.InputCh(3)); Serial.print(APM_RC.InputCh(3));
Serial.print("AUX(mode):"); Serial.print("AUX(mode):");
Serial.print(APM_RC.InputCh(4)); Serial.print(APM_RC.InputCh(4));
Serial.print("AUX2:"); Serial.print("AUX2:");
Serial.print(APM_RC.InputCh(5)); Serial.print(APM_RC.InputCh(5));
Serial.println(); Serial.println();
delay(200); delay(200);
} }
} }
#endif #endif
delay(1000); delay(1000);
DataFlash.StartWrite(1); // Start a write session on page 1 DataFlash.StartWrite(1); // Start a write session on page 1
timer = millis(); timer = millis();
tlmTimer = millis(); tlmTimer = millis();
Read_adc_raw(); // Initialize ADC readings... Read_adc_raw(); // Initialize ADC readings...
delay(20); delay(20);
// Switch Left & Right lights on
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
motorArmed = 0; motorArmed = 0;
digitalWrite(LED_Green,HIGH); // Ready to go... digitalWrite(LED_Green,HIGH); // Ready to go...
} }
/* ***** MAIN LOOP ***** */ /* ***** MAIN LOOP ***** */
void loop(){ void loop(){
int aux; int aux;
int i; int i;
float aux_float; float aux_float;
@ -560,56 +605,56 @@ void loop(){
int log_pitch; int log_pitch;
int log_yaw; int log_yaw;
if((millis()-timer)>=10) // Main loop 100Hz if((millis()-timer)>=10) // Main loop 100Hz
{ {
counter++; counter++;
timer_old = timer; timer_old = timer;
timer=millis(); timer=millis();
G_Dt = (timer-timer_old)/1000.0; // Real time of loop run G_Dt = (timer-timer_old)/1000.0; // Real time of loop run
// IMU DCM Algorithm // IMU DCM Algorithm
Read_adc_raw(); Read_adc_raw();
if (MAGNETOMETER == 1) { if (MAGNETOMETER == 1) {
if (counter > 10) // Read compass data at 10Hz... (10 loop runs) if (counter > 10) // Read compass data at 10Hz... (10 loop runs)
{ {
counter=0; counter=0;
APM_Compass.Read(); // Read magnetometer APM_Compass.Read(); // Read magnetometer
APM_Compass.Calculate(roll,pitch); // Calculate heading APM_Compass.Calculate(roll,pitch); // Calculate heading
} }
} }
Matrix_update(); Matrix_update();
Normalize(); Normalize();
Drift_correction(); Drift_correction();
Euler_angles(); Euler_angles();
// ***************** // *****************
// Output data // Output data
log_roll = ToDeg(roll)*10; log_roll = ToDeg(roll)*10;
log_pitch = ToDeg(pitch)*10; log_pitch = ToDeg(pitch)*10;
log_yaw = ToDeg(yaw)*10; log_yaw = ToDeg(yaw)*10;
#ifndef CONFIGURATOR #ifndef CONFIGURATOR
Serial.print(log_roll); Serial.print(log_roll);
Serial.print(","); Serial.print(",");
Serial.print(log_pitch); Serial.print(log_pitch);
Serial.print(","); Serial.print(",");
Serial.print(log_yaw); Serial.print(log_yaw);
for (int i=0;i<6;i++) for (int i=0;i<6;i++)
{ {
Serial.print(AN[i]); Serial.print(AN[i]);
Serial.print(","); Serial.print(",");
} }
#endif #endif
// Write Sensor raw data to DataFlash log // Write Sensor raw data to DataFlash log
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],gyro_temp); Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],gyro_temp);
// Write attitude to DataFlash log // Write attitude to DataFlash log
Log_Write_Attitude(log_roll,log_pitch,log_yaw); Log_Write_Attitude(log_roll,log_pitch,log_yaw);
if (APM_RC.GetState()==1) // New radio frame? if (APM_RC.GetState()==1) // New radio frame?
{ {
// Commands from radio Rx... // Commands from radio Rx...
// Stick position defines the desired angle in roll, pitch and yaw // Stick position defines the desired angle in roll, pitch and yaw
ch_roll = channel_filter(APM_RC.InputCh(0),ch_roll); ch_roll = channel_filter(APM_RC.InputCh(0),ch_roll);
@ -631,7 +676,7 @@ void loop(){
command_rx_yaw -= 360.0; command_rx_yaw -= 360.0;
else if (command_rx_yaw < -180) else if (command_rx_yaw < -180)
command_rx_yaw += 360.0; command_rx_yaw += 360.0;
// Read through comments in Attitude_control() if you wish to use transmitter to adjust P gains // Read through comments in Attitude_control() if you wish to use transmitter to adjust P gains
// I use K_aux (channel 6) to adjust gains linked to a knob in the radio... [not used now] // I use K_aux (channel 6) to adjust gains linked to a knob in the radio... [not used now]
//K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2; //K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2;
@ -639,38 +684,38 @@ void loop(){
if (K_aux < 0) if (K_aux < 0)
K_aux = 0; K_aux = 0;
//Serial.print(","); //Serial.print(",");
//Serial.print(K_aux); //Serial.print(K_aux);
// We read the Quad Mode from Channel 5 // We read the Quad Mode from Channel 5
if (ch_aux < 1200) if (ch_aux < 1200)
{ {
AP_mode = 1; // Position hold mode (GPS position control) AP_mode = 1; // Position hold mode (GPS position control)
digitalWrite(LED_Yellow,HIGH); // Yellow LED On digitalWrite(LED_Yellow,HIGH); // Yellow LED On
} }
else else
{ {
AP_mode = 2; // Normal mode (Stabilization assist mode) AP_mode = 2; // Normal mode (Stabilization assist mode)
digitalWrite(LED_Yellow,LOW); // Yellow LED off digitalWrite(LED_Yellow,LOW); // Yellow LED off
} }
// Write Radio data to DataFlash log // 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,int(K_aux*100),(int)AP_mode);
} // END new radio data } // END new radio data
if (AP_mode==1) // Position Control if (AP_mode==1) // Position Control
{ {
if (target_position==0) // If this is the first time we switch to Position control, actual position is our target position if (target_position==0) // If this is the first time we switch to Position control, actual position is our target position
{ {
target_lattitude = GPS.Lattitude; target_lattitude = GPS.Lattitude;
target_longitude = GPS.Longitude; target_longitude = GPS.Longitude;
#ifndef CONFIGURATOR #ifndef CONFIGURATOR
Serial.println(); Serial.println();
Serial.print("* Target:"); Serial.print("* Target:");
Serial.print(target_longitude); Serial.print(target_longitude);
Serial.print(","); Serial.print(",");
Serial.println(target_lattitude); Serial.println(target_lattitude);
#endif #endif
target_position=1; target_position=1;
//target_sonar_altitude = sonar_value; //target_sonar_altitude = sonar_value;
//Initial_Throttle = ch3; //Initial_Throttle = ch3;
@ -678,57 +723,60 @@ void loop(){
altitude_I = 0; altitude_I = 0;
gps_roll_I = 0; gps_roll_I = 0;
gps_pitch_I = 0; gps_pitch_I = 0;
} }
} }
else else
target_position=0; target_position=0;
//Read GPS //Read GPS
GPS.Read(); GPS.Read();
if (GPS.NewData) // New GPS data? if (GPS.NewData) // New GPS data?
{ {
GPS.NewData=0; // We Reset the flag... GPS.NewData=0; // We Reset the flag...
//Output GPS data //Output GPS data
//Serial.print(","); //Serial.print(",");
//Serial.print(GPS.Lattitude); //Serial.print(GPS.Lattitude);
//Serial.print(","); //Serial.print(",");
//Serial.print(GPS.Longitude); //Serial.print(GPS.Longitude);
// Write GPS data to DataFlash log // Write GPS data to DataFlash log
Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude, GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats); Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude, GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats);
if (GPS.Fix) if (GPS.Fix)
digitalWrite(LED_Red,HIGH); // GPS Fix => Blue LED digitalWrite(LED_Red,HIGH); // GPS Fix => Blue LED
else else
digitalWrite(LED_Red,LOW); digitalWrite(LED_Red,LOW);
if (AP_mode==1) if (AP_mode==1)
{ {
if ((target_position==1)&&(GPS.Fix)) if ((target_position==1)&&(GPS.Fix))
{ {
Position_control(target_lattitude,target_longitude); // Call position hold routine Position_control(target_lattitude,target_longitude); // Call position hold routine
} }
else else
{ {
//Serial.print("NOFIX"); //Serial.print("NOFIX");
command_gps_roll=0; command_gps_roll=0;
command_gps_pitch=0; command_gps_pitch=0;
}
} }
} }
}
// Control methodology selected using AUX2 // Control methodology selected using AUX2
if (ch_aux2 < 1200) if (ch_aux2 < 1200) {
gled_speed = 1200;
Attitude_control_v2(); Attitude_control_v2();
}
else else
{ {
gled_speed = 400;
Rate_control(); Rate_control();
// 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);
command_rx_yaw_diff = 0; command_rx_yaw_diff = 0;
} }
// Arm motor output : Throttle down and full yaw right for more than 2 seconds // Arm motor output : Throttle down and full yaw right for more than 2 seconds
if (ch_throttle < 1200) { if (ch_throttle < 1200) {
control_yaw = 0; control_yaw = 0;
@ -760,24 +808,29 @@ void loop(){
Arming_counter=0; Arming_counter=0;
Disarming_counter=0; Disarming_counter=0;
} }
// Quadcopter mix // Quadcopter mix
// Ask Jose if we still need this IF statement, and if we want to do an ESC calibration // Ask Jose if we still need this IF statement, and if we want to do an ESC calibration
if (motorArmed == 1) { if (motorArmed == 1) {
#ifdef FLIGHT_MODE_+ digitalWrite(FR_LED, HIGH); // AM-Mode
rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000);
leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000); #ifdef FLIGHT_MODE_+
frontMotor = constrain(ch_throttle + control_pitch + control_yaw, minThrottle, 2000); rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000);
backMotor = constrain(ch_throttle - control_pitch + control_yaw, minThrottle, 2000); leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000);
#endif frontMotor = constrain(ch_throttle + control_pitch + control_yaw, minThrottle, 2000);
#ifdef FLIGHT_MODE_X backMotor = constrain(ch_throttle - control_pitch + control_yaw, minThrottle, 2000);
frontMotor = constrain(ch_throttle + control_roll + control_pitch - control_yaw, minThrottle, 2000); // front left motor #endif
rightMotor = constrain(ch_throttle - control_roll + control_pitch + control_yaw, minThrottle, 2000); // front right motor #ifdef FLIGHT_MODE_X
leftMotor = constrain(ch_throttle + control_roll - control_pitch + control_yaw, minThrottle, 2000); // rear left motor frontMotor = constrain(ch_throttle + control_roll + control_pitch - control_yaw, minThrottle, 2000); // front left motor
backMotor = constrain(ch_throttle - control_roll - control_pitch - control_yaw, minThrottle, 2000); // rear right motor rightMotor = constrain(ch_throttle - control_roll + control_pitch + control_yaw, minThrottle, 2000); // front right motor
#endif leftMotor = constrain(ch_throttle + control_roll - control_pitch + control_yaw, minThrottle, 2000); // rear left motor
backMotor = constrain(ch_throttle - control_roll - control_pitch - control_yaw, minThrottle, 2000); // rear right motor
#endif
} }
if (motorArmed == 0) { if (motorArmed == 0) {
digitalWrite(FR_LED, LOW); // AM-Mode
digitalWrite(LED_Green,HIGH); // Ready LED on
rightMotor = MIN_THROTTLE; rightMotor = MIN_THROTTLE;
leftMotor = MIN_THROTTLE; leftMotor = MIN_THROTTLE;
frontMotor = MIN_THROTTLE; frontMotor = MIN_THROTTLE;
@ -797,15 +850,34 @@ void loop(){
APM_RC.Force_Out0_Out1(); APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3(); APM_RC.Force_Out2_Out3();
#ifndef CONFIGURATOR #ifndef CONFIGURATOR
Serial.println(); // Line END Serial.println(); // Line END
#endif #endif
} }
#ifdef CONFIGURATOR #ifdef CONFIGURATOR
if((millis()-tlmTimer)>=100) { if((millis()-tlmTimer)>=100) {
readSerialCommand(); readSerialCommand();
sendSerialTelemetry(); sendSerialTelemetry();
tlmTimer = millis(); tlmTimer = millis();
} }
#endif #endif
}
// AM and Mode lights
if(millis() - gled_timer > gled_speed) {
gled_timer = millis();
if(gled_status == HIGH) {
digitalWrite(LED_Green, LOW);
digitalWrite(RE_LED, LOW);
gled_status = LOW;
}
else {
digitalWrite(LED_Green, HIGH);
if(motorArmed) digitalWrite(RE_LED, HIGH);
gled_status = HIGH;
}
}
}