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

Geographic_correction_factor is now internally calculated (no need for external parameter) 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@822 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-11-10 21:38:56 +00:00
parent b839951ce5
commit 9e6fcded41
3 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArducopterNG/Arducopter.h
View File

@ -484,7 +484,7 @@ float Kp_ROLLPITCH;
float Ki_ROLLPITCH;
float Kp_YAW;
float Ki_YAW;
float GEOG_CORRECTION_FACTOR;
float GEOG_CORRECTION_FACTOR=0;
int MAGNETOMETER;
float Kp_RateRoll;
float Ki_RateRoll;
@ -550,7 +550,7 @@ void defaultUserConfig() {
Ki_ROLLPITCH = 0.00000015;
Kp_YAW = 1.0;
Ki_YAW = 0.00002;
GEOG_CORRECTION_FACTOR = 0.87;
GEOG_CORRECTION_FACTOR = 0.0; // will be automatically calculated
MAGNETOMETER = 0;
Kp_RateRoll = 1.95;
Ki_RateRoll = 0.0;
@ -656,4 +656,4 @@ void defaultUserConfig() {
// end of file
// end of file

2
ArducopterNG/ArducopterNG.pde
View File

@ -210,6 +210,8 @@ void setup() {
mediumLoop = mainLoop;
GPS_timer = mainLoop;
motorArmed = 0;
GEOG_CORRECTION_FACTOR = 0; // Geographic correction factor will be automatically calculated
Read_adc_raw(); // Initialize ADC readings...

11
ArducopterNG/Navigation.pde
View File

@ -58,12 +58,15 @@ void Position_control(long lat_dest, long lon_dest)
long Lon_diff;
long Lat_diff;
#ifdef IsGPS // it is safe to disable this, if no IsGPS we never come here anyways
Lon_diff = lon_dest - GPS.Longitude;
Lat_diff = lat_dest - GPS.Lattitude;
#endif
//If we have not calculated GEOG_CORRECTION_FACTOR we calculate it here as cos(lattitude)
if (GEOG_CORRECTION_FACTOR==0)
GEOG_CORRECTION_FACTOR = cos(ToRad(GPS.Lattitude/10000000.0));
// 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] [This simplification is valid for low roll angles]
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;
@ -99,4 +102,4 @@ void Reset_I_terms_navigation()
gps_pitch_I = 0;
}