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

Copter: rename nav_yaw to control_yaw

This commit is contained in:
Randy Mackay 2013-11-27 22:48:20 +09:00
parent f9e19b8006
commit 7ef04bb73b
6 changed files with 42 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduCopter/ArduCopter.pde
View File

@ -537,8 +537,8 @@ static uint8_t command_cond_index;
// NAV_LOCATION - have we reached the desired location?
// NAV_DELAY - have we waited at the waypoint the desired time?
static float lon_error, lat_error; // Used to report how many cm we are from the next waypoint or loiter target position
static int16_t control_roll;
static int16_t control_pitch;
static int16_t control_roll; // desired roll angle of copter in centi-degrees
static int16_t control_pitch; // desired pitch angle of copter in centi-degrees
static uint8_t rtl_state; // records state of rtl (initial climb, returning home, etc)
static uint8_t land_state; // records state of land (flying to location, descending)
@ -726,7 +726,7 @@ static uint32_t throttle_integrator;
// Navigation Yaw control
////////////////////////////////////////////////////////////////////////////////
// The Commanded Yaw from the autopilot.
static int32_t nav_yaw;
static int32_t control_yaw;
// Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION
static Vector3f yaw_look_at_WP;
// bearing from current location to the yaw_look_at_WP
@ -1407,7 +1407,7 @@ bool set_yaw_mode(uint8_t new_yaw_mode)
yaw_initialised = true;
break;
case YAW_RESETTOARMEDYAW:
nav_yaw = ahrs.yaw_sensor; // store current yaw so we can start rotating back to correct one
control_yaw = ahrs.yaw_sensor; // store current yaw so we can start rotating back to correct one
yaw_initialised = true;
break;
}
@ -1430,9 +1430,9 @@ void update_yaw_mode(void)
case YAW_HOLD:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor;
control_yaw = ahrs.yaw_sensor;
}
// heading hold at heading held in nav_yaw but allow input from pilot
// heading hold at heading held in control_yaw but allow input from pilot
get_yaw_rate_stabilized_ef(g.rc_4.control_in);
break;
@ -1444,13 +1444,13 @@ void update_yaw_mode(void)
case YAW_LOOK_AT_NEXT_WP:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor;
control_yaw = ahrs.yaw_sensor;
}else{
// point towards next waypoint (no pilot input accepted)
// we don't use wp_bearing because we don't want the copter to turn too much during flight
nav_yaw = get_yaw_slew(nav_yaw, original_wp_bearing, AUTO_YAW_SLEW_RATE);
control_yaw = get_yaw_slew(control_yaw, original_wp_bearing, AUTO_YAW_SLEW_RATE);
}
get_stabilize_yaw(nav_yaw);
get_stabilize_yaw(control_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {
@ -1461,7 +1461,7 @@ void update_yaw_mode(void)
case YAW_LOOK_AT_LOCATION:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor;
control_yaw = ahrs.yaw_sensor;
}
// point towards a location held in yaw_look_at_WP
get_look_at_yaw();
@ -1475,7 +1475,7 @@ void update_yaw_mode(void)
case YAW_CIRCLE:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor;
control_yaw = ahrs.yaw_sensor;
}
// points toward the center of the circle or does a panorama
get_circle_yaw();
@ -1489,12 +1489,12 @@ void update_yaw_mode(void)
case YAW_LOOK_AT_HOME:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor;
control_yaw = ahrs.yaw_sensor;
}else{
// keep heading always pointing at home with no pilot input allowed
nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE);
control_yaw = get_yaw_slew(control_yaw, home_bearing, AUTO_YAW_SLEW_RATE);
}
get_stabilize_yaw(nav_yaw);
get_stabilize_yaw(control_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {
@ -1505,18 +1505,18 @@ void update_yaw_mode(void)
case YAW_LOOK_AT_HEADING:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor;
control_yaw = ahrs.yaw_sensor;
}else{
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_heading, yaw_look_at_heading_slew);
control_yaw = get_yaw_slew(control_yaw, yaw_look_at_heading, yaw_look_at_heading_slew);
}
get_stabilize_yaw(nav_yaw);
get_stabilize_yaw(control_yaw);
break;
case YAW_LOOK_AHEAD:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor;
control_yaw = ahrs.yaw_sensor;
}
// Commanded Yaw to automatically look ahead.
get_look_ahead_yaw(g.rc_4.control_in);
@ -1525,7 +1525,7 @@ void update_yaw_mode(void)
case YAW_DRIFT:
// if we have landed reset yaw target to current heading
if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor;
control_yaw = ahrs.yaw_sensor;
}
get_yaw_drift();
break;
@ -1533,12 +1533,12 @@ void update_yaw_mode(void)
case YAW_RESETTOARMEDYAW:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor;
control_yaw = ahrs.yaw_sensor;
}else{
// changes yaw to be same as when quad was armed
nav_yaw = get_yaw_slew(nav_yaw, initial_armed_bearing, AUTO_YAW_SLEW_RATE);
control_yaw = get_yaw_slew(control_yaw, initial_armed_bearing, AUTO_YAW_SLEW_RATE);
}
get_stabilize_yaw(nav_yaw);
get_stabilize_yaw(control_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {

30
ArduCopter/Attitude.pde
View File

@ -381,11 +381,11 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
int32_t target_rate = stick_angle * g.acro_yaw_p;
// convert the input to the desired yaw rate
nav_yaw += target_rate * G_Dt;
nav_yaw = wrap_360_cd(nav_yaw);
control_yaw += target_rate * G_Dt;
control_yaw = wrap_360_cd(control_yaw);
// calculate difference between desired heading and current heading
angle_error = wrap_180_cd(nav_yaw - ahrs.yaw_sensor);
angle_error = wrap_180_cd(control_yaw - ahrs.yaw_sensor);
// limit the maximum overshoot
angle_error = constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
@ -401,8 +401,8 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
}
#endif // HELI_FRAME
// update nav_yaw to be within max_angle_overshoot of our current heading
nav_yaw = wrap_360_cd(angle_error + ahrs.yaw_sensor);
// update control_yaw to be within max_angle_overshoot of our current heading
control_yaw = wrap_360_cd(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);
@ -732,7 +732,7 @@ static void get_circle_yaw()
// if circle radius is zero do panorama
if( g.circle_radius == 0 ) {
// slew yaw towards circle angle
nav_yaw = get_yaw_slew(nav_yaw, ToDeg(circle_angle)*100, AUTO_YAW_SLEW_RATE);
control_yaw = get_yaw_slew(control_yaw, ToDeg(circle_angle)*100, AUTO_YAW_SLEW_RATE);
}else{
look_at_yaw_counter++;
if( look_at_yaw_counter >= 10 ) {
@ -740,11 +740,11 @@ static void get_circle_yaw()
yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP);
}
// slew yaw
nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
control_yaw = get_yaw_slew(control_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
}
// call stabilize yaw controller
get_stabilize_yaw(nav_yaw);
get_stabilize_yaw(control_yaw);
}
// get_look_at_yaw - updates bearing to location held in look_at_yaw_WP and calls stabilize yaw controller
@ -760,20 +760,20 @@ static void get_look_at_yaw()
}
// slew yaw and call stabilize controller
nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(nav_yaw);
control_yaw = get_yaw_slew(control_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(control_yaw);
}
static void get_look_ahead_yaw(int16_t pilot_yaw)
{
// Commanded Yaw to automatically look ahead.
if (g_gps->fix && g_gps->ground_speed_cm > YAW_LOOK_AHEAD_MIN_SPEED) {
nav_yaw = get_yaw_slew(nav_yaw, g_gps->ground_course_cd, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(wrap_360_cd(nav_yaw + pilot_yaw)); // Allow pilot to "skid" around corners up to 45 degrees
control_yaw = get_yaw_slew(control_yaw, g_gps->ground_course_cd, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(wrap_360_cd(control_yaw + pilot_yaw)); // Allow pilot to "skid" around corners up to 45 degrees
}else{
nav_yaw += pilot_yaw * g.acro_yaw_p * G_Dt;
nav_yaw = wrap_360_cd(nav_yaw);
get_stabilize_yaw(nav_yaw);
control_yaw += pilot_yaw * g.acro_yaw_p * G_Dt;
control_yaw = wrap_360_cd(control_yaw);
get_stabilize_yaw(control_yaw);
}
}

2
ArduCopter/Log.pde
View File

@ -450,7 +450,7 @@ static void Log_Write_Attitude()
pitch : (int16_t)ahrs.pitch_sensor,
yaw_in : (int16_t)g.rc_4.control_in,
yaw : (uint16_t)ahrs.yaw_sensor,
nav_yaw : (uint16_t)nav_yaw
nav_yaw : (uint16_t)control_yaw
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}

4
ArduCopter/commands_logic.pde
View File

@ -756,7 +756,7 @@ static void do_yaw()
yaw_look_at_heading = wrap_360_cd(command_cond_queue.alt * 100);
}else{
// relative angle
yaw_look_at_heading = wrap_360_cd(nav_yaw + command_cond_queue.alt * 100);
yaw_look_at_heading = wrap_360_cd(control_yaw + command_cond_queue.alt * 100);
}
// get turn speed
@ -764,7 +764,7 @@ static void do_yaw()
// default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
}else{
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - nav_yaw) / 100) / command_cond_queue.lat;
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - control_yaw) / 100) / command_cond_queue.lat;
yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec
}

2
ArduCopter/defines.h
View File

@ -15,7 +15,7 @@
// Flight modes
// ------------
#define YAW_HOLD 0 // heading hold at heading in nav_yaw but allow input from pilot
#define YAW_HOLD 0 // heading hold at heading in control_yaw but allow input from pilot
#define YAW_ACRO 1 // pilot controlled yaw using rate controller
#define YAW_LOOK_AT_NEXT_WP 2 // point towards next waypoint (no pilot input accepted)
#define YAW_LOOK_AT_LOCATION 3 // point towards a location held in yaw_look_at_WP (no pilot input accepted)

2
ArduCopter/system.pde
View File

@ -463,7 +463,7 @@ static bool set_mode(uint8_t mode)
// reset acro angle targets to current attitude
acro_roll = ahrs.roll_sensor;
acro_pitch = ahrs.pitch_sensor;
nav_yaw = ahrs.yaw_sensor;
control_yaw = ahrs.yaw_sensor;
break;
default: