Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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

4
ArducopterNG/Arducopter.h
View File

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

2
ArducopterNG/ArducopterNG.pde
View File

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

9
ArducopterNG/Navigation.pde
View File

@ -58,12 +58,15 @@ void Position_control(long lat_dest, long lon_dest)
long Lon_diff; long Lon_diff;
long Lat_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; Lon_diff = lon_dest - GPS.Longitude;
Lat_diff = lat_dest - GPS.Lattitude; 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 // 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_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;