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

View File

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