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

Corrected bug on GPS position hold (added GPS_Dt for D and I terms of the control). Separated navigation.pde file. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@231 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-08-18 19:23:29 +00:00
parent c0e5fa9908
commit 13f8f8ef11
4 changed files with 91 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Arducopter/ArduCopter.h
View File

@ -145,8 +145,12 @@ float Temporary_Matrix[3][3]={
float speed_3d=0; float speed_3d=0;
int GPS_ground_speed=0; int GPS_ground_speed=0;
long timer=0; //general porpuse timer // Main timers
long timer=0;
long timer_old; long timer_old;
long GPS_timer;
long GPS_timer_old;
float GPS_Dt=0.2; // GPS Dt
// Attitude control variables // Attitude control variables
float command_rx_roll=0; // User commands float command_rx_roll=0; // User commands
@ -408,4 +412,4 @@ void readUserConfig() {
pitch_mid = readEEPROM(CHPITCH_MID); pitch_mid = readEEPROM(CHPITCH_MID);
yaw_mid = readEEPROM(CHYAW_MID); yaw_mid = readEEPROM(CHYAW_MID);
} }

61
Arducopter/Arducopter.pde
View File

@ -1,5 +1,5 @@
/* ********************************************************************** */ /* ********************************************************************** */
/* j ArduCopter Quadcopter code */ /* ArduCopter Quadcopter code */
/* */ /* */
/* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */ /* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */
/* IMU DCM code from Diydrones.com */ /* IMU DCM code from Diydrones.com */
@ -88,58 +88,6 @@
// 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 FrameConfig.h
/* ************************************************************ */
/* Altitude control... (based on sonar) */
void Altitude_control(int target_sonar_altitude)
{
err_altitude_old = err_altitude;
err_altitude = target_sonar_altitude - Sonar_value;
altitude_D = (float)(err_altitude - err_altitude_old) / G_Dt;
altitude_I += (float)err_altitude * G_Dt;
altitude_I = constrain(altitude_I, -100, 100);
command_altitude = Initial_Throttle + KP_ALTITUDE * err_altitude + KD_ALTITUDE * altitude_D + KI_ALTITUDE * altitude_I;
}
/* ************************************************************ */
/* Position control... */
void Position_control(long lat_dest, long lon_dest)
{
long Lon_diff;
long Lat_diff;
float gps_err_roll;
float gps_err_pitch;
Lon_diff = lon_dest - GPS.Longitude;
Lat_diff = lat_dest - GPS.Lattitude;
// 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_roll_D = (gps_err_roll-gps_err_roll_old)/G_Dt;
gps_roll_I += gps_err_roll*G_Dt;
gps_roll_I = constrain(gps_roll_I,-500,500);
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_pitch_D = (gps_err_pitch-gps_err_pitch_old)/G_Dt;
gps_pitch_I += gps_err_pitch*G_Dt;
gps_pitch_I = constrain(gps_pitch_I,-500,500);
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
}
/* ************************************************************ */ /* ************************************************************ */
// STABLE MODE // STABLE MODE
// ROLL, PITCH and YAW PID controls... // ROLL, PITCH and YAW PID controls...
@ -448,7 +396,7 @@ void loop(){
counter++; counter++;
timer_old = timer; timer_old = timer;
timer=millis(); timer=millis();
G_Dt = (timer-timer_old)/1000.0; // Real time of loop run G_Dt = (timer-timer_old)*0.001; // Real time of loop run
// IMU DCM Algorithm // IMU DCM Algorithm
Read_adc_raw(); Read_adc_raw();
@ -574,6 +522,9 @@ void loop(){
GPS.Read(); GPS.Read();
if (GPS.NewData) // New GPS data? if (GPS.NewData) // New GPS data?
{ {
GPS_timer_old=GPS_timer; // Update GPS timer
GPS_timer = timer;
GPS_Dt = (GPS_timer-GPS_timer_old)*0.001; // GPS_Dt
GPS.NewData=0; // We Reset the flag... GPS.NewData=0; // We Reset the flag...
//Output GPS data //Output GPS data
@ -727,4 +678,4 @@ void loop(){
} }

71
Arducopter/Navigation.pde Normal file
View File

@ -0,0 +1,71 @@
/*
ArduCopter v1.3 - Aug 2010
www.ArduCopter.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* ************************************************************ */
/* GPS based Position control */
void Position_control(long lat_dest, long lon_dest)
{
long Lon_diff;
long Lat_diff;
float gps_err_roll;
float gps_err_pitch;
Lon_diff = lon_dest - GPS.Longitude;
Lat_diff = lat_dest - GPS.Lattitude;
// 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_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);
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_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);
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
}
/* ************************************************************ */
/* Altitude control... (based on sonar) // NOT TESTED!!*/
void Altitude_control(int target_sonar_altitude)
{
err_altitude_old = err_altitude;
err_altitude = target_sonar_altitude - Sonar_value;
altitude_D = (float)(err_altitude - err_altitude_old) / G_Dt;
altitude_I += (float)err_altitude * G_Dt;
altitude_I = constrain(altitude_I, -100, 100);
command_altitude = Initial_Throttle + KP_ALTITUDE * err_altitude + KD_ALTITUDE * altitude_D + KI_ALTITUDE * altitude_I;
}

16
Arducopter/SerialCom.pde
View File

@ -144,13 +144,13 @@ void readSerialCommand() {
KD_QUAD_YAW = 1.2; KD_QUAD_YAW = 1.2;
KI_QUAD_YAW = 0.15; KI_QUAD_YAW = 0.15;
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; KD_GPS_ROLL = 0.006;
KI_GPS_ROLL = 0.004; KI_GPS_ROLL = 0.008;
KP_GPS_PITCH = 0.012; KP_GPS_PITCH = 0.02;
KD_GPS_PITCH = 0.005; KD_GPS_PITCH = 0.006;
KI_GPS_PITCH = 0.004; KI_GPS_PITCH = 0.008;
GPS_MAX_ANGLE = 10; GPS_MAX_ANGLE = 18;
KP_ALTITUDE = 0.8; KP_ALTITUDE = 0.8;
KD_ALTITUDE = 0.2; KD_ALTITUDE = 0.2;
KI_ALTITUDE = 0.2; KI_ALTITUDE = 0.2;
@ -441,4 +441,4 @@ void comma() {
Serial.print(','); Serial.print(',');
} }
*/ */