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

New Yaw controller using rate feed forward and stabilize. 

Signed-off-by: LeonardTHall <LeonardTHall@gmail.com>
This commit is contained in:
LeonardTHall 2012-10-14 23:00:16 +10:30 committed by rmackay9
parent 930d387b8d
commit d7b7e1cefa
3 changed files with 42 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduCopter/ArduCopter.pde
View File

@ -1611,28 +1611,7 @@ void update_yaw_mode(void)
#endif #endif
case YAW_HOLD: case YAW_HOLD:
if(g.rc_4.control_in != 0) { get_yaw_rate_stabilized_ef(g.rc_4.control_in);
get_stabilize_rate_yaw(g.rc_4.control_in);
yaw_stopped = false;
yaw_timer = 150;
}else if (!yaw_stopped) {
get_stabilize_rate_yaw(0);
yaw_timer--;
if((yaw_timer == 0) || (fabs(omega.z) < .17)) {
yaw_stopped = true;
nav_yaw = ahrs.yaw_sensor;
}
}else{
// reset target yaw to current yaw if the motors are disarmed or throttle is zero
// Note: we do not want to reset yaw if failsafe has been triggered even though throttle maybe zero (in fact, normally throttle is zero in failsafe)
if(motors.armed() == false || ((g.rc_3.control_in == 0) && (control_mode <= ACRO) && !failsafe))
nav_yaw = ahrs.yaw_sensor;
get_stabilize_yaw(nav_yaw);
}
return;
break; break;
case YAW_LOOK_AT_HOME: case YAW_LOOK_AT_HOME:

33
ArduCopter/Attitude.pde
View File

@ -147,6 +147,37 @@ get_acro_yaw(int32_t target_rate)
set_yaw_rate_target(target_rate, BODY_FRAME); set_yaw_rate_target(target_rate, BODY_FRAME);
} }
// Yaw with rate input and stabilized in the earth frame
static void
get_yaw_rate_stabilized_ef(int32_t stick_angle)
{
int32_t angle_error = 0;
// convert the input to the desired yaw rate
int32_t target_rate = stick_angle * g.acro_p;
// convert the input to the desired yaw rate
nav_yaw += target_rate * G_Dt;
nav_yaw = wrap_360(nav_yaw);
// angle error with maximum of +- max_angle_overshoot
angle_error = wrap_180(nav_yaw - ahrs.yaw_sensor);
// this limits the maximum overshoot
angle_error = constrain(angle_error, -MAX_ANGLE_OVERSHOOT, MAX_ANGLE_OVERSHOOT);
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !failsafe)) {
angle_error = 0;
}
// update nav_yaw to be within max_angle_overshoot of our current heading
nav_yaw = angle_error + ahrs.yaw_sensor;
// set earth frame targets for rate controller
set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME);
}
/* /*
* static int16_t * static int16_t
* get_acro_yaw2(int32_t target_rate) * get_acro_yaw2(int32_t target_rate)
@ -262,7 +293,7 @@ update_rate_contoller_targets()
yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef + sin_roll * pitch_rate_target_ef; yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef + sin_roll * pitch_rate_target_ef;
} }
} }
// run roll, pitch and yaw rate controllers and send output to motors // run roll, pitch and yaw rate controllers and send output to motors
// targets for these controllers comes from stabilize controllers // targets for these controllers comes from stabilize controllers
void void

9
ArduCopter/config.h
View File

@ -750,6 +750,15 @@
# define STABILIZE_D_SCHEDULE 0.5 # define STABILIZE_D_SCHEDULE 0.5
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// Rate controlled stabilized variables
//
#ifndef MAX_ANGLE_OVERSHOOT
#define MAX_ANGLE_OVERSHOOT 1000
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Loiter control gains // Loiter control gains
// //