mirror of https://github.com/ArduPilot/ardupilot
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
parent
12d54e7e72
commit
36628eeb06
|
|
@ -27,6 +27,12 @@
|
|||
// 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 */
|
||||
#define LED_Yellow 36
|
||||
|
|
@ -48,12 +54,16 @@
|
|||
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
|
||||
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 */
|
||||
|
||||
|
|
@ -91,54 +101,42 @@ int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers
|
|||
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_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_P[3]= {0, 0, 0};//Omega Proportional correction
|
||||
float Omega_I[3]= {0, 0, 0};//Omega Integrator
|
||||
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 Accel_magnitude;
|
||||
//float Accel_weight;
|
||||
|
||||
float errorRollPitch[3]= {0, 0, 0};
|
||||
float errorYaw[3]= {0, 0, 0};
|
||||
float errorCourse=0;
|
||||
float COGX=0; //Course overground X axis
|
||||
float COGY=1; //Course overground Y axis
|
||||
float errorRollPitch[3] = {0, 0, 0};
|
||||
float errorYaw[3] = {0, 0, 0};
|
||||
float errorCourse = 0;
|
||||
float COGX = 0; //Course overground X axis
|
||||
float COGY = 1; //Course overground Y axis
|
||||
|
||||
float roll=0;
|
||||
float pitch=0;
|
||||
float yaw=0;
|
||||
float roll = 0;
|
||||
float pitch = 0;
|
||||
float yaw = 0;
|
||||
|
||||
unsigned int counter=0;
|
||||
unsigned int counter = 0;
|
||||
|
||||
float DCM_Matrix[3][3]= {
|
||||
{
|
||||
1,0,0 }
|
||||
,{
|
||||
0,1,0 }
|
||||
,{
|
||||
0,0,1 }
|
||||
};
|
||||
{ 1,0,0 },
|
||||
{ 0,1,0 },
|
||||
{ 0,0,1 }};
|
||||
float Update_Matrix[3][3]={
|
||||
{
|
||||
0,1,2 }
|
||||
,{
|
||||
3,4,5 }
|
||||
,{
|
||||
6,7,8 }
|
||||
}; //Gyros here
|
||||
{ 0,1,2 },
|
||||
{ 3,4,5 },
|
||||
{ 6,7,8 }}; //Gyros here
|
||||
|
||||
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
|
||||
float speed_3d=0;
|
||||
|
|
@ -200,6 +198,30 @@ float command_altitude;
|
|||
float altitude_I;
|
||||
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
|
||||
int Sonar_value=0;
|
||||
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
|
||||
|
|
@ -243,4 +265,4 @@ long tlmTimer = 0;
|
|||
|
||||
// Arming/Disarming
|
||||
uint8_t Arming_counter=0;
|
||||
uint8_t Disarming_counter=0;
|
||||
uint8_t Disarming_counter=0;
|
||||
|
|
|
|||
|
|
@ -30,14 +30,20 @@
|
|||
GEAR OFF = Flight Assist (Stable Mode)
|
||||
|
||||
**** 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
|
||||
Yellow LED On = GPS Hold Mode
|
||||
Yellow LED Off = Flight Assist Mode (No GPS)
|
||||
Red LED On = GPS Fix, 2D or 3D
|
||||
Red LED Off = No GPS Fix
|
||||
|
||||
|
||||
Green LED blink slow = Motors armed, Stable mode
|
||||
Green LED blink rapid = Motors armed, Acro mode
|
||||
|
||||
*/
|
||||
|
||||
/* User definable modules */
|
||||
|
|
@ -51,6 +57,18 @@
|
|||
|
||||
#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 */
|
||||
|
||||
// Frame build condiguration
|
||||
|
|
@ -67,6 +85,9 @@
|
|||
#include <APM_RC.h>
|
||||
#include <DataFlash.h>
|
||||
#include <APM_Compass.h>
|
||||
#ifdef UseBMP
|
||||
#include <APM_BMP085.h>
|
||||
#endif
|
||||
|
||||
//#include <GPS_NMEA.h> // General NMEA GPS
|
||||
#include <GPS_MTK.h> // MediaTEK DIY Drones GPS.
|
||||
|
|
@ -77,7 +98,8 @@
|
|||
#include "ArduCopter.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
|
||||
// need, go and change them in FrameConfig.h
|
||||
// need, go and change them in UserConfig.h
|
||||
|
||||
/* ************************************************************ */
|
||||
// STABLE MODE
|
||||
|
|
@ -127,14 +149,14 @@ void Attitude_control_v2()
|
|||
else
|
||||
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...
|
||||
pitch_rate = ToDeg(Omega[1]);
|
||||
err_pitch_rate = ((ch_pitch - pitch_mid) >> 1) - pitch_rate;
|
||||
|
||||
pitch_I += err_pitch*G_Dt;
|
||||
pitch_I = constrain(pitch_I,-20,20);
|
||||
pitch_I += err_pitch * G_Dt;
|
||||
pitch_I = constrain(pitch_I, -20, 20);
|
||||
// D term
|
||||
pitch_D = - pitch_rate;
|
||||
|
||||
|
|
@ -197,7 +219,7 @@ void Rate_control()
|
|||
err_yaw = ((ch_yaw - yaw_mid) * xmitFactor) - currentYawRate;
|
||||
|
||||
yaw_I += err_yaw*G_Dt;
|
||||
yaw_I = constrain(yaw_I,-20,20);
|
||||
yaw_I = constrain(yaw_I, -20, 20);
|
||||
|
||||
yaw_D = currentYawRate - previousYawRate;
|
||||
previousYawRate = currentYawRate;
|
||||
|
|
@ -215,15 +237,15 @@ int channel_filter(int ch, int ch_old)
|
|||
if (ch_old==0) // ch_old not initialized
|
||||
return(ch);
|
||||
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)
|
||||
return(ch_old-40); // We limit the max difference between readings
|
||||
if (diff_ch_old <- 40)
|
||||
return(ch_old - 40); // We limit the max difference between readings
|
||||
}
|
||||
else
|
||||
{
|
||||
if (diff_ch_old>40)
|
||||
return(ch_old+40);
|
||||
if (diff_ch_old > 40)
|
||||
return(ch_old + 40);
|
||||
}
|
||||
return((ch + ch_old) >> 1); // Small filtering
|
||||
//return(ch);
|
||||
|
|
@ -234,7 +256,7 @@ int channel_filter(int ch, int ch_old)
|
|||
/* ************************************************************ */
|
||||
void setup()
|
||||
{
|
||||
int i;
|
||||
int i, j;
|
||||
float aux_float[3];
|
||||
|
||||
pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1)
|
||||
|
|
@ -246,7 +268,17 @@ void setup()
|
|||
pinMode(RELE_pin,OUTPUT); // Rele output
|
||||
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_ADC.Init(); // APM ADC library initialization
|
||||
|
|
@ -263,13 +295,12 @@ void setup()
|
|||
#endif
|
||||
|
||||
readUserConfig(); // Load user configurable items from EEPROM
|
||||
|
||||
|
||||
// Safety measure for Channel mids
|
||||
if(roll_mid < 1400 || roll_mid > 1600) roll_mid = 1500;
|
||||
if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
|
||||
if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
|
||||
|
||||
|
||||
// RC channels Initialization (Quad motors)
|
||||
APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped
|
||||
APM_RC.OutputCh(1,MIN_THROTTLE);
|
||||
|
|
@ -306,6 +337,7 @@ void setup()
|
|||
aux_float[1] = gyro_offset_pitch;
|
||||
aux_float[2] = gyro_offset_yaw;
|
||||
|
||||
j = 0;
|
||||
// Take the gyro offset values
|
||||
for(i=0;i<300;i++)
|
||||
{
|
||||
|
|
@ -319,20 +351,42 @@ void setup()
|
|||
//Serial.println();
|
||||
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
|
||||
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++)
|
||||
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
|
||||
for(i=0;i<6;i++)
|
||||
{
|
||||
Serial.print("AN[]:");
|
||||
Serial.println(AN_OFFSET[i]);
|
||||
}
|
||||
|
||||
Serial.print("Yaw neutral value:");
|
||||
// Serial.println(Neutro_yaw);
|
||||
// Serial.println(Neutro_yaw);
|
||||
Serial.print(yaw_mid);
|
||||
#endif
|
||||
|
||||
|
|
@ -366,11 +420,13 @@ void setup()
|
|||
tlmTimer = millis();
|
||||
Read_adc_raw(); // Initialize ADC readings...
|
||||
delay(20);
|
||||
|
||||
// Switch Left & Right lights on
|
||||
|
||||
#ifdef IsAM
|
||||
// Switch Left & Right lights on
|
||||
digitalWrite(RI_LED, HIGH);
|
||||
digitalWrite(LE_LED, HIGH);
|
||||
|
||||
#endif
|
||||
|
||||
motorArmed = 0;
|
||||
digitalWrite(LED_Green,HIGH); // Ready to go...
|
||||
}
|
||||
|
|
@ -384,12 +440,12 @@ void loop(){
|
|||
int aux;
|
||||
int i;
|
||||
float aux_float;
|
||||
|
||||
//Log variables
|
||||
int log_roll;
|
||||
int log_pitch;
|
||||
int log_yaw;
|
||||
|
||||
|
||||
if((millis()-timer)>=10) // Main loop 100Hz
|
||||
{
|
||||
counter++;
|
||||
|
|
@ -409,12 +465,13 @@ void loop(){
|
|||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
Matrix_update();
|
||||
Normalize();
|
||||
Drift_correction();
|
||||
Euler_angles();
|
||||
// *****************
|
||||
|
||||
// *****************
|
||||
// Output data
|
||||
log_roll = ToDeg(roll) * 10;
|
||||
log_pitch = ToDeg(pitch) * 10;
|
||||
|
|
@ -427,7 +484,7 @@ void loop(){
|
|||
Serial.print(",");
|
||||
Serial.print(log_yaw);
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
Serial.print(AN[i]);
|
||||
Serial.print(",");
|
||||
|
|
@ -439,7 +496,7 @@ void loop(){
|
|||
// Write attitude to DataFlash log
|
||||
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...
|
||||
// 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 = (ch_pitch-CHANN_CENTER) / 12.0;
|
||||
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;
|
||||
command_rx_yaw += aux_float;
|
||||
command_rx_yaw_diff = aux_float;
|
||||
|
|
@ -604,8 +661,9 @@ void loop(){
|
|||
// Quadcopter mix
|
||||
// Ask Jose if we still need this IF statement, and if we want to do an ESC calibration
|
||||
if (motorArmed == 1) {
|
||||
digitalWrite(FR_LED, HIGH); // AM-Mode
|
||||
|
||||
#ifdef IsAM
|
||||
digitalWrite(FR_LED, HIGH); // AM-Mode
|
||||
#endif
|
||||
#ifdef FLIGHT_MODE_+
|
||||
rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000);
|
||||
leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000);
|
||||
|
|
@ -620,9 +678,11 @@ void loop(){
|
|||
#endif
|
||||
}
|
||||
if (motorArmed == 0) {
|
||||
#ifdef IsAM
|
||||
digitalWrite(FR_LED, LOW); // AM-Mode
|
||||
#endif
|
||||
digitalWrite(LED_Green,HIGH); // Ready LED on
|
||||
|
||||
|
||||
rightMotor = MIN_THROTTLE;
|
||||
leftMotor = MIN_THROTTLE;
|
||||
frontMotor = MIN_THROTTLE;
|
||||
|
|
@ -638,8 +698,8 @@ void loop(){
|
|||
APM_RC.OutputCh(1, leftMotor); // Left motor
|
||||
APM_RC.OutputCh(2, frontMotor); // Front motor
|
||||
APM_RC.OutputCh(3, backMotor); // Back motor
|
||||
|
||||
// InstantPWM
|
||||
|
||||
// InstantPWM
|
||||
APM_RC.Force_Out0_Out1();
|
||||
APM_RC.Force_Out2_Out3();
|
||||
|
||||
|
|
@ -674,7 +734,10 @@ void loop(){
|
|||
}
|
||||
}
|
||||
|
||||
} // End of void loop()
|
||||
|
||||
|
||||
// END of Arducopter.pde
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -135,11 +135,17 @@ void comma() {
|
|||
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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -33,27 +33,27 @@ void Position_control(long lat_dest, long lon_dest)
|
|||
// ROLL
|
||||
gps_err_roll_old = gps_err_roll;
|
||||
//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 = constrain(gps_roll_I,-800,800);
|
||||
gps_roll_I += gps_err_roll * GPS_Dt;
|
||||
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 = constrain(command_gps_roll,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
|
||||
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
|
||||
|
||||
// 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 = constrain(gps_pitch_I,-800,800);
|
||||
gps_pitch_I += gps_err_pitch * GPS_Dt;
|
||||
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 = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
|
||||
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
|
||||
}
|
||||
|
||||
/* ************************************************************ */
|
||||
|
|
@ -68,4 +68,4 @@ void Altitude_control(int target_sonar_altitude)
|
|||
command_altitude = Initial_Throttle + KP_ALTITUDE * err_altitude + KD_ALTITUDE * altitude_D + KI_ALTITUDE * altitude_I;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -375,7 +375,7 @@ void sendSerialTelemetry() {
|
|||
case 'X': // Stop sending messages
|
||||
break;
|
||||
case '!': // Send flight software version
|
||||
Serial.println(SWVER);
|
||||
Serial.println(VER);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case '.': // Modify GPS settings
|
||||
|
|
@ -403,4 +403,4 @@ float readFloatSerial() {
|
|||
}
|
||||
while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
|
||||
return atof(data);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,13 +25,14 @@ TODO:
|
|||
|
||||
************************************************************* */
|
||||
|
||||
|
||||
|
||||
/*************************************************************/
|
||||
// Safety & Security
|
||||
|
||||
// Arm & Disarm delays
|
||||
#define ARM_DELAY 200
|
||||
#define DISARM_DELAY 100
|
||||
#define ARM_DELAY 200 // milliseconds of how long you need to keep rudder to max right for arming motors
|
||||
#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 ROLL_MID 1478 // Radio Roll channel mid value
|
||||
#define PITCH_MID 1483 // Radio Pitch channel mid value
|
||||
#define ROLL_MID 1500 // Radio Roll channel mid value
|
||||
#define PITCH_MID 1500 // Radio Pitch 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 CHANN_CENTER 1500 // Channel center, legacy
|
||||
|
|
@ -68,25 +69,25 @@ TODO:
|
|||
|
||||
// Following variables stored in EEPROM
|
||||
float KP_QUAD_ROLL;
|
||||
float KD_QUAD_ROLL;
|
||||
float KI_QUAD_ROLL;
|
||||
float KD_QUAD_ROLL;
|
||||
float KP_QUAD_PITCH;
|
||||
float KD_QUAD_PITCH;
|
||||
float KI_QUAD_PITCH;
|
||||
float KD_QUAD_PITCH;
|
||||
float KP_QUAD_YAW;
|
||||
float KD_QUAD_YAW;
|
||||
float KI_QUAD_YAW;
|
||||
float KD_QUAD_YAW;
|
||||
float STABLE_MODE_KP_RATE;
|
||||
float KP_GPS_ROLL;
|
||||
float KD_GPS_ROLL;
|
||||
float KI_GPS_ROLL;
|
||||
float KD_GPS_ROLL;
|
||||
float KP_GPS_PITCH;
|
||||
float KD_GPS_PITCH;
|
||||
float KI_GPS_PITCH;
|
||||
float KD_GPS_PITCH;
|
||||
float GPS_MAX_ANGLE;
|
||||
float KP_ALTITUDE;
|
||||
float KD_ALTITUDE;
|
||||
float KI_ALTITUDE;
|
||||
float KD_ALTITUDE;
|
||||
int acc_offset_x;
|
||||
int acc_offset_y;
|
||||
int acc_offset_z;
|
||||
|
|
@ -126,25 +127,25 @@ float ch_aux2_offset = 0;
|
|||
// when a "Default EEPROM Value" command is sent through serial interface
|
||||
void setUserConfig() {
|
||||
KP_QUAD_ROLL = 1.8;
|
||||
KD_QUAD_ROLL = 0.4; //0.48
|
||||
KI_QUAD_ROLL = 0.30; //0.4
|
||||
KD_QUAD_ROLL = 0.4; //0.48
|
||||
KP_QUAD_PITCH = 1.8;
|
||||
KD_QUAD_PITCH = 0.4; //0.48
|
||||
KI_QUAD_PITCH = 0.30; //0.4
|
||||
KD_QUAD_PITCH = 0.4; //0.48
|
||||
KP_QUAD_YAW = 3.6;
|
||||
KD_QUAD_YAW = 1.2;
|
||||
KI_QUAD_YAW = 0.15;
|
||||
KD_QUAD_YAW = 1.2;
|
||||
STABLE_MODE_KP_RATE = 0.2; // New param for stable mode
|
||||
KP_GPS_ROLL = 0.012;
|
||||
KD_GPS_ROLL = 0.005;
|
||||
KI_GPS_ROLL = 0.004;
|
||||
KP_GPS_PITCH = 0.012;
|
||||
KD_GPS_PITCH = 0.005;
|
||||
KI_GPS_PITCH = 0.004;
|
||||
GPS_MAX_ANGLE = 10;
|
||||
KP_GPS_ROLL = 0.02;
|
||||
KI_GPS_ROLL = 0.008;
|
||||
KD_GPS_ROLL = 0.006;
|
||||
KP_GPS_PITCH = 0.02;
|
||||
KI_GPS_PITCH = 0.008;
|
||||
KD_GPS_PITCH = 0.006;
|
||||
GPS_MAX_ANGLE = 18;
|
||||
KP_ALTITUDE = 0.8;
|
||||
KD_ALTITUDE = 0.2;
|
||||
KI_ALTITUDE = 0.2;
|
||||
KD_ALTITUDE = 0.2;
|
||||
acc_offset_x = 2073;
|
||||
acc_offset_y = 2056;
|
||||
acc_offset_z = 2010;
|
||||
|
|
@ -186,25 +187,25 @@ void setUserConfig() {
|
|||
|
||||
// EEPROM storage addresses
|
||||
#define KP_QUAD_ROLL_ADR 0
|
||||
#define KD_QUAD_ROLL_ADR 4
|
||||
#define KI_QUAD_ROLL_ADR 8
|
||||
#define KD_QUAD_ROLL_ADR 4
|
||||
#define KP_QUAD_PITCH_ADR 12
|
||||
#define KD_QUAD_PITCH_ADR 16
|
||||
#define KI_QUAD_PITCH_ADR 20
|
||||
#define KD_QUAD_PITCH_ADR 16
|
||||
#define KP_QUAD_YAW_ADR 24
|
||||
#define KD_QUAD_YAW_ADR 28
|
||||
#define KI_QUAD_YAW_ADR 32
|
||||
#define KD_QUAD_YAW_ADR 28
|
||||
#define STABLE_MODE_KP_RATE_ADR 36
|
||||
#define KP_GPS_ROLL_ADR 40
|
||||
#define KD_GPS_ROLL_ADR 44
|
||||
#define KI_GPS_ROLL_ADR 48
|
||||
#define KD_GPS_ROLL_ADR 44
|
||||
#define KP_GPS_PITCH_ADR 52
|
||||
#define KD_GPS_PITCH_ADR 56
|
||||
#define KI_GPS_PITCH_ADR 60
|
||||
#define KD_GPS_PITCH_ADR 56
|
||||
#define GPS_MAX_ANGLE_ADR 64
|
||||
#define KP_ALTITUDE_ADR 68
|
||||
#define KD_ALTITUDE_ADR 72
|
||||
#define KI_ALTITUDE_ADR 76
|
||||
#define KD_ALTITUDE_ADR 72
|
||||
#define acc_offset_x_ADR 80
|
||||
#define acc_offset_y_ADR 84
|
||||
#define acc_offset_z_ADR 88
|
||||
|
|
@ -268,25 +269,25 @@ void writeEEPROM(float value, int address) {
|
|||
|
||||
void readUserConfig() {
|
||||
KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR);
|
||||
KD_QUAD_ROLL = readEEPROM(KD_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);
|
||||
KD_QUAD_PITCH = readEEPROM(KD_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);
|
||||
KD_QUAD_YAW = readEEPROM(KD_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);
|
||||
KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
|
||||
KD_GPS_ROLL = readEEPROM(KD_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);
|
||||
KD_GPS_PITCH = readEEPROM(KD_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);
|
||||
KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR);
|
||||
KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
|
||||
KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR);
|
||||
KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
|
||||
acc_offset_x = readEEPROM(acc_offset_x_ADR);
|
||||
acc_offset_y = readEEPROM(acc_offset_y_ADR);
|
||||
acc_offset_z = readEEPROM(acc_offset_z_ADR);
|
||||
|
|
@ -326,4 +327,4 @@ void readUserConfig() {
|
|||
ch_aux_offset = readEEPROM(ch_aux_offset_ADR);
|
||||
ch_aux2_offset = readEEPROM(ch_aux2_offset_ADR); */
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue