zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ArduCopter: auto yaw changes to allow pilot override of yaw during missions 

Added set_yaw_mode to better control of yaw controller changes and variable initialisation.
Replaced AUTO_YAW mode with separate yaw controllers YAW_LOOK_AT_NEXT_WP, YAW_LOOK_AT_LOCATION, YAW_LOOK_AT_HEADING.
Pilot manual override of yaw causes yaw to change to YAW_HOLD (i.e. manual yaw) until next waypoint is reached.
Added get_yaw_slew function to control how quickly autopilot turns copter
Changed YAW_LOOK_AHEAD to use GPS heading and moved to new get_look_ahead_yaw function in Attitude.pde
Renamed variables: target_bearing->wp_bearing, original_target_bearing->original_wp_bearing.
Removed auto_yaw_tracking and auto_yaw variables and update_auto_yaw function as they are no longer needed.
Simplified MAV_CMD_CONDITION_YAW handling (do_yaw).  We lose ability to control direction of turn and ability to do long panorama shots but it now works between waypoints and save 20bytes.
master
rmackay9 12 years ago
parent
85c8bdce15
commit
9dd978576b
13 changed files with 308 additions and 282 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 243
      ArduCopter/ArduCopter.pde
  2. 20
      ArduCopter/Attitude.pde
  3. 4
      ArduCopter/GCS_Mavlink.pde
  4. 2
      ArduCopter/Log.pde
  5. 2
      ArduCopter/Parameters.pde
  6. 11
      ArduCopter/commands.pde
  7. 140
      ArduCopter/commands_logic.pde
  8. 2
      ArduCopter/commands_process.pde
  9. 28
      ArduCopter/config.h
  10. 14
      ArduCopter/defines.h
  11. 60
      ArduCopter/navigation.pde
  12. 62
      ArduCopter/system.pde
  13. 2
      ArduCopter/test.pde

243
ArduCopter/ArduCopter.pde
Unescape View File

@ -428,7 +428,6 @@ static struct AP_System{ @@ -428,7 +428,6 @@ static struct AP_System{
// updated after GPS read - 5-10hz
static int16_t lon_speed; // expressed in cm/s. positive numbers mean moving east
static int16_t lat_speed; // expressed in cm/s. positive numbers when moving north
static int16_t ground_bearing; // expressed in centidegrees
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
@ -549,7 +548,7 @@ union float_int { @@ -549,7 +548,7 @@ union float_int {
// Location & Navigation
////////////////////////////////////////////////////////////////////////////////
// This is the angle from the copter to the "next_WP" location in degrees * 100
static int32_t target_bearing;
static int32_t wp_bearing;
// Status of the Waypoint tracking mode. Options include:
// NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE
static byte wp_control;
@ -646,7 +645,7 @@ static float circle_angle; @@ -646,7 +645,7 @@ static float circle_angle;
// units are in radians, default is 5° per second
static const float circle_rate = 0.0872664625;
// used to track the delat in Circle Mode
static int32_t old_target_bearing;
static int32_t old_wp_bearing;
// deg : how many times to circle * 360 for Loiter/Circle Mission command
static int16_t loiter_total;
// deg : how far we have turned around a waypoint
@ -708,11 +707,11 @@ static int16_t saved_toy_throttle; @@ -708,11 +707,11 @@ static int16_t saved_toy_throttle;
// Each Flight mode is a unique combination of these modes
//
// The current desired control scheme for Yaw
static byte yaw_mode;
static uint8_t yaw_mode;
// The current desired control scheme for roll and pitch / navigation
static byte roll_pitch_mode;
static uint8_t roll_pitch_mode;
// The current desired control scheme for altitude hold
static byte throttle_mode;
static uint8_t throttle_mode;
////////////////////////////////////////////////////////////////////////////////
@ -727,8 +726,8 @@ static uint16_t land_detector; @@ -727,8 +726,8 @@ static uint16_t land_detector;
////////////////////////////////////////////////////////////////////////////////
// Navigation general
////////////////////////////////////////////////////////////////////////////////
// The location of the copter in relation to home, updated every GPS read
static int32_t home_to_copter_bearing;
// The location of home in relation to the copter, updated every GPS read
static int32_t home_bearing;
// distance between plane and home in cm
static int32_t home_distance;
// distance between plane and next_WP in cm
@ -760,8 +759,8 @@ static struct Location guided_WP; @@ -760,8 +759,8 @@ static struct Location guided_WP;
////////////////////////////////////////////////////////////////////////////////
// deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP
static int32_t original_target_bearing;
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int32_t original_wp_bearing;
// The amount of angle correction applied to wp_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
@ -808,28 +807,15 @@ static int8_t alt_change_flag; @@ -808,28 +807,15 @@ static int8_t alt_change_flag;
////////////////////////////////////////////////////////////////////////////////
// The Commanded Yaw from the autopilot.
static int32_t nav_yaw;
// A speed governer for Yaw control - limits the rate the quad can be turned by the autopilot
static int32_t auto_yaw;
static uint8_t yaw_timer;
// Options include: no tracking, point at next wp, or at a target
static byte auto_yaw_tracking = MAV_ROI_WPNEXT;
// In AP Mission scripting we have a fine level of control for Yaw
// This is our initial angle for relative Yaw movements
static int32_t command_yaw_start;
// Timer values used to control the speed of Yaw movements
static uint32_t command_yaw_start_time;
static uint16_t command_yaw_time; // how long we are turning
static int32_t command_yaw_end; // what angle are we trying to be
// how many degrees will we turn
static int32_t command_yaw_delta;
// Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION
static struct Location yaw_look_at_WP;
// bearing from current location to the yaw_look_at_WP
static int32_t yaw_look_at_WP_bearing;
// yaw used for YAW_LOOK_AT_HEADING yaw_mode
static int32_t yaw_look_at_heading;
// Deg/s we should turn
static int16_t command_yaw_speed;
// Direction we will turn – 1 = CW, 0 or -1 = CCW
static byte command_yaw_dir;
// Direction we will turn – 1 = relative, 0 = Absolute
static byte command_yaw_relative;
// Yaw will point at this location if auto_yaw_tracking is set to MAV_ROI_LOCATION
static struct Location target_WP;
static int16_t yaw_look_at_heading_slew;
@ -1480,55 +1466,169 @@ static void update_GPS(void) @@ -1480,55 +1466,169 @@ static void update_GPS(void)
}
}
// set_yaw_mode - update yaw mode and initialise any variables required
bool set_yaw_mode(uint8_t new_yaw_mode)
{
// boolean to ensure proper initialisation of throttle modes
bool yaw_initialised = false;
// return immediately if no change
if( new_yaw_mode == yaw_mode ) {
return true;
}
switch( new_yaw_mode ) {
case YAW_HOLD:
case YAW_ACRO:
yaw_initialised = true;
break;
case YAW_LOOK_AT_NEXT_WP:
if( ap.home_is_set ) {
yaw_initialised = true;
}
break;
case YAW_LOOK_AT_LOCATION:
if( ap.home_is_set ) {
// update bearing - assumes yaw_look_at_WP has been intialised before set_yaw_mode was called
yaw_look_at_WP_bearing = get_bearing_cd(&current_loc, &yaw_look_at_WP);
yaw_initialised = true;
}
break;
case YAW_LOOK_AT_HEADING:
yaw_initialised = true;
break;
case YAW_LOOK_AT_HOME:
if( ap.home_is_set ) {
yaw_initialised = true;
}
break;
case YAW_TOY:
yaw_initialised = true;
break;
case YAW_LOOK_AHEAD:
if( ap.home_is_set ) {
yaw_initialised = true;
}
break;
}
// if initialisation has been successful update the yaw mode
if( yaw_initialised ) {
yaw_mode = new_yaw_mode;
}
// return success or failure
return yaw_initialised;
}
// update_yaw_mode - run high level yaw controllers
// 100hz update rate
void update_yaw_mode(void)
{
switch(yaw_mode) {
case YAW_HOLD:
// heading hold at heading held in nav_yaw but allow input from pilot
get_yaw_rate_stabilized_ef(g.rc_4.control_in);
break;
case YAW_ACRO:
// pilot controlled yaw using rate controller
if(g.axis_enabled) {
get_yaw_rate_stabilized_ef(g.rc_4.control_in);
}else{
get_acro_yaw(g.rc_4.control_in);
}
return;
break;
// update to allow external roll/yaw mixing
#if TOY_LOOKUP == TOY_EXTERNAL_MIXER
case YAW_TOY:
#endif
case YAW_LOOK_AT_NEXT_WP:
// 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);
get_stabilize_yaw(nav_yaw);
case YAW_HOLD:
get_yaw_rate_stabilized_ef(g.rc_4.control_in);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {
set_yaw_mode(YAW_HOLD);
}
break;
case YAW_LOOK_AT_LOCATION:
// point towards a location held in yaw_look_at_WP (no pilot input accepted)
nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(nav_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {
set_yaw_mode(YAW_HOLD);
}
break;
case YAW_LOOK_AT_HOME:
//nav_yaw updated in update_navigation()
// keep heading always pointing at home with no pilot input allowed
nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(nav_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {
set_yaw_mode(YAW_HOLD);
}
break;
case YAW_AUTO:
nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60); // 40 deg a second
//cliSerial->printf("nav_yaw %d ", nav_yaw);
nav_yaw = wrap_360(nav_yaw);
case YAW_LOOK_AT_HEADING:
// 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);
get_stabilize_yaw(nav_yaw);
break;
case YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
if (g_gps->ground_speed > YAW_LOOK_AHEAD_RATE){ // Speed in cm/s.
nav_yaw += constrain(wrap_180(ground_bearing - nav_yaw), -60, 60); // 40 deg a second
nav_yaw = wrap_360(nav_yaw);
get_stabilize_yaw(nav_yaw + g.rc_4.control_in); // Allow pilot to "skid" around corners up to 45°
} else {
nav_yaw += g.rc_4.control_in * g.acro_p * G_Dt;
nav_yaw = wrap_360(nav_yaw);
get_stabilize_yaw(nav_yaw);
}
break;
get_look_ahead_yaw(g.rc_4.control_in);
break;
#if TOY_LOOKUP == TOY_EXTERNAL_MIXER
case YAW_TOY:
// update to allow external roll/yaw mixing
// keep heading always pointing at home with no pilot input allowed
nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(nav_yaw);
break;
#endif
}
}
// set_roll_pitch_mode - update roll/pitch mode and initialise any variables as required
bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
{
// boolean to ensure proper initialisation of throttle modes
bool roll_pitch_initialised = false;
// return immediately if no change
if( new_roll_pitch_mode == roll_pitch_mode ) {
return true;
}
switch( new_roll_pitch_mode ) {
case ROLL_PITCH_STABLE:
case ROLL_PITCH_ACRO:
case ROLL_PITCH_AUTO:
case ROLL_PITCH_STABLE_OF:
case ROLL_PITCH_TOY:
roll_pitch_initialised = true;
break;
}
// if initialisation has been successful update the yaw mode
if( roll_pitch_initialised ) {
roll_pitch_mode = new_roll_pitch_mode;
}
// return success or failure
return roll_pitch_initialised;
}
// update_roll_pitch_mode - run high level roll and pitch controllers
// 100hz update rate
void update_roll_pitch_mode(void)
{
if (ap.do_flip) {
@ -1671,6 +1771,11 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) @@ -1671,6 +1771,11 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
// boolean to ensure proper initialisation of throttle modes
bool throttle_initialised = false;
// return immediately if no change
if( new_throttle_mode == throttle_mode ) {
return true;
}
// initialise any variables required for the new throttle mode
switch(new_throttle_mode) {
case THROTTLE_MANUAL:
@ -1726,8 +1831,8 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) @@ -1726,8 +1831,8 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
return throttle_initialised;
}
// update_throttle_mode - run high level throttle controllers
// 50 hz update rate
// controls all throttle behavior
void update_throttle_mode(void)
{
int16_t pilot_climb_rate;
@ -2195,10 +2300,10 @@ static void update_nav_wp() @@ -2195,10 +2300,10 @@ static void update_nav_wp()
}else if(wp_control == CIRCLE_MODE) {
// check if we have missed the WP
int16_t loiter_delta = (target_bearing - old_target_bearing)/100;
int16_t loiter_delta = (wp_bearing - old_wp_bearing)/100;
// reset the old value
old_target_bearing = target_bearing;
old_wp_bearing = wp_bearing;
// wrap values
if (loiter_delta > 180) loiter_delta -= 360;
@ -2249,27 +2354,3 @@ static void update_nav_wp() @@ -2249,27 +2354,3 @@ static void update_nav_wp()
nav_lat = constrain(nav_lat, -2000, 2000); // 20°
}
}
static void update_auto_yaw()
{
switch( wp_control ) {
case LOITER_MODE:
// just hold nav_yaw
break;
case WP_MODE:
if(auto_yaw_tracking == MAV_ROI_LOCATION) {
auto_yaw = get_bearing_cd(&current_loc, &target_WP);
}else if(auto_yaw_tracking == MAV_ROI_WPNEXT) {
// Point towards next WP
// we don't use target_bearing because we don't want the copter to turn too much during flight
auto_yaw = original_target_bearing;
}
break;
case CIRCLE_MODE:
auto_yaw = get_bearing_cd(&current_loc, &circle_WP);
break;
case NO_NAV_MODE:
// just hold nav_yaw
break;
}
}

20
ArduCopter/Attitude.pde
Unescape View File

@ -699,6 +699,23 @@ get_of_pitch(int32_t input_pitch) @@ -699,6 +699,23 @@ get_of_pitch(int32_t input_pitch)
#endif
}
/*************************************************************
* yaw controllers
*************************************************************/
// update_throttle_cruise - update throttle cruise if necessary
static void get_look_ahead_yaw(int16_t pilot_yaw)
{
// Commanded Yaw to automatically look ahead.
if (g_gps->fix && g_gps->ground_course > YAW_LOOK_AHEAD_MIN_SPEED) {
nav_yaw = get_yaw_slew(nav_yaw, g_gps->ground_course, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(wrap_360(nav_yaw + pilot_yaw)); // Allow pilot to "skid" around corners up to 45 degrees
}else{
nav_yaw += pilot_yaw * g.acro_p * G_Dt;
nav_yaw = wrap_360(nav_yaw);
get_stabilize_yaw(nav_yaw);
}
}
/*************************************************************
* throttle control
@ -826,9 +843,6 @@ get_throttle_accel(int16_t z_target_accel) @@ -826,9 +843,6 @@ get_throttle_accel(int16_t z_target_accel)
//
// limit the rate
output = constrain(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max);
//Serial.printf("ThAccel 1 z_target_accel:%4.2f z_accel_meas:%4.2f z_accel_error:%4.2f output:%4.2f p:%4.2f i:%4.2f d:%4.2f \n", 1.0*z_target_accel, 1.0*z_accel_meas, 1.0*z_accel_error, 1.0*output, 1.0*p, 1.0*i, 1.0*d);
//Serial.printf("motors.reached_limit:%4.2f reset_accel_throttle_counter:%4.2f output:%4.2f p:%4.2f i:%4.2f d:%4.2f \n", 1.0*motors.reached_limit(0xff), 1.0*reset_accel_throttle_counter, 1.0*output, 1.0*p, 1.0*i, 1.0*d);
//Serial.printf("One G: z_target_accel:%4.2f z_accel_meas:%4.2f accel_one_g:%4.2f \n", 1.0*z_target_accel, 1.0*z_accel_meas, 1.0*accel_one_g);
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash

4
ArduCopter/GCS_Mavlink.pde
Unescape View File

@ -230,8 +230,8 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) @@ -230,8 +230,8 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
target_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_bearing / 1.0e2,
wp_bearing / 1.0e2,
wp_distance / 1.0e2,
altitude_error / 1.0e2,
0,

2
ArduCopter/Log.pde
Unescape View File

@ -562,7 +562,7 @@ static void Log_Write_Nav_Tuning() @@ -562,7 +562,7 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt(wp_distance); // 1
DataFlash.WriteInt(target_bearing/100); // 2
DataFlash.WriteInt(wp_bearing/100); // 2
DataFlash.WriteInt(long_error); // 3
DataFlash.WriteInt(lat_error); // 4

2
ArduCopter/Parameters.pde
Unescape View File

@ -284,7 +284,7 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -284,7 +284,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: AUTO_SLEW
// @DisplayName: Auto Slew Rate
// @Description: This restricts the rate of change of the attitude allowed by the Auto Controller
// @Description: This restricts the rate of change of the roll and pitch attitude commanded by the auto pilot
// @Units: Degrees/Second
// @Range: 1 45
// @Increment: 1

11
ArduCopter/commands.pde
Unescape View File

@ -11,9 +11,6 @@ static void init_commands() @@ -11,9 +11,6 @@ static void init_commands()
ap.fast_corner = false;
reset_desired_speed();
// default auto mode yaw tracking
auto_yaw_tracking = MAV_ROI_WPNEXT;
}
// Getters
@ -169,14 +166,14 @@ static void set_next_WP(struct Location *wp) @@ -169,14 +166,14 @@ static void set_next_WP(struct Location *wp)
// this is handy for the groundstation
// -----------------------------------
wp_distance = get_distance_cm(&current_loc, &next_WP);
target_bearing = get_bearing_cd(&prev_WP, &next_WP);
wp_bearing = get_bearing_cd(&prev_WP, &next_WP);
// calc the location error:
calc_location_error(&next_WP);
// to check if we have missed the WP
// ---------------------------------
original_target_bearing = target_bearing;
original_wp_bearing = wp_bearing;
}
@ -190,14 +187,10 @@ static void init_home() @@ -190,14 +187,10 @@ static void init_home()
home.lat = g_gps->latitude; // Lat * 10**7
home.alt = 0; // Home is always 0
// to point yaw towards home until we set it with Mavlink
target_WP = home;
// Save Home to EEPROM
// -------------------
// no need to save this to EPROM
set_cmd_with_index(home, 0);
//print_wp(&home, 0);
#if INERTIAL_NAV_XY == ENABLED
// set inertial nav's home position

140
ArduCopter/commands_logic.pde
Unescape View File

@ -16,7 +16,7 @@ static void process_nav_command() @@ -16,7 +16,7 @@ static void process_nav_command()
break;
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
yaw_mode = YAW_HOLD;
set_yaw_mode(YAW_HOLD);
do_land();
break;
@ -217,9 +217,8 @@ static void do_RTL(void) @@ -217,9 +217,8 @@ static void do_RTL(void)
rtl_state = RTL_STATE_RETURNING_HOME;
// set roll, pitch and yaw modes
roll_pitch_mode = RTL_RP;
yaw_mode = YAW_AUTO;
auto_yaw_tracking = MAV_ROI_WPNEXT;
set_roll_pitch_mode(RTL_RP);
set_yaw_mode(YAW_LOOK_AT_HOME);
set_throttle_mode(RTL_THR);
// set navigation mode
@ -321,9 +320,9 @@ static void do_loiter_turns() @@ -321,9 +320,9 @@ static void do_loiter_turns()
loiter_total = command_nav_queue.p1 * 360;
loiter_sum = 0;
old_target_bearing = target_bearing;
old_wp_bearing = wp_bearing;
circle_angle = target_bearing + 18000;
circle_angle = wp_bearing + 18000;
circle_angle = wrap_360(circle_angle);
circle_angle *= RADX100;
}
@ -472,7 +471,7 @@ static bool verify_RTL() @@ -472,7 +471,7 @@ static bool verify_RTL()
rtl_loiter_start_time = millis();
// give pilot back control of yaw
yaw_mode = YAW_HOLD;
set_yaw_mode(YAW_HOLD);
}
break;
@ -556,57 +555,29 @@ static void do_within_distance() @@ -556,57 +555,29 @@ static void do_within_distance()
static void do_yaw()
{
//cliSerial->println("dyaw ");
auto_yaw_tracking = MAV_ROI_NONE;
// target angle in degrees
command_yaw_start = nav_yaw; // current position
command_yaw_start_time = millis();
command_yaw_dir = command_cond_queue.p1; // 1 = clockwise, 0 = counterclockwise
command_yaw_speed = command_cond_queue.lat * 100; // ms * 100
command_yaw_relative = command_cond_queue.lng; // 1 = Relative, 0 = Absolute
// if unspecified turn at 30° per second
if(command_yaw_speed == 0)
command_yaw_speed = 3000;
// ensure direction is valid, if invalid default to counter clockwise
if(command_yaw_dir > 1)
command_yaw_dir = 0; // 0 = counter clockwise, 1 = clockwise
// get final angle, 1 = Relative, 0 = Absolute
if( command_cond_queue.lng == 0 ) {
// absolute angle
yaw_look_at_heading = wrap_360(command_cond_queue.alt * 100);
}else{
// relative angle
yaw_look_at_heading = wrap_360(nav_yaw + command_cond_queue.alt * 100);
}
if(command_yaw_relative == 1) {
// relative
command_yaw_delta = command_cond_queue.alt * 100;
if(command_yaw_dir == 0) { // 0 = counter clockwise, 1 = clockwise
command_yaw_end = command_yaw_start - command_yaw_delta;
}else{
command_yaw_end = command_yaw_start + command_yaw_delta;
}
command_yaw_end = wrap_360(command_yaw_end);
// get turn speed
if( command_cond_queue.lat == 0 ) {
// default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
}else{
// absolute
command_yaw_end = command_cond_queue.alt * 100;
// calculate the delta travel in deg * 100
if(command_yaw_dir == 0) { // 0 = counter clockwise, 1 = clockwise
if(command_yaw_start > command_yaw_end) {
command_yaw_delta = command_yaw_start - command_yaw_end;
}else{
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
}
}else{
if(command_yaw_start >= command_yaw_end) {
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
}else{
command_yaw_delta = command_yaw_end - command_yaw_start;
}
}
command_yaw_delta = wrap_360(command_yaw_delta);
int32_t turn_rate = (wrap_180(yaw_look_at_heading - nav_yaw) / 100) / command_cond_queue.lat;
yaw_look_at_heading_slew = constrain(turn_rate, 1, 360); // deg / sec
}
// rate to turn deg per second - default is ten
command_yaw_time = (command_yaw_delta / command_yaw_speed) * 1000;
// set yaw mode
set_yaw_mode(YAW_LOOK_AT_HEADING);
// TO-DO: restore support for clockwise / counter clockwise rotation held in command_cond_queue.p1
// command_cond_queue.p1; // 0 = undefined, 1 = clockwise, -1 = counterclockwise
}
@ -653,35 +624,12 @@ static bool verify_within_distance() @@ -653,35 +624,12 @@ static bool verify_within_distance()
return false;
}
// verify_yaw - return true if we have reached the desired heading
static bool verify_yaw()
{
//cliSerial->printf("vyaw %d\n", (int)(nav_yaw/100));
if((millis() - command_yaw_start_time) > command_yaw_time) {
// time out
// make sure we hold at the final desired yaw angle
nav_yaw = command_yaw_end;
auto_yaw = nav_yaw;
// TO-DO: there's still a problem with Condition_yaw, it will do it two times(probably more) sometimes, if it hasn't reached the next waypoint yet.
// it should only do it one time so there should be code here to prevent another Condition_Yaw.
//cliSerial->println("Y");
if( labs(wrap_180(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200 ) {
return true;
}else{
// else we need to be at a certain place
// power is a ratio of the time : .5 = half done
float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
if(command_yaw_dir == 0) { // 0 = counter clockwise, 1 = clockwise
nav_yaw = command_yaw_start - ((float)command_yaw_delta * power );
}else{
nav_yaw = command_yaw_start + ((float)command_yaw_delta * power );
}
nav_yaw = wrap_360(nav_yaw);
auto_yaw = nav_yaw;
//cliSerial->printf("ny %ld\n",nav_yaw);
return false;
}
}
@ -697,8 +645,7 @@ static bool verify_nav_roi() @@ -697,8 +645,7 @@ static bool verify_nav_roi()
// check if mount type requires us to rotate the quad
if( camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll ) {
// ensure yaw has gotten to within 2 degrees of the target
if( labs(wrap_180(ahrs.yaw_sensor-auto_yaw)) <= 200 ) {
nav_yaw = auto_yaw; // ensure target yaw for YAW_HOLD is our desired yaw
if( labs(wrap_180(ahrs.yaw_sensor-yaw_look_at_WP_bearing)) <= 200 ) {
return true;
}else{
return false;
@ -710,8 +657,7 @@ static bool verify_nav_roi() @@ -710,8 +657,7 @@ static bool verify_nav_roi()
#else
// if we have no camera mount simply check we've reached the desired yaw
// ensure yaw has gotten to within 2 degrees of the target
if( abs(wrap_180(ahrs.yaw_sensor-auto_yaw)) <= 200 ) {
nav_yaw = auto_yaw; // ensure target yaw for YAW_HOLD is our desired yaw
if( labs(wrap_180(ahrs.yaw_sensor-yaw_look_at_WP_bearing)) <= 200 ) {
return true;
}else{
return false;
@ -728,12 +674,20 @@ static void do_change_speed() @@ -728,12 +674,20 @@ static void do_change_speed()
g.waypoint_speed_max = command_cond_queue.p1 * 100;
}
// do_target_yaw - initialise yaw mode based on requested yaw target
static void do_target_yaw()
{
auto_yaw_tracking = command_cond_queue.p1;
if(auto_yaw_tracking == MAV_ROI_LOCATION) {
target_WP = command_cond_queue;
switch( command_cond_queue.p1 ) {
case MAV_ROI_NONE:
set_yaw_mode(YAW_HOLD);
break;
case MAV_ROI_WPNEXT:
set_yaw_mode(YAW_LOOK_AT_NEXT_WP);
break;
case MAV_ROI_LOCATION:
yaw_look_at_WP = command_cond_queue;
set_yaw_mode(YAW_LOOK_AT_LOCATION);
break;
}
}
@ -896,9 +850,8 @@ static void do_nav_roi() @@ -896,9 +850,8 @@ static void do_nav_roi()
// check if mount type requires us to rotate the quad
if( camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll ) {
auto_yaw_tracking = MAV_ROI_LOCATION;
target_WP = command_nav_queue;
auto_yaw = get_bearing_cd(&current_loc, &target_WP);
yaw_look_at_WP = command_nav_queue;
set_yaw_mode(YAW_LOOK_AT_LOCATION);
}
// send the command to the camera mount
camera_mount.set_roi_cmd(&command_nav_queue);
@ -910,10 +863,9 @@ static void do_nav_roi() @@ -910,10 +863,9 @@ static void do_nav_roi()
// 3: point at a location given by alt, lon, lat parameters
// 4: point at a target given a target id (can't be implmented)
#else
// if we have no camera mount simply rotate the quad
auto_yaw_tracking = MAV_ROI_LOCATION;
target_WP = command_nav_queue;
auto_yaw = get_bearing_cd(&current_loc, &target_WP);
// if we have no camera mount aim the quad at the location
yaw_look_at_WP = command_nav_queue;
set_yaw_mode(YAW_LOOK_AT_LOCATION);
#endif
}

2
ArduCopter/commands_process.pde
Unescape View File

@ -79,7 +79,7 @@ static void update_commands() @@ -79,7 +79,7 @@ static void update_commands()
if(tmp_loc.lat == 0) {
ap.fast_corner = false;
}else{
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_wp_bearing;
temp = wrap_180(temp);
ap.fast_corner = labs(temp) < 6000;
}

28
ArduCopter/config.h
Unescape View File

@ -235,15 +235,7 @@ @@ -235,15 +235,7 @@
#endif
//////////////////////////////////////////////////////////////////////////////
// Channel Config (custom MOT channel mappings)
//
#ifndef CONFIG_CHANNELS
# define CONFIG_CHANNELS CHANNEL_CONFIG_DEFAULT
#endif
//////////////////////////////////////////////////////////////////////////////
// Acrobatics
// Channel 7 default option
//
#ifndef CH7_OPTION
@ -532,7 +524,9 @@ @@ -532,7 +524,9 @@
// Alt Hold Mode
#ifndef ALT_HOLD_YAW
# define ALT_HOLD_YAW YAW_HOLD
//# define ALT_HOLD_YAW YAW_HOLD
// debug -- remove me!!
# define ALT_HOLD_YAW YAW_LOOK_AHEAD
#endif
#ifndef ALT_HOLD_RP
@ -545,7 +539,7 @@ @@ -545,7 +539,7 @@
// AUTO Mode
#ifndef AUTO_YAW
# define AUTO_YAW YAW_AUTO
# define AUTO_YAW YAW_LOOK_AT_NEXT_WP
#endif
#ifndef AUTO_RP
@ -558,7 +552,7 @@ @@ -558,7 +552,7 @@
// CIRCLE Mode
#ifndef CIRCLE_YAW
# define CIRCLE_YAW YAW_AUTO
# define CIRCLE_YAW YAW_LOOK_AT_NEXT_WP
#endif
#ifndef CIRCLE_RP
@ -698,8 +692,8 @@ @@ -698,8 +692,8 @@
# define STABILIZE_YAW_IMAX 8.0 // degrees * 100
#endif
#ifndef YAW_LOOK_AHEAD_RATE
# define YAW_LOOK_AHEAD_RATE 1000 // dimensionless, smaller number means stronger effect
#ifndef YAW_LOOK_AHEAD_MIN_SPEED
# define YAW_LOOK_AHEAD_MIN_SPEED 1000 // minimum ground speed in cm/s required before copter is aimed at ground course
#endif
@ -832,7 +826,11 @@ @@ -832,7 +826,11 @@
#endif
#ifndef AUTO_SLEW_RATE
# define AUTO_SLEW_RATE 30 // degrees
# define AUTO_SLEW_RATE 30 // degrees/sec
#endif
#ifndef AUTO_YAW_SLEW_RATE
# define AUTO_YAW_SLEW_RATE 60 // degrees/sec
#endif

14
ArduCopter/defines.h
Unescape View File

@ -13,12 +13,14 @@ @@ -13,12 +13,14 @@
// Flight modes
// ------------
#define YAW_HOLD 0
#define YAW_ACRO 1
#define YAW_AUTO 2
#define YAW_LOOK_AT_HOME 3
#define YAW_TOY 4 // THOR This is the Yaw mode
#define YAW_LOOK_AHEAD 5 // WARNING! CODE IN DEVELOPMENT NOT PROVEN
#define YAW_HOLD 0 // heading hold at heading in nav_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)
#define YAW_LOOK_AT_HOME 4 // point towards home (no pilot input accepted)
#define YAW_LOOK_AT_HEADING 5 // point towards a particular angle (not pilot input accepted)
#define YAW_LOOK_AHEAD 6 // WARNING! CODE IN DEVELOPMENT NOT PROVEN
#define YAW_TOY 7 // THOR This is the Yaw mode
#define ROLL_PITCH_STABLE 0

60
ArduCopter/navigation.pde
Unescape View File

@ -46,9 +46,6 @@ static void update_navigation() @@ -46,9 +46,6 @@ static void update_navigation()
// calculate velocity
calc_velocity_and_position();
// calculate ground bearing
calc_ground_bearing();
// calculate distance, angles to target
calc_distance_and_bearing();
@ -130,11 +127,6 @@ static void calc_velocity_and_position(){ @@ -130,11 +127,6 @@ static void calc_velocity_and_position(){
last_gps_latitude = g_gps->latitude;
}
static void calc_ground_bearing(){
ground_bearing = atan2( lat_speed , lon_speed ) * DEGX100;
ground_bearing = wrap_360(ground_bearing); // atan2 returns a value of -pi to +pi, so we need to wrap this.
}
//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************
@ -145,10 +137,13 @@ static void calc_distance_and_bearing() @@ -145,10 +137,13 @@ static void calc_distance_and_bearing()
wp_distance = get_distance_cm(&current_loc, &next_WP);
home_distance = get_distance_cm(&current_loc, &home);
// target_bearing is where we should be heading
// wp_bearing is bearing to next waypoint
// --------------------------------------------
target_bearing = get_bearing_cd(&current_loc, &next_WP);
home_to_copter_bearing = get_bearing_cd(&home, &current_loc);
wp_bearing = get_bearing_cd(&current_loc, &next_WP);
home_bearing = get_bearing_cd(&current_loc, &home);
// bearing to target (used when yaw_mode = YAW_LOOK_AT_LOCATION)
yaw_look_at_WP_bearing = get_bearing_cd(&current_loc, &yaw_look_at_WP);
}
static void calc_location_error(struct Location *next_loc)
@ -179,9 +174,6 @@ static void run_navigation_contollers() @@ -179,9 +174,6 @@ static void run_navigation_contollers()
// note: wp_control is handled by commands_logic
verify_commands();
// calculates desired Yaw
update_auto_yaw();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
@ -192,9 +184,7 @@ static void run_navigation_contollers() @@ -192,9 +184,7 @@ static void run_navigation_contollers()
wp_verify_byte = 0;
verify_nav_wp();
if (wp_control == WP_MODE) {
update_auto_yaw();
} else {
if (wp_control != WP_MODE) {
set_mode(LOITER);
}
update_nav_wp();
@ -204,9 +194,6 @@ static void run_navigation_contollers() @@ -204,9 +194,6 @@ static void run_navigation_contollers()
// execute the RTL state machine
verify_RTL();
// calculates desired Yaw
update_auto_yaw();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
@ -252,9 +239,6 @@ static void run_navigation_contollers() @@ -252,9 +239,6 @@ static void run_navigation_contollers()
case CIRCLE:
wp_control = CIRCLE_MODE;
// calculates desired Yaw
update_auto_yaw();
update_nav_wp();
break;
@ -271,24 +255,16 @@ static void run_navigation_contollers() @@ -271,24 +255,16 @@ static void run_navigation_contollers()
// get distance to home
if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home
// we reset the angular offset to be a vector from home to the quad
initial_simple_bearing = home_to_copter_bearing;
initial_simple_bearing = wrap_360(home_bearing+18000);
//cliSerial->printf("ISB: %d\n", initial_simple_bearing);
}
}
if(yaw_mode == YAW_LOOK_AT_HOME) {
if(ap.home_is_set) {
nav_yaw = get_bearing_cd(&current_loc, &home);
} else {
nav_yaw = 0;
}
}
}
static bool check_missed_wp()
{
int32_t temp;
temp = target_bearing - original_target_bearing;
temp = wp_bearing - original_wp_bearing;
temp = wrap_180(temp);
return (labs(temp) > 9000); // we passed the waypoint by 100 degrees
}
@ -383,7 +359,7 @@ static void calc_nav_rate(int16_t max_speed) @@ -383,7 +359,7 @@ static void calc_nav_rate(int16_t max_speed)
cross_speed = constrain(cross_speed, -150, 150);
// rotate by 90 to deal with trig functions
temp = (9000l - target_bearing) * RADX100;
temp = (9000l - wp_bearing) * RADX100;
temp_x = cos(temp);
temp_y = sin(temp);
@ -471,9 +447,9 @@ static void update_crosstrack(void) @@ -471,9 +447,9 @@ static void update_crosstrack(void)
// Crosstrack Error
// ----------------
if (wp_distance >= (g.crosstrack_min_distance * 100) &&
abs(wrap_180(target_bearing - original_target_bearing)) < 4500) {
abs(wrap_180(wp_bearing - original_wp_bearing)) < 4500) {
float temp = (target_bearing - original_target_bearing) * RADX100;
float temp = (wp_bearing - original_wp_bearing) * RADX100;
crosstrack_error = sin(temp) * wp_distance; // Meters we are off track line
}else{
// fade out crosstrack
@ -549,7 +525,7 @@ static void reset_nav_params(void) @@ -549,7 +525,7 @@ static void reset_nav_params(void)
crosstrack_error = 0;
// Will be set by new command
target_bearing = 0;
wp_bearing = 0;
// Will be set by new command
wp_distance = 0;
@ -563,9 +539,6 @@ static void reset_nav_params(void) @@ -563,9 +539,6 @@ static void reset_nav_params(void)
nav_pitch = 0;
auto_roll = 0;
auto_pitch = 0;
// make sure we stick to Nav yaw on takeoff
auto_yaw = nav_yaw;
}
static int32_t wrap_360(int32_t error)
@ -581,3 +554,10 @@ static int32_t wrap_180(int32_t error) @@ -581,3 +554,10 @@ static int32_t wrap_180(int32_t error)
if (error < -18000) error += 36000;
return error;
}
// get_yaw_slew - reduces rate of change of yaw to a maximum
// assumes it is called at 100hz so centi-degrees and update rate cancel each other out
static int32_t get_yaw_slew(int32_t current_yaw, int32_t desired_yaw, int16_t deg_per_sec)
{
return wrap_360(current_yaw + constrain(wrap_180(desired_yaw - current_yaw), -deg_per_sec, deg_per_sec));
}

62
ArduCopter/system.pde
Unescape View File

@ -392,6 +392,7 @@ static void startup_ground(void) @@ -392,6 +392,7 @@ static void startup_ground(void)
reset_I_all();
}
// set_mode - change flight mode and perform any necessary initialisation
static void set_mode(byte mode)
{
// Switch to stabilize mode if requested mode requires a GPS lock
@ -430,8 +431,8 @@ static void set_mode(byte mode) @@ -430,8 +431,8 @@ static void set_mode(byte mode)
case ACRO:
ap.manual_throttle = true;
ap.manual_attitude = true;
yaw_mode = YAW_ACRO;
roll_pitch_mode = ROLL_PITCH_ACRO;
set_yaw_mode(YAW_ACRO);
set_roll_pitch_mode(ROLL_PITCH_ACRO);
set_throttle_mode(THROTTLE_MANUAL);
// reset acro axis targets to current attitude
if(g.axis_enabled){
@ -444,16 +445,16 @@ static void set_mode(byte mode) @@ -444,16 +445,16 @@ static void set_mode(byte mode)
case STABILIZE:
ap.manual_throttle = true;
ap.manual_attitude = true;
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_STABLE;
set_yaw_mode(YAW_HOLD);
set_roll_pitch_mode(ROLL_PITCH_STABLE);
set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED);
break;
case ALT_HOLD:
ap.manual_throttle = false;
ap.manual_attitude = true;
yaw_mode = ALT_HOLD_YAW;
roll_pitch_mode = ALT_HOLD_RP;
set_yaw_mode(ALT_HOLD_YAW);
set_roll_pitch_mode(ALT_HOLD_RP);
set_throttle_mode(ALT_HOLD_THR);
force_new_altitude(max(current_loc.alt, 100));
break;
@ -461,8 +462,8 @@ static void set_mode(byte mode) @@ -461,8 +462,8 @@ static void set_mode(byte mode)
case AUTO:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = AUTO_YAW;
roll_pitch_mode = AUTO_RP;
set_yaw_mode(AUTO_YAW);
set_roll_pitch_mode(AUTO_RP);
set_throttle_mode(AUTO_THR);
// loads the commands from where we left off
@ -472,19 +473,24 @@ static void set_mode(byte mode) @@ -472,19 +473,24 @@ static void set_mode(byte mode)
case CIRCLE:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = CIRCLE_YAW;
roll_pitch_mode = CIRCLE_RP;
set_throttle_mode(CIRCLE_THR);
// start circling around current location
set_next_WP(&current_loc);
circle_WP = next_WP;
// set yaw to point to center of circle
yaw_look_at_WP = circle_WP;
set_yaw_mode(YAW_LOOK_AT_LOCATION);
set_roll_pitch_mode(CIRCLE_RP);
set_throttle_mode(CIRCLE_THR);
circle_angle = 0;
break;
case LOITER:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP;
set_yaw_mode(LOITER_YAW);
set_roll_pitch_mode(LOITER_RP);
set_throttle_mode(LOITER_THR);
set_next_WP(&current_loc);
break;
@ -492,8 +498,8 @@ static void set_mode(byte mode) @@ -492,8 +498,8 @@ static void set_mode(byte mode)
case POSITION:
ap.manual_throttle = true;
ap.manual_attitude = false;
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_AUTO;
set_yaw_mode(YAW_HOLD);
set_roll_pitch_mode(ROLL_PITCH_AUTO);
set_throttle_mode(THROTTLE_MANUAL);
set_next_WP(&current_loc);
break;
@ -501,10 +507,10 @@ static void set_mode(byte mode) @@ -501,10 +507,10 @@ static void set_mode(byte mode)
case GUIDED:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = YAW_AUTO;
roll_pitch_mode = ROLL_PITCH_AUTO;
set_yaw_mode(YAW_LOOK_AT_NEXT_WP);
set_roll_pitch_mode(ROLL_PITCH_AUTO);
set_throttle_mode(THROTTLE_AUTO);
next_WP = current_loc;
next_WP = current_loc;
set_next_WP(&guided_WP);
break;
@ -512,13 +518,13 @@ static void set_mode(byte mode) @@ -512,13 +518,13 @@ static void set_mode(byte mode)
if( ap.home_is_set ) {
// switch to loiter if we have gps
ap.manual_attitude = false;
yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP;
set_yaw_mode(LOITER_YAW);
set_roll_pitch_mode(LOITER_RP);
}else{
// otherwise remain with stabilize roll and pitch
ap.manual_attitude = true;
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_STABLE;
set_yaw_mode(YAW_HOLD);
set_roll_pitch_mode(ROLL_PITCH_STABLE);
}
ap.manual_throttle = false;
do_land();
@ -533,8 +539,8 @@ static void set_mode(byte mode) @@ -533,8 +539,8 @@ static void set_mode(byte mode)
case OF_LOITER:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = OF_LOITER_YAW;
roll_pitch_mode = OF_LOITER_RP;
set_yaw_mode(OF_LOITER_YAW);
set_roll_pitch_mode(OF_LOITER_RP);
set_throttle_mode(OF_LOITER_THR);
set_next_WP(&current_loc);
break;
@ -545,8 +551,8 @@ static void set_mode(byte mode) @@ -545,8 +551,8 @@ static void set_mode(byte mode)
case TOY_A:
ap.manual_throttle = false;
ap.manual_attitude = true;
yaw_mode = YAW_TOY;
roll_pitch_mode = ROLL_PITCH_TOY;
set_yaw_mode(YAW_TOY);
set_roll_pitch_mode(ROLL_PITCH_TOY);
set_throttle_mode(THROTTLE_AUTO);
wp_control = NO_NAV_MODE;
@ -558,8 +564,8 @@ static void set_mode(byte mode) @@ -558,8 +564,8 @@ static void set_mode(byte mode)
case TOY_M:
ap.manual_throttle = false;
ap.manual_attitude = true;
yaw_mode = YAW_TOY;
roll_pitch_mode = ROLL_PITCH_TOY;
set_yaw_mode(YAW_TOY);
set_roll_pitch_mode(ROLL_PITCH_TOY);
wp_control = NO_NAV_MODE;
set_throttle_mode(THROTTLE_HOLD);
break;

2
ArduCopter/test.pde
Unescape View File

@ -981,7 +981,7 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv) @@ -981,7 +981,7 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv)
// got 23506;, should be 22800
update_navigation();
cliSerial->printf_P(PSTR("bear: %ld\n"), target_bearing);
cliSerial->printf_P(PSTR("bear: %ld\n"), wp_bearing);
return 0;
}

Write Preview
Loading…
Cancel
Save