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

cleaning up code and adding functions from APM main code 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@250 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-08-20 16:41:09 +00:00
parent 12d54e7e72
commit 36628eeb06
6 changed files with 218 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
Arducopter/ArduCopter.h
View File

@ -27,6 +27,12 @@
// User configurable settings are on UserConfig.h // User configurable settings are on UserConfig.h
/*******************************************************************/ /*******************************************************************/
/**************************************************************/
// Special features that might disapear in future releases
//#define jpframe // This is only Jani's special frame, you should never use unless you know what you are doing
// As default this should be always checked off.
/* APM Hardware definitions */ /* APM Hardware definitions */
#define LED_Yellow 36 #define LED_Yellow 36
@ -48,12 +54,16 @@
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, MAGX, MAGY, MAGZ // Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ, MAGX, MAGY, MAGZ
int SENSOR_SIGN[]={
1, -1, -1, // GYROX, GYROY, GYROZ
-1, 1, 1, // ACCELX, ACCELY, ACCELZ
-1, -1, -1}; // MAGNETOX, MAGNETOY, MAGNETOZ
//{-1,1,-1,1,-1,1,-1,-1,-1};
#ifndef jpframe
int SENSOR_SIGN[]={
1, -1, -1, -1, 1, 1, -1, -1, -1};
//{-1,1,-1,1,-1,1,-1,-1,-1};
#else
int SENSOR_SIGN[]={
1, -1, 1, -1, 1, 1, -1, -1, -1};
//{-1,1,-1,1,-1,1,-1,-1,-1};
#endif
/* APM Hardware definitions, END */ /* APM Hardware definitions, END */
@ -94,51 +104,39 @@ 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]= {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_Vector_unfiltered[3]= {0, 0, 0}; //Store the acceleration in a vector
float Gyro_Vector[3]= {0, 0, 0};//Store the gyros rutn rate in a vector float Gyro_Vector[3]= {0, 0, 0}; //Store the gyros rutn rate in a vector
float Omega_Vector[3]= {0, 0, 0}; //Corrected Gyro_Vector data float Omega_Vector[3]= {0, 0, 0}; //Corrected Gyro_Vector data
float Omega_P[3]= {0, 0, 0};//Omega Proportional correction float Omega_P[3]= {0, 0, 0}; //Omega Proportional correction
float Omega_I[3]= {0, 0, 0};//Omega Integrator float Omega_I[3]= {0, 0, 0}; //Omega Integrator
float Omega[3]= {0, 0, 0}; float Omega[3]= {0, 0, 0};
//float Accel_magnitude; //float Accel_magnitude;
//float Accel_weight; //float Accel_weight;
float errorRollPitch[3]= {0, 0, 0}; float errorRollPitch[3] = {0, 0, 0};
float errorYaw[3]= {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
float roll=0; float roll = 0;
float pitch=0; float pitch = 0;
float yaw=0; float yaw = 0;
unsigned int counter=0; unsigned int counter = 0;
float DCM_Matrix[3][3]= { float DCM_Matrix[3][3]= {
{ { 1,0,0 },
1,0,0 } { 0,1,0 },
,{ { 0,0,1 }};
0,1,0 }
,{
0,0,1 }
};
float Update_Matrix[3][3]={ float Update_Matrix[3][3]={
{ { 0,1,2 },
0,1,2 } { 3,4,5 },
,{ { 6,7,8 }}; //Gyros here
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
float speed_3d=0; float speed_3d=0;
@ -200,6 +198,30 @@ float command_altitude;
float altitude_I; float altitude_I;
float altitude_D; float altitude_D;
//Pressure Sensor variables
#ifdef UseBMP
unsigned long abs_press = 0;
unsigned long abs_press_filt = 0;
unsigned long abs_press_gnd = 0;
int ground_temperature = 0; //
int temp_unfilt = 0;
long ground_alt = 0; // Ground altitude from gps at startup in centimeters
long press_alt = 0; // Pressure altitude
#endif
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
#define AIRSPEED_PIN 1 // Need to correct value
#define BATTERY_PIN 1 // Need to correct value
#define RELAY_PIN 47
#define LOW_VOLTAGE 11.4 // Pack voltage at which to trigger alarm
#define INPUT_VOLTAGE 5.2 // (Volts) voltage your power regulator is feeding your ArduPilot to have an accurate pressure and battery level readings. (you need a multimeter to measure and set this of course)
#define VOLT_DIV_RATIO 1.0 // Voltage divider ratio set with thru-hole resistor (see manual)
float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage, initialized above threshold for filter
// Sonar variables // Sonar variables
int Sonar_value=0; int Sonar_value=0;
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters #define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters

111
Arducopter/Arducopter.pde
View File

@ -30,6 +30,11 @@
GEAR OFF = Flight Assist (Stable Mode) GEAR OFF = Flight Assist (Stable Mode)
**** LED Feedback **** **** LED Feedback ****
Bootup Sequence:
1) A, B, C LED's blinking rapidly while waiting ESCs to bootup and initial shake to end from connecting battery
2) A, B, C LED's have running light while calibrating Gyro/Acc's
3) Green LED Solid after initialization finished
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)
@ -38,6 +43,7 @@
Green LED blink slow = Motors armed, Stable mode Green LED blink slow = Motors armed, Stable mode
Green LED blink rapid = Motors armed, Acro mode Green LED blink rapid = Motors armed, Acro mode
*/ */
/* User definable modules */ /* User definable modules */
@ -51,6 +57,18 @@
#define CONFIGURATOR // Do se use Configurator or normal text output over serial link #define CONFIGURATOR // Do se use Configurator or normal text output over serial link
/**********************************************/
// Not in use yet, starting to work with battery monitors and pressure sensors.
// Added 19-08-2010
//#define UseAirspeed
//#define UseBMP
//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it wired up!)
/**********************************************/
/* User definable modules - END */ /* User definable modules - END */
// Frame build condiguration // Frame build condiguration
@ -67,6 +85,9 @@
#include <APM_RC.h> #include <APM_RC.h>
#include <DataFlash.h> #include <DataFlash.h>
#include <APM_Compass.h> #include <APM_Compass.h>
#ifdef UseBMP
#include <APM_BMP085.h>
#endif
//#include <GPS_NMEA.h> // General NMEA GPS //#include <GPS_NMEA.h> // General NMEA GPS
#include <GPS_MTK.h> // MediaTEK DIY Drones GPS. #include <GPS_MTK.h> // MediaTEK DIY Drones GPS.
@ -77,7 +98,8 @@
#include "ArduCopter.h" #include "ArduCopter.h"
#include "UserConfig.h" #include "UserConfig.h"
#define SWVER 1.31 // Current software version (only numeric values) /* Software version */
#define VER 1.32 // Current software version (only numeric values)
/* ***************************************************************************** */ /* ***************************************************************************** */
@ -85,7 +107,7 @@
/* ***************************************************************************** */ /* ***************************************************************************** */
// Normal users does not need to edit anything below these lines. If you have // Normal users does not need to edit anything below these lines. If you have
// need, go and change them in FrameConfig.h // need, go and change them in UserConfig.h
/* ************************************************************ */ /* ************************************************************ */
// STABLE MODE // STABLE MODE
@ -127,14 +149,14 @@ void Attitude_control_v2()
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 - pitch_mid) >> 1) - pitch_rate; err_pitch_rate = ((ch_pitch - pitch_mid) >> 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;
@ -197,7 +219,7 @@ void Rate_control()
err_yaw = ((ch_yaw - yaw_mid) * xmitFactor) - currentYawRate; err_yaw = ((ch_yaw - yaw_mid) * 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;
@ -215,15 +237,15 @@ int channel_filter(int ch, int 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);
@ -234,7 +256,7 @@ int channel_filter(int ch, int ch_old)
/* ************************************************************ */ /* ************************************************************ */
void setup() void setup()
{ {
int i; int i, j;
float aux_float[3]; float aux_float[3];
pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1) pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1)
@ -246,7 +268,17 @@ void setup()
pinMode(RELE_pin,OUTPUT); // Rele output pinMode(RELE_pin,OUTPUT); // Rele output
digitalWrite(RELE_pin,LOW); digitalWrite(RELE_pin,LOW);
delay(1000); // Wait until frame is not moving after initial power cord has connected // delay(1000); // Wait until frame is not moving after initial power cord has connected
for(i = 0; i <= 50; i++) {
digitalWrite(LED_Green, HIGH);
digitalWrite(LED_Yellow, HIGH);
digitalWrite(LED_Red, HIGH);
delay(20);
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, LOW);
delay(20);
}
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
@ -269,7 +301,6 @@ void setup()
if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500; if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500; if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
// RC channels Initialization (Quad motors) // RC channels Initialization (Quad motors)
APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped
APM_RC.OutputCh(1,MIN_THROTTLE); APM_RC.OutputCh(1,MIN_THROTTLE);
@ -306,6 +337,7 @@ void setup()
aux_float[1] = gyro_offset_pitch; aux_float[1] = gyro_offset_pitch;
aux_float[2] = gyro_offset_yaw; aux_float[2] = gyro_offset_yaw;
j = 0;
// Take the gyro offset values // Take the gyro offset values
for(i=0;i<300;i++) for(i=0;i<300;i++)
{ {
@ -319,20 +351,42 @@ void setup()
//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);
// Runnings lights effect to let user know that we are taking mesurements
if(j == 0) {
digitalWrite(LED_Green, HIGH);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, LOW);
} }
else if (j == 1) {
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, HIGH);
digitalWrite(LED_Red, LOW);
}
else {
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, HIGH);
}
if((i % 5) == 0) j++;
if(j >= 3) j = 0;
}
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, LOW);
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);
Serial.print(yaw_mid); Serial.print(yaw_mid);
#endif #endif
@ -367,9 +421,11 @@ void setup()
Read_adc_raw(); // Initialize ADC readings... Read_adc_raw(); // Initialize ADC readings...
delay(20); delay(20);
#ifdef IsAM
// Switch Left & Right lights on // Switch Left & Right lights on
digitalWrite(RI_LED, HIGH); digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH); digitalWrite(LE_LED, HIGH);
#endif
motorArmed = 0; motorArmed = 0;
digitalWrite(LED_Green,HIGH); // Ready to go... digitalWrite(LED_Green,HIGH); // Ready to go...
@ -384,12 +440,12 @@ void loop(){
int aux; int aux;
int i; int i;
float aux_float; float aux_float;
//Log variables //Log variables
int log_roll; int log_roll;
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++;
@ -409,12 +465,13 @@ void loop(){
} }
} }
#endif #endif
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;
@ -427,7 +484,7 @@ void loop(){
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(",");
@ -439,7 +496,7 @@ void loop(){
// 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
@ -455,7 +512,7 @@ void loop(){
command_rx_pitch_old = command_rx_pitch; command_rx_pitch_old = command_rx_pitch;
command_rx_pitch = (ch_pitch-CHANN_CENTER) / 12.0; command_rx_pitch = (ch_pitch-CHANN_CENTER) / 12.0;
command_rx_pitch_diff = command_rx_pitch - command_rx_pitch_old; command_rx_pitch_diff = command_rx_pitch - command_rx_pitch_old;
// aux_float = (ch_yaw-Neutro_yaw) / 180.0; // aux_float = (ch_yaw-Neutro_yaw) / 180.0;
aux_float = (ch_yaw-yaw_mid) / 180.0; aux_float = (ch_yaw-yaw_mid) / 180.0;
command_rx_yaw += aux_float; command_rx_yaw += aux_float;
command_rx_yaw_diff = aux_float; command_rx_yaw_diff = aux_float;
@ -604,8 +661,9 @@ void loop(){
// 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 IsAM
digitalWrite(FR_LED, HIGH); // AM-Mode digitalWrite(FR_LED, HIGH); // AM-Mode
#endif
#ifdef FLIGHT_MODE_+ #ifdef FLIGHT_MODE_+
rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000); rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000);
leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000); leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000);
@ -620,7 +678,9 @@ void loop(){
#endif #endif
} }
if (motorArmed == 0) { if (motorArmed == 0) {
#ifdef IsAM
digitalWrite(FR_LED, LOW); // AM-Mode digitalWrite(FR_LED, LOW); // AM-Mode
#endif
digitalWrite(LED_Green,HIGH); // Ready LED on digitalWrite(LED_Green,HIGH); // Ready LED on
rightMotor = MIN_THROTTLE; rightMotor = MIN_THROTTLE;
@ -674,7 +734,10 @@ void loop(){
} }
} }
} // End of void loop()
// END of Arducopter.pde
}

10
Arducopter/Functions.pde
View File

@ -135,8 +135,14 @@ void comma() {
Serial.print(','); Serial.print(',');
} }
#if BATTERY_EVENT == 1
void low_battery_event(void)
{
// send_message(SEVERITY_HIGH,"Low Battery!");
// set_mode(RTL);
// throttle_cruise = THROTTLE_CRUISE;
}
#endif

24
Arducopter/Navigation.pde
View File

@ -33,27 +33,27 @@ void Position_control(long lat_dest, long lon_dest)
// 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)/GPS_Dt; gps_roll_D = (gps_err_roll-gps_err_roll_old) / GPS_Dt;
gps_roll_I += gps_err_roll*GPS_Dt; gps_roll_I += gps_err_roll * GPS_Dt;
gps_roll_I = constrain(gps_roll_I,-800,800); gps_roll_I = constrain(gps_roll_I, -800, 800);
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)/GPS_Dt; gps_pitch_D = (gps_err_pitch - gps_err_pitch_old) / GPS_Dt;
gps_pitch_I += gps_err_pitch*GPS_Dt; gps_pitch_I += gps_err_pitch * GPS_Dt;
gps_pitch_I = constrain(gps_pitch_I,-800,800); gps_pitch_I = constrain(gps_pitch_I, -800, 800);
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
} }
/* ************************************************************ */ /* ************************************************************ */

2
Arducopter/SerialCom.pde
View File

@ -375,7 +375,7 @@ void sendSerialTelemetry() {
case 'X': // Stop sending messages case 'X': // Stop sending messages
break; break;
case '!': // Send flight software version case '!': // Send flight software version
Serial.println(SWVER); Serial.println(VER);
queryType = 'X'; queryType = 'X';
break; break;
case '.': // Modify GPS settings case '.': // Modify GPS settings

69
Arducopter/UserConfig.h
View File

@ -26,12 +26,13 @@ TODO:
************************************************************* */ ************************************************************* */
/*************************************************************/ /*************************************************************/
// Safety & Security // Safety & Security
// Arm & Disarm delays // Arm & Disarm delays
#define ARM_DELAY 200 #define ARM_DELAY 200 // milliseconds of how long you need to keep rudder to max right for arming motors
#define DISARM_DELAY 100 #define DISARM_DELAY 100 // milliseconds of how long you need to keep rudder to max left for disarming motors
/*************************************************************/ /*************************************************************/
@ -57,10 +58,10 @@ TODO:
//#define RADIO_TEST_MODE //#define RADIO_TEST_MODE
#define ROLL_MID 1478 // Radio Roll channel mid value #define ROLL_MID 1500 // Radio Roll channel mid value
#define PITCH_MID 1483 // Radio Pitch channel mid value #define PITCH_MID 1500 // Radio Pitch channel mid value
#define YAW_MID 1500 // Radio Yaw channel mid value #define YAW_MID 1500 // Radio Yaw channel mid value
#define THROTTLE_MID 1502 // Radio Throttle channel mid value #define THROTTLE_MID 1505 // Radio Throttle channel mid value
#define AUX_MID 1500 #define AUX_MID 1500
#define CHANN_CENTER 1500 // Channel center, legacy #define CHANN_CENTER 1500 // Channel center, legacy
@ -68,25 +69,25 @@ TODO:
// Following variables stored in EEPROM // Following variables stored in EEPROM
float KP_QUAD_ROLL; float KP_QUAD_ROLL;
float KD_QUAD_ROLL;
float KI_QUAD_ROLL; float KI_QUAD_ROLL;
float KD_QUAD_ROLL;
float KP_QUAD_PITCH; float KP_QUAD_PITCH;
float KD_QUAD_PITCH;
float KI_QUAD_PITCH; float KI_QUAD_PITCH;
float KD_QUAD_PITCH;
float KP_QUAD_YAW; float KP_QUAD_YAW;
float KD_QUAD_YAW;
float KI_QUAD_YAW; float KI_QUAD_YAW;
float KD_QUAD_YAW;
float STABLE_MODE_KP_RATE; float STABLE_MODE_KP_RATE;
float KP_GPS_ROLL; float KP_GPS_ROLL;
float KD_GPS_ROLL;
float KI_GPS_ROLL; float KI_GPS_ROLL;
float KD_GPS_ROLL;
float KP_GPS_PITCH; float KP_GPS_PITCH;
float KD_GPS_PITCH;
float KI_GPS_PITCH; float KI_GPS_PITCH;
float KD_GPS_PITCH;
float GPS_MAX_ANGLE; float GPS_MAX_ANGLE;
float KP_ALTITUDE; float KP_ALTITUDE;
float KD_ALTITUDE;
float KI_ALTITUDE; float KI_ALTITUDE;
float KD_ALTITUDE;
int acc_offset_x; int acc_offset_x;
int acc_offset_y; int acc_offset_y;
int acc_offset_z; int acc_offset_z;
@ -126,25 +127,25 @@ float ch_aux2_offset = 0;
// when a "Default EEPROM Value" command is sent through serial interface // when a "Default EEPROM Value" command is sent through serial interface
void setUserConfig() { void setUserConfig() {
KP_QUAD_ROLL = 1.8; KP_QUAD_ROLL = 1.8;
KD_QUAD_ROLL = 0.4; //0.48
KI_QUAD_ROLL = 0.30; //0.4 KI_QUAD_ROLL = 0.30; //0.4
KD_QUAD_ROLL = 0.4; //0.48
KP_QUAD_PITCH = 1.8; KP_QUAD_PITCH = 1.8;
KD_QUAD_PITCH = 0.4; //0.48
KI_QUAD_PITCH = 0.30; //0.4 KI_QUAD_PITCH = 0.30; //0.4
KD_QUAD_PITCH = 0.4; //0.48
KP_QUAD_YAW = 3.6; KP_QUAD_YAW = 3.6;
KD_QUAD_YAW = 1.2;
KI_QUAD_YAW = 0.15; KI_QUAD_YAW = 0.15;
KD_QUAD_YAW = 1.2;
STABLE_MODE_KP_RATE = 0.2; // New param for stable mode STABLE_MODE_KP_RATE = 0.2; // New param for stable mode
KP_GPS_ROLL = 0.012; KP_GPS_ROLL = 0.02;
KD_GPS_ROLL = 0.005; KI_GPS_ROLL = 0.008;
KI_GPS_ROLL = 0.004; KD_GPS_ROLL = 0.006;
KP_GPS_PITCH = 0.012; KP_GPS_PITCH = 0.02;
KD_GPS_PITCH = 0.005; KI_GPS_PITCH = 0.008;
KI_GPS_PITCH = 0.004; KD_GPS_PITCH = 0.006;
GPS_MAX_ANGLE = 10; GPS_MAX_ANGLE = 18;
KP_ALTITUDE = 0.8; KP_ALTITUDE = 0.8;
KD_ALTITUDE = 0.2;
KI_ALTITUDE = 0.2; KI_ALTITUDE = 0.2;
KD_ALTITUDE = 0.2;
acc_offset_x = 2073; acc_offset_x = 2073;
acc_offset_y = 2056; acc_offset_y = 2056;
acc_offset_z = 2010; acc_offset_z = 2010;
@ -186,25 +187,25 @@ void setUserConfig() {
// EEPROM storage addresses // EEPROM storage addresses
#define KP_QUAD_ROLL_ADR 0 #define KP_QUAD_ROLL_ADR 0
#define KD_QUAD_ROLL_ADR 4
#define KI_QUAD_ROLL_ADR 8 #define KI_QUAD_ROLL_ADR 8
#define KD_QUAD_ROLL_ADR 4
#define KP_QUAD_PITCH_ADR 12 #define KP_QUAD_PITCH_ADR 12
#define KD_QUAD_PITCH_ADR 16
#define KI_QUAD_PITCH_ADR 20 #define KI_QUAD_PITCH_ADR 20
#define KD_QUAD_PITCH_ADR 16
#define KP_QUAD_YAW_ADR 24 #define KP_QUAD_YAW_ADR 24
#define KD_QUAD_YAW_ADR 28
#define KI_QUAD_YAW_ADR 32 #define KI_QUAD_YAW_ADR 32
#define KD_QUAD_YAW_ADR 28
#define STABLE_MODE_KP_RATE_ADR 36 #define STABLE_MODE_KP_RATE_ADR 36
#define KP_GPS_ROLL_ADR 40 #define KP_GPS_ROLL_ADR 40
#define KD_GPS_ROLL_ADR 44
#define KI_GPS_ROLL_ADR 48 #define KI_GPS_ROLL_ADR 48
#define KD_GPS_ROLL_ADR 44
#define KP_GPS_PITCH_ADR 52 #define KP_GPS_PITCH_ADR 52
#define KD_GPS_PITCH_ADR 56
#define KI_GPS_PITCH_ADR 60 #define KI_GPS_PITCH_ADR 60
#define KD_GPS_PITCH_ADR 56
#define GPS_MAX_ANGLE_ADR 64 #define GPS_MAX_ANGLE_ADR 64
#define KP_ALTITUDE_ADR 68 #define KP_ALTITUDE_ADR 68
#define KD_ALTITUDE_ADR 72
#define KI_ALTITUDE_ADR 76 #define KI_ALTITUDE_ADR 76
#define KD_ALTITUDE_ADR 72
#define acc_offset_x_ADR 80 #define acc_offset_x_ADR 80
#define acc_offset_y_ADR 84 #define acc_offset_y_ADR 84
#define acc_offset_z_ADR 88 #define acc_offset_z_ADR 88
@ -268,25 +269,25 @@ void writeEEPROM(float value, int address) {
void readUserConfig() { void readUserConfig() {
KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR); KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR);
KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR);
KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR); KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR);
KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR);
KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR); KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR);
KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR);
KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR); KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR);
KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR);
KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR); KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR); KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR);
KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR); STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR);
KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR); KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR); KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);
KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR); KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR);
KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR);
KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR); KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR);
KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR);
GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR); GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR);
KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR); KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR);
KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR); KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR);
KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
acc_offset_x = readEEPROM(acc_offset_x_ADR); acc_offset_x = readEEPROM(acc_offset_x_ADR);
acc_offset_y = readEEPROM(acc_offset_y_ADR); acc_offset_y = readEEPROM(acc_offset_y_ADR);
acc_offset_z = readEEPROM(acc_offset_z_ADR); acc_offset_z = readEEPROM(acc_offset_z_ADR);