diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 9ea10e9c09..e945dd9737 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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 ) { diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index ec546eac4b..0b2916099b 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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); } } diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index b4b35ac486..45bd044c48 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -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)); } diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index c738053f02..0b5ccd6c74 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -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 } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 72c8edc872..df92272ea7 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 9978dcd670..e572444987 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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: