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

gps hold and baro updates 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@877 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-11-21 12:56:25 +00:00
parent 0eef05b734
commit e7eda28984
4 changed files with 26 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArducopterNG/Arducopter.h
View File

@ -288,10 +288,12 @@ int err_altitude_old;
float command_altitude; float command_altitude;
float altitude_I; float altitude_I;
float altitude_D; float altitude_D;
int ch_throttle_altitude_hold;
//Pressure Sensor variables //Pressure Sensor variables
long target_baro_altitude; long target_baro_altitude; // Altitude in cm
long press_alt = 0; long press_alt = 0;
byte Baro_new_data = 0;
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO #define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO

21
ArducopterNG/ArducopterNG.pde
View File

@ -60,7 +60,7 @@
//#define IsCAM // Do we have camera stabilization in use, If you activate, check OUTPUT pins from ArduUser.h //#define IsCAM // Do we have camera stabilization in use, If you activate, check OUTPUT pins from ArduUser.h
//#define UseAirspeed // Quads don't use AirSpeed... Legacy, jp 19-10-10 //#define UseAirspeed // Quads don't use AirSpeed... Legacy, jp 19-10-10
//#define UseBMP // Use pressure sensor #define UseBMP // Use pressure sensor
//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!) //#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!)
#define CONFIGURATOR #define CONFIGURATOR
@ -314,6 +314,15 @@ void loop()
} }
} }
#endif #endif
#ifdef UseBMP
if (Baro_new_data) // New altitude data?
{
ch_throttle_altitude_hold = Altitude_control_baro(press_alt,target_baro_altitude); // Altitude control
Baro_new_data=0;
Serial.println(ch_throttle_altitude_hold);
}
ch_throttle = ch_throttle_altitude_hold;
#endif
} }
else // First time we enter in GPS position hold we capture the target position as the actual position else // First time we enter in GPS position hold we capture the target position as the actual position
{ {
@ -322,14 +331,14 @@ void loop()
target_lattitude = GPS.Lattitude; target_lattitude = GPS.Lattitude;
target_longitude = GPS.Longitude; target_longitude = GPS.Longitude;
target_position=1; target_position=1;
//target_sonar_altitude = sonar_value;
target_baro_altitude = press_alt;
Initial_Throttle = ch_throttle;
Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde)
} }
#endif #endif
command_gps_roll=0; command_gps_roll=0;
command_gps_pitch=0; command_gps_pitch=0;
target_baro_altitude = press_alt;
Initial_Throttle = ch_throttle;
ch_throttle_altitude_hold = ch_throttle;
Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde)
} }
} }
else else
@ -384,6 +393,8 @@ void loop()
if (APM_BMP085.Read()){ if (APM_BMP085.Read()){
read_baro(); read_baro();
Baro_new_data = 1; Baro_new_data = 1;
//Serial.print("B ");
//Serial.println(press_alt);
} }
#endif #endif
break; break;

10
ArducopterNG/Navigation.pde
View File

@ -106,11 +106,11 @@ void Reset_I_terms_navigation()
/* Altitude control... (based on barometer) */ /* Altitude control... (based on barometer) */
int Altitude_control_baro(int altitude, int target_altitude) int Altitude_control_baro(int altitude, int target_altitude)
{ {
#define ALTITUDE_CONTROL_BARO_OUTPUT_MIN 50 #define ALTITUDE_CONTROL_BARO_OUTPUT_MIN 40
#define ALTITUDE_CONTROL_BARO_OUTPUT_MAX 90 #define ALTITUDE_CONTROL_BARO_OUTPUT_MAX 80
#define KP_BARO_ALTITUDE 0.65 #define KP_BARO_ALTITUDE 0.25 //0.3
#define KD_BARO_ALTITUDE 0.05 #define KD_BARO_ALTITUDE 0.09 //0.09
#define KI_BARO_ALTITUDE 0.1 #define KI_BARO_ALTITUDE 0.1
int command_altitude; int command_altitude;
@ -119,7 +119,7 @@ int Altitude_control_baro(int altitude, int target_altitude)
err_altitude = target_altitude - altitude; err_altitude = target_altitude - altitude;
altitude_D = (float)(err_altitude-err_altitude_old)/0.05; // 20Hz altitude_D = (float)(err_altitude-err_altitude_old)/0.05; // 20Hz
altitude_I += (float)err_altitude*0.05; altitude_I += (float)err_altitude*0.05;
altitude_I = constrain(altitude_I,-100,100); altitude_I = constrain(altitude_I,-120,120);
command_altitude = KP_BARO_ALTITUDE*err_altitude + KD_BARO_ALTITUDE*altitude_D + KI_BARO_ALTITUDE*altitude_I; command_altitude = KP_BARO_ALTITUDE*err_altitude + KD_BARO_ALTITUDE*altitude_D + KI_BARO_ALTITUDE*altitude_I;
command_altitude = Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_BARO_OUTPUT_MIN,ALTITUDE_CONTROL_BARO_OUTPUT_MAX); command_altitude = Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_BARO_OUTPUT_MIN,ALTITUDE_CONTROL_BARO_OUTPUT_MAX);
return command_altitude; return command_altitude;

4
ArducopterNG/Sensors.pde
View File

@ -96,7 +96,7 @@ void read_baro(void)
if (press_alt == 0) if (press_alt == 0)
press_alt = (1.0 - tempPresAlt) * 4433000; // Altitude in cm press_alt = (1.0 - tempPresAlt) * 4433000; // Altitude in cm
else else
press_alt = press_alt * 0.9 + ((1.0 - tempPresAlt) * 443300); // Altitude in cm (filtered) press_alt = press_alt * 0.75 + ((1.0 - tempPresAlt) * 4433000)*0.25; // Altitude in cm (filtered)
} }
#endif #endif