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

Moved implementation of alt hold from NG to ACM. 

Tweaked the nav pitch and roll commands to no longer use bearing error.
Angle boost is now a lot less thrust. I was getting too much added thrust when pitched at 45°

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1475 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-01-10 02:24:26 +00:00
parent 1a9a76c2ab
commit bb7e3af943
1 changed files with 38 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
ArduCopterMega/flight_control.pde
View File

@ -21,18 +21,34 @@ void output_auto_throttle()
void calc_nav_throttle()
{
long err = constrain (altitude_error, -150, 150); //+-1.5 meters
long temp = pid_throttle.get_pid(err, deltaMiliSeconds, 1.0);
nav_throttle = (float)(throttle_cruise + temp) * angle_boost();
long t_out;
if(altitude_sensor == BARO) {
t_out = pid_baro_throttle.get_pid(err, deltaMiliSeconds, 1.0);
// limit output of throttle control
t_out = throttle_cruise + constrain(t_out, -50, 100);
} else {
// SONAR
t_out = pid_sonar_throttle.get_pid(err, deltaMiliSeconds, 1.0);
// limit output of throttle control
t_out = throttle_cruise + constrain(t_out, -60, 100);
}
nav_throttle = (float)(throttle_cruise + t_out) * angle_boost();
}
float angle_boost()
{
//static byte flipper;
float temp = 1 / (cos(dcm.roll) * cos(dcm.pitch));
//temp *= .5;
if(temp > 1.2)
temp = 1.2;
//float temp = 1 / (cos(dcm.roll) * cos(dcm.pitch));
float temp = (1.0 - (cos(dcm.roll) * cos(dcm.pitch)));
temp = 1.0 + (temp / 1.5);
// limit the boost value
if(temp > 1.3)
temp = 1.3;
return temp;
}
@ -69,6 +85,8 @@ picth and roll control
// -----------------------------------
void calc_nav_pid()
{
// how hard to pitch to target
nav_angle = pid_nav.get_pid(wp_distance * 100, dTnav, 1.0);
nav_angle = constrain(nav_angle, -pitch_max, pitch_max);
}
@ -77,16 +95,19 @@ void calc_nav_pid()
// ---------------------------------------------------
void calc_nav_pitch()
{
long b_err = bearing_error;
// how hard to pitch to target
long angle = wrap_360(nav_bearing - yaw_sensor);
bool rev = false;
float roll_out;
if(b_err > 18000){
b_err -= 18000;
if(angle > 18000){
angle -= 18000;
rev = true;
}
roll_out = abs(b_err - 18000);
roll_out = abs(angle - 18000);
roll_out = (9000.0 - roll_out) / 9000.0;
roll_out = (rev) ? roll_out : -roll_out;
@ -97,19 +118,21 @@ void calc_nav_pitch()
// --------------------------------------------------
void calc_nav_roll()
{
long b_err = bearing_error;
long angle = wrap_360(nav_bearing - yaw_sensor);
bool rev = false;
float roll_out;
if(b_err > 18000){
b_err -= 18000;
if(angle > 18000){
angle -= 18000;
rev = true;
}
roll_out = abs(b_err - 9000);
roll_out = abs(angle - 9000);
roll_out = (9000.0 - roll_out) / 9000.0;
roll_out = (rev) ? -roll_out : roll_out;
nav_roll = (float)nav_angle * roll_out;
}