mirror of https://github.com/ArduPilot/ardupilot
Yaw updates. 2.0.13
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2403 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
1ff40b053a
commit
8088f24427
|
@ -518,6 +518,11 @@ void fast_loop()
|
||||||
if (delta_ms_fast_loop > G_Dt_max)
|
if (delta_ms_fast_loop > G_Dt_max)
|
||||||
G_Dt_max = delta_ms_fast_loop;
|
G_Dt_max = delta_ms_fast_loop;
|
||||||
|
|
||||||
|
// Read radio
|
||||||
|
// ----------
|
||||||
|
if (APM_RC.GetState() == 1)
|
||||||
|
read_radio(); // read the radio first
|
||||||
|
|
||||||
// custom code/exceptions for flight modes
|
// custom code/exceptions for flight modes
|
||||||
// ---------------------------------------
|
// ---------------------------------------
|
||||||
update_current_flight_mode();
|
update_current_flight_mode();
|
||||||
|
@ -538,9 +543,6 @@ void fast_loop()
|
||||||
|
|
||||||
void medium_loop()
|
void medium_loop()
|
||||||
{
|
{
|
||||||
// Read radio
|
|
||||||
// ----------
|
|
||||||
read_radio(); // read the radio first
|
|
||||||
|
|
||||||
// reads all of the necessary trig functions for cameras, throttle, etc.
|
// reads all of the necessary trig functions for cameras, throttle, etc.
|
||||||
update_trig();
|
update_trig();
|
||||||
|
@ -984,6 +986,10 @@ void update_current_flight_mode(void)
|
||||||
|
|
||||||
// Output Pitch, Roll, Yaw and Throttle
|
// Output Pitch, Roll, Yaw and Throttle
|
||||||
// ------------------------------------
|
// ------------------------------------
|
||||||
|
// are we at rest? reset nav_yaw
|
||||||
|
if(g.rc_3.control_in == 0){
|
||||||
|
nav_yaw = dcm.yaw_sensor;
|
||||||
|
}
|
||||||
|
|
||||||
// Yaw control
|
// Yaw control
|
||||||
output_manual_yaw();
|
output_manual_yaw();
|
||||||
|
@ -1031,6 +1037,11 @@ void update_current_flight_mode(void)
|
||||||
limit_nav_pitch_roll(4500);
|
limit_nav_pitch_roll(4500);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// are we at rest? reset nav_yaw
|
||||||
|
if(g.rc_3.control_in == 0){
|
||||||
|
nav_yaw = dcm.yaw_sensor;
|
||||||
|
}
|
||||||
|
|
||||||
// Output Pitch, Roll, Yaw and Throttle
|
// Output Pitch, Roll, Yaw and Throttle
|
||||||
// ------------------------------------
|
// ------------------------------------
|
||||||
// Yaw control
|
// Yaw control
|
||||||
|
@ -1052,6 +1063,7 @@ void update_current_flight_mode(void)
|
||||||
nav_pitch = 0;
|
nav_pitch = 0;
|
||||||
nav_roll = 0;
|
nav_roll = 0;
|
||||||
|
|
||||||
|
// allow interactive changing of atitude
|
||||||
adjust_altitude();
|
adjust_altitude();
|
||||||
|
|
||||||
// Yaw control
|
// Yaw control
|
||||||
|
@ -1095,7 +1107,7 @@ void update_current_flight_mode(void)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case LOITER:
|
case LOITER:
|
||||||
|
// allow interactive changing of atitude
|
||||||
adjust_altitude();
|
adjust_altitude();
|
||||||
|
|
||||||
#if AUTO_RESET_LOITER == 1
|
#if AUTO_RESET_LOITER == 1
|
||||||
|
|
|
@ -243,7 +243,7 @@ output_yaw_with_hold(boolean hold)
|
||||||
yaw_error = constrain(yaw_error, -4000, 4000); // limit error to 60 degees
|
yaw_error = constrain(yaw_error, -4000, 4000); // limit error to 60 degees
|
||||||
|
|
||||||
// Apply PID and save the new angle back to RC_Channel
|
// Apply PID and save the new angle back to RC_Channel
|
||||||
g.rc_4.servo_out = g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
|
g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
|
||||||
|
|
||||||
// add in yaw dampener
|
// add in yaw dampener
|
||||||
g.rc_4.servo_out -= constrain(dampener, -1600, 1600);
|
g.rc_4.servo_out -= constrain(dampener, -1600, 1600);
|
||||||
|
@ -277,18 +277,14 @@ output_yaw_with_hold(boolean hold)
|
||||||
void
|
void
|
||||||
output_yaw_with_hold(boolean hold)
|
output_yaw_with_hold(boolean hold)
|
||||||
{
|
{
|
||||||
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
|
|
||||||
rate = constrain(rate, -36000, 36000); // limit to something fun!
|
|
||||||
int dampener = rate * g.pid_yaw.kD(); // 34377 * .175 = 6000
|
|
||||||
|
|
||||||
// re-define nav_yaw if we have stick input
|
// re-define nav_yaw if we have stick input
|
||||||
if(g.rc_4.control_in != 0){
|
if(g.rc_4.control_in != 0){
|
||||||
// set nav_yaw + or - the current location
|
// set nav_yaw + or - the current location
|
||||||
nav_yaw = (long)g.rc_4.control_in + dcm.yaw_sensor;
|
nav_yaw = (long)g.rc_4.control_in + dcm.yaw_sensor;
|
||||||
|
}
|
||||||
|
|
||||||
// we need to wrap our value so we can be 0 to 360 (*100)
|
// we need to wrap our value so we can be 0 to 360 (*100)
|
||||||
nav_yaw = wrap_360(nav_yaw);
|
nav_yaw = wrap_360(nav_yaw);
|
||||||
}
|
|
||||||
|
|
||||||
// how far off is nav_yaw from our current yaw?
|
// how far off is nav_yaw from our current yaw?
|
||||||
yaw_error = nav_yaw - dcm.yaw_sensor;
|
yaw_error = nav_yaw - dcm.yaw_sensor;
|
||||||
|
@ -297,13 +293,13 @@ output_yaw_with_hold(boolean hold)
|
||||||
yaw_error = wrap_180(yaw_error);
|
yaw_error = wrap_180(yaw_error);
|
||||||
|
|
||||||
// limit the error we're feeding to the PID
|
// limit the error we're feeding to the PID
|
||||||
yaw_error = constrain(yaw_error, -4000, 4000); // limit error to 60 degees
|
yaw_error = constrain(yaw_error, -3500, 3500); // limit error to 60 degees
|
||||||
|
|
||||||
// Apply PID and save the new angle back to RC_Channel
|
// Apply PID and save the new angle back to RC_Channel
|
||||||
g.rc_4.servo_out = g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
|
g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
|
||||||
|
|
||||||
// add in yaw dampener
|
// add in yaw dampener
|
||||||
g.rc_4.servo_out -= constrain(dampener, -1600, 1600);
|
g.rc_4.servo_out -= degrees(omega.z) * 100 * g.pid_yaw.kD();
|
||||||
yaw_debug = YAW_HOLD; //0
|
yaw_error = constrain(yaw_error, -2500, 2500); // limit error to 60 degees
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -357,13 +357,13 @@
|
||||||
# define YAW_P 0.4 // increase for more aggressive Yaw Hold, decrease if it's bouncy
|
# define YAW_P 0.4 // increase for more aggressive Yaw Hold, decrease if it's bouncy
|
||||||
#endif
|
#endif
|
||||||
#ifndef YAW_I
|
#ifndef YAW_I
|
||||||
# define YAW_I 0.0001 // set to .0001 to try and get over user's steady state error caused by poor balance
|
# define YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance
|
||||||
#endif
|
#endif
|
||||||
#ifndef YAW_D
|
#ifndef YAW_D
|
||||||
# define YAW_D 0.05 // Trying a lower value to prevent odd behavior
|
# define YAW_D 0.05 // Trying a lower value to prevent odd behavior
|
||||||
#endif
|
#endif
|
||||||
#ifndef YAW_IMAX
|
#ifndef YAW_IMAX
|
||||||
# define YAW_IMAX .5 // degrees * 100
|
# define YAW_IMAX 1 // degrees * 100
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
@ -427,7 +427,7 @@
|
||||||
# define THROTTLE_BARO_P 0.25
|
# define THROTTLE_BARO_P 0.25
|
||||||
#endif
|
#endif
|
||||||
#ifndef THROTTLE_BARO_I
|
#ifndef THROTTLE_BARO_I
|
||||||
# define THROTTLE_BARO_I 0.0045
|
# define THROTTLE_BARO_I 0.01 //with 4m error, 12.5s windup
|
||||||
#endif
|
#endif
|
||||||
#ifndef THROTTLE_BARO_D
|
#ifndef THROTTLE_BARO_D
|
||||||
# define THROTTLE_BARO_D 0.03
|
# define THROTTLE_BARO_D 0.03
|
||||||
|
|
|
@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
|
||||||
};
|
};
|
||||||
|
|
||||||
// Create the top-level menu object.
|
// Create the top-level menu object.
|
||||||
MENU(main_menu, "AC 2.0.12 Beta", main_menu_commands);
|
MENU(main_menu, "AC 2.0.13 Beta", main_menu_commands);
|
||||||
|
|
||||||
void init_ardupilot()
|
void init_ardupilot()
|
||||||
{
|
{
|
||||||
|
|
Loading…
Reference in New Issue