Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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.
This commit is contained in:
rmackay9 2012-12-08 14:23:32 +09:00
parent 6596f1728a
commit f62c377062
13 changed files with 311 additions and 285 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

247
ArduCopter/ArduCopter.pde
View File

@ -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 {
// 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;
// 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;
// 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;
////////////////////////////////////////////////////////////////////////////////
// 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;
////////////////////////////////////////////////////////////////////////////////
// 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;
////////////////////////////////////////////////////////////////////////////////
// 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)
}
}
// 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_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_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_AHEAD:
// Commanded Yaw to automatically look ahead.
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;
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;
#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 )
// 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 )
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()
}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()
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
View File

@ -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)
//
// 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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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)
// 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()
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

142
ArduCopter/commands_logic.pde
View File

@ -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)
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()
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()
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()
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
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 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{
// 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);
// relative angle
yaw_look_at_heading = wrap_360(nav_yaw + command_cond_queue.alt * 100);
}
// rate to turn deg per second - default is ten
command_yaw_time = (command_yaw_delta / command_yaw_speed) * 1000;
// 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{
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
}
// 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()
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()
// 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()
#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()
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()
// 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()
// 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
View File

@ -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
View File

@ -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 @@
// 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 @@
// 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 @@
// 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 @@
# 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 @@
#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
View File

@ -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
View File

@ -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(){
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()
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()
// 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()
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()
// 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()
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()
// 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)
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)
// 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)
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)
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)
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
View File

@ -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)
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)
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)
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)
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)
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)
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)
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)
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)
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)
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
View File

@ -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;
}