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

added new PID nav functions to split lat and long based on Randy's work.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1486 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-01-11 21:14:57 +00:00
parent 754c7bc710
commit 40b4c0d8ac
1 changed files with 30 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
ArduCopterMega/ArduCopterMega.pde
View File

@ -159,9 +159,10 @@ int max_yaw_dampener;
boolean rate_yaw_flag; boolean rate_yaw_flag;
// Nav // Nav
PID pid_nav (EE_GAIN_7); PID pid_nav_lat (EE_GAIN_7);
PID pid_baro_throttle (EE_GAIN_8); PID pid_nav_lon (EE_GAIN_8);
PID pid_sonar_throttle (EE_GAIN_9); PID pid_baro_throttle (EE_GAIN_9);
PID pid_sonar_throttle (EE_GAIN_10);
// GPS variables // GPS variables
// ------------- // -------------
@ -278,6 +279,8 @@ int loiter_time_max; // millis : how long to stay in LOITER mode
long nav_roll; // deg * 100 : target roll angle long nav_roll; // deg * 100 : target roll angle
long nav_pitch; // deg * 100 : target pitch angle long nav_pitch; // deg * 100 : target pitch angle
long nav_yaw; // deg * 100 : target yaw angle long nav_yaw; // deg * 100 : target yaw angle
long nav_lat; // for error calcs
long nav_lon; // for error calcs
int nav_throttle; // 0-1000 for throttle control int nav_throttle; // 0-1000 for throttle control
long command_yaw_start; // what angle were we to begin with long command_yaw_start; // what angle were we to begin with
@ -508,13 +511,15 @@ void medium_loop()
medium_loopCounter++; medium_loopCounter++;
if(GPS.new_data){ if(GPS.new_data){
GPS.new_data = false;
dTnav = millis() - medium_loopTimer; dTnav = millis() - medium_loopTimer;
medium_loopTimer = millis(); medium_loopTimer = millis();
}
// calculate the plane's desired bearing // calculate the plane's desired bearing
// ------------------------------------- // -------------------------------------
navigate(); navigate();
}
break; break;
// command processing // command processing
@ -608,10 +613,6 @@ void medium_loop()
flight_lights(); flight_lights();
#endif #endif
#if ENABLE_xx
do_something_usefull();
#endif
if (millis() - perf_mon_timer > 20000) { if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) { if (mainLoop_count != 0) {
@ -738,9 +739,9 @@ void update_current_flight_mode(void)
default: default:
// Intput Pitch, Roll, Yaw and Throttle // Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------ // ------------------------------------
calc_nav_pid(); //calc_nav_pid();
calc_nav_roll(); //calc_nav_roll();
calc_nav_pitch(); //calc_nav_pitch();
// based on altitude error // based on altitude error
// ----------------------- // -----------------------
@ -783,6 +784,7 @@ void update_current_flight_mode(void)
break; break;
case FBW: case FBW:
// we are currently using manual throttle during alpha testing. // we are currently using manual throttle during alpha testing.
fbw_timer++; fbw_timer++;
//call at 5 hz //call at 5 hz
@ -791,32 +793,18 @@ void update_current_flight_mode(void)
if(home_is_set == false){ if(home_is_set == false){
// we are not using GPS // we are not using GPS
// reset the location, RTL won't function // reset the location
// RTL won't function
current_loc.lat = home.lat = 0; current_loc.lat = home.lat = 0;
current_loc.lng = home.lng = 0; current_loc.lng = home.lng = 0;
// set dTnav manually
dTnav = 200;
} }
next_WP.lat = home.lat + rc_1.control_in /5; // 10 meteres next_WP.lat = home.lat + rc_1.control_in / 5; // 10 meteres
next_WP.lng = home.lng -rc_2.control_in /5; // 10 meteres next_WP.lng = home.lng - rc_2.control_in / 5; // 10 meteres
// waypoint distance from plane
// ----------------------------
wp_distance = GPS_wp_distance = getDistance(&current_loc, &next_WP);
// target_bearing is where we should be heading
// --------------------------------------------
nav_bearing = target_bearing = get_bearing(&current_loc, &next_WP);
// not really needed
//update_navigation();
} }
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
calc_nav_pid();
calc_nav_roll();
calc_nav_pitch();
// Yaw control // Yaw control
// ----------- // -----------
output_manual_yaw(); output_manual_yaw();
@ -827,7 +815,6 @@ void update_current_flight_mode(void)
// apply throttle control // apply throttle control
output_manual_throttle(); output_manual_throttle();
// perform stabilzation // perform stabilzation
output_stabilize(); output_stabilize();
break; break;
@ -850,7 +837,6 @@ void update_current_flight_mode(void)
// ----------------------- // -----------------------
calc_nav_throttle(); calc_nav_throttle();
// Output Pitch, Roll, Yaw and Throttle // Output Pitch, Roll, Yaw and Throttle
// ------------------------------------ // ------------------------------------
// apply throttle control // apply throttle control
@ -863,9 +849,9 @@ void update_current_flight_mode(void)
case RTL: case RTL:
// Intput Pitch, Roll, Yaw and Throttle // Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------ // ------------------------------------
calc_nav_pid(); //calc_nav_pid();
calc_nav_roll(); //calc_nav_roll();
calc_nav_pitch(); //calc_nav_pitch();
// based on altitude error // based on altitude error
// ----------------------- // -----------------------
@ -885,9 +871,9 @@ void update_current_flight_mode(void)
case POSITION_HOLD: case POSITION_HOLD:
// Intput Pitch, Roll, Yaw and Throttle // Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------ // ------------------------------------
calc_nav_pid(); //calc_nav_pid();
calc_nav_roll(); //calc_nav_roll();
calc_nav_pitch(); //calc_nav_pitch();
// Yaw control // Yaw control
// ----------- // -----------