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

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1266 f9c3cf11-9bcb-44bc-f272-b75c42450872

This commit is contained in:
jasonshort 2010-12-26 05:18:25 +00:00
parent e93daf2546
commit 1fc9aa189e
1 changed files with 30 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ArduCopterMega/flight_control.pde
View File

@ -8,29 +8,32 @@ throttle control
void output_manual_throttle() void output_manual_throttle()
{ {
rc_3.servo_out = rc_3.control_in; rc_3.servo_out = rc_3.control_in;
//rc_3.servo_out = (float)rc_3.servo_out * throttle_boost(); rc_3.servo_out = (float)rc_3.servo_out * angle_boost();
} }
// Autopilot // Autopilot
// --------- // ---------
void output_auto_throttle() void output_auto_throttle()
{ {
rc_3.servo_out = (float)nav_throttle * throttle_boost(); rc_3.servo_out = (float)nav_throttle * angle_boost();
rc_3.servo_out = max(rc_3.servo_out, 1);
} }
// Jason
void calc_nav_throttle() void calc_nav_throttle()
{ {
long err = constrain (altitude_error, -300, 300); //+-5 meters long err = constrain (altitude_error, -300, 300); //+-3 meters
long temp = pid_throttle.get_pid(err, deltaMiliSeconds, 1.0); long temp = pid_throttle.get_pid(err, deltaMiliSeconds, 1.0);
nav_throttle = (float)(throttle_cruise + temp) * throttle_boost(); nav_throttle = (float)(throttle_cruise + temp) * angle_boost();
} }
float throttle_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));
if(temp > 1.25) temp *= .5;
temp = 1.25; if(temp > 1.2)
temp = 1.2;
return temp; return temp;
} }
@ -42,19 +45,38 @@ float throttle_boost()
yaw control yaw control
****************************************************************/ ****************************************************************/
void input_yaw_hold() void input_yaw_hold_2()
{ {
if(rc_3.control_in == 0){ if(rc_3.control_in == 0){
// Reset the yaw hold
nav_yaw = yaw_sensor; nav_yaw = yaw_sensor;
}else if(rc_4.control_in == 0){ }else if(rc_4.control_in == 0){
// do nothing // do nothing
}else{ }else{
// create up to 60° of yaw error
nav_yaw = yaw_sensor + rc_4.control_in; nav_yaw = yaw_sensor + rc_4.control_in;
nav_yaw = wrap_360(nav_yaw); nav_yaw = wrap_360(nav_yaw);
} }
} }
void input_yaw_hold()
{
if(rc_3.control_in == 0){
// Reset the yaw hold
nav_yaw = yaw_sensor;
}else if(rc_4.control_in == 0){
// do nothing
}else{
// create up to 60° of yaw error
nav_yaw += ((long)rc_4.control_in * (long)deltaMiliSeconds) / 500; // we'll get 60° * 2 or 120° per second
nav_yaw = wrap_360(nav_yaw);
}
}
/*void output_yaw_stabilize() /*void output_yaw_stabilize()
{ {
rc_4.servo_out = rc_4.control_in; rc_4.servo_out = rc_4.control_in;