diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 219c653c49..ed2dfdb619 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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(¤t_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 ) // 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(¤t_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(¤t_loc, &circle_WP); - break; - case NO_NAV_MODE: - // just hold nav_yaw - break; - } -} diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 034cfca1f8..3f966f7002 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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 diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 6bfa7d8422..a1487777bd 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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, diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index e5df669c80..90ffc52f76 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -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 diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 5de279a3a8..18427db378 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -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 diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 8cb5bc4e1b..b6ef87f552 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -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(¤t_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 diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 4759eec755..d3b510875a 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -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 + // 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() 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(¤t_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(¤t_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 } diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 8b8bf92c34..e506e2e728 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -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; } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 168186f0dc..888974ad20 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 4e033483e9..1a89bf9d62 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 20ef827ff7..51bc5cc1be 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -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(¤t_loc, &next_WP); home_distance = get_distance_cm(¤t_loc, &home); - // target_bearing is where we should be heading + // wp_bearing is bearing to next waypoint // -------------------------------------------- - target_bearing = get_bearing_cd(¤t_loc, &next_WP); - home_to_copter_bearing = get_bearing_cd(&home, ¤t_loc); + wp_bearing = get_bearing_cd(¤t_loc, &next_WP); + home_bearing = get_bearing_cd(¤t_loc, &home); + + // bearing to target (used when yaw_mode = YAW_LOOK_AT_LOCATION) + yaw_look_at_WP_bearing = get_bearing_cd(¤t_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(¤t_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)); +} \ No newline at end of file diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index df832abdbb..eca4275a85 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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(¤t_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(¤t_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(¤t_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(¤t_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; diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index e63d736154..74725dc717 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -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; }