diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b1142cb00e..0fe3c3f134 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -657,8 +657,6 @@ static int32_t baro_alt; // Flight modes are combinations of Roll/Pitch, Yaw and Throttle control modes // Each Flight mode is a unique combination of these modes // -// The current desired control scheme for Yaw -static uint8_t yaw_mode = STABILIZE_YAW; // The current desired control scheme for roll and pitch / navigation static uint8_t roll_pitch_mode = STABILIZE_RP; // The current desired control scheme for altitude hold @@ -710,16 +708,20 @@ static uint32_t throttle_integrator; //////////////////////////////////////////////////////////////////////////////// // Navigation Yaw control //////////////////////////////////////////////////////////////////////////////// +// auto flight mode's yaw mode +static uint8_t auto_yaw_mode = AUTO_YAW_LOOK_AT_NEXT_WP; // The Commanded Yaw from the autopilot. static int32_t control_yaw; // Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION static Vector3f yaw_look_at_WP; // bearing from current location to the yaw_look_at_WP -static int32_t yaw_look_at_WP_bearing; +static float 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 yaw_look_at_heading_slew; +// heading when in yaw_look_ahead_bearing +static float yaw_look_ahead_bearing; @@ -1359,239 +1361,6 @@ static void update_GPS(void) failsafe_gps_check(); } -// 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: - yaw_initialised = true; - break; - case YAW_ACRO: - yaw_initialised = true; - acro_yaw_rate = 0; - 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 = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP); - yaw_initialised = true; - } - break; - case YAW_CIRCLE: - if( ap.home_is_set ) { - // set yaw to point to center of circle - yaw_look_at_WP = circle_center; - // initialise bearing to current heading - yaw_look_at_WP_bearing = ahrs.yaw_sensor; - 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_LOOK_AHEAD: - if( ap.home_is_set ) { - yaw_initialised = true; - } - break; - case YAW_DRIFT: - yaw_initialised = true; - break; - case YAW_RESETTOARMEDYAW: - control_yaw = ahrs.yaw_sensor; // store current yaw so we can start rotating back to correct one - 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) -{ - int16_t pilot_yaw = g.rc_4.control_in; - - // do not process pilot's yaw input during radio failsafe - if (failsafe.radio) { - pilot_yaw = 0; - } - - switch(yaw_mode) { - - case YAW_HOLD: - // if we are landed reset yaw target to current heading - if (ap.land_complete) { - control_yaw = ahrs.yaw_sensor; - } - // heading hold at heading held in control_yaw but allow input from pilot - get_yaw_rate_stabilized_ef(pilot_yaw); - break; - - case YAW_ACRO: - // pilot controlled yaw using rate controller - get_yaw_rate_stabilized_bf(pilot_yaw); - break; - - case YAW_LOOK_AT_NEXT_WP: - // if we are landed reset yaw target to current heading - if (ap.land_complete) { - control_yaw = ahrs.yaw_sensor; - }else{ - // point towards next waypoint (no pilot input accepted) - // we don't use wp_bearing because we don't want the copter to turn too much during flight - control_yaw = get_yaw_slew(control_yaw, original_wp_bearing, AUTO_YAW_SLEW_RATE); - } - get_stabilize_yaw(control_yaw); - - // if there is any pilot input, switch to YAW_HOLD mode for the next iteration - if (pilot_yaw != 0) { - set_yaw_mode(YAW_HOLD); - } - break; - - case YAW_LOOK_AT_LOCATION: - // if we are landed reset yaw target to current heading - if (ap.land_complete) { - control_yaw = ahrs.yaw_sensor; - } - // point towards a location held in yaw_look_at_WP - get_look_at_yaw(); - - // if there is any pilot input, switch to YAW_HOLD mode for the next iteration - if (pilot_yaw != 0) { - set_yaw_mode(YAW_HOLD); - } - break; - - case YAW_CIRCLE: - // if we are landed reset yaw target to current heading - if (ap.land_complete) { - control_yaw = ahrs.yaw_sensor; - } - // points toward the center of the circle or does a panorama - get_circle_yaw(); - - // if there is any pilot input, switch to YAW_HOLD mode for the next iteration - if (pilot_yaw != 0) { - set_yaw_mode(YAW_HOLD); - } - break; - - case YAW_LOOK_AT_HOME: - // if we are landed reset yaw target to current heading - if (ap.land_complete) { - control_yaw = ahrs.yaw_sensor; - }else{ - // keep heading always pointing at home with no pilot input allowed - control_yaw = get_yaw_slew(control_yaw, home_bearing, AUTO_YAW_SLEW_RATE); - } - get_stabilize_yaw(control_yaw); - - // if there is any pilot input, switch to YAW_HOLD mode for the next iteration - if (pilot_yaw != 0) { - set_yaw_mode(YAW_HOLD); - } - break; - - case YAW_LOOK_AT_HEADING: - // if we are landed reset yaw target to current heading - if (ap.land_complete) { - control_yaw = ahrs.yaw_sensor; - }else{ - // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed - control_yaw = get_yaw_slew(control_yaw, yaw_look_at_heading, yaw_look_at_heading_slew); - } - get_stabilize_yaw(control_yaw); - break; - - case YAW_LOOK_AHEAD: - // if we are landed reset yaw target to current heading - if (ap.land_complete) { - control_yaw = ahrs.yaw_sensor; - } - // Commanded Yaw to automatically look ahead. - get_look_ahead_yaw(pilot_yaw); - break; - - case YAW_DRIFT: - // if we have landed reset yaw target to current heading - if (ap.land_complete) { - control_yaw = ahrs.yaw_sensor; - } - get_yaw_drift(); - break; - - case YAW_RESETTOARMEDYAW: - // if we are landed reset yaw target to current heading - if (ap.land_complete) { - control_yaw = ahrs.yaw_sensor; - }else{ - // changes yaw to be same as when quad was armed - control_yaw = get_yaw_slew(control_yaw, initial_armed_bearing, AUTO_YAW_SLEW_RATE); - } - get_stabilize_yaw(control_yaw); - - // if there is any pilot input, switch to YAW_HOLD mode for the next iteration - if (pilot_yaw != 0) { - set_yaw_mode(YAW_HOLD); - } - - break; - } -} - -// get yaw mode based on WP_YAW_BEHAVIOR parameter -// set rtl parameter to true if this is during an RTL -uint8_t get_wp_yaw_mode(bool rtl) -{ - switch (g.wp_yaw_behavior) { - case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: - return YAW_LOOK_AT_NEXT_WP; - break; - - case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL: - if( rtl ) { - return YAW_HOLD; - }else{ - return YAW_LOOK_AT_NEXT_WP; - } - break; - - case WP_YAW_BEHAVIOR_LOOK_AHEAD: - return YAW_LOOK_AHEAD; - break; - - default: - return YAW_HOLD; - break; - } -} - // 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) { diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index e7c12b71bb..ab4847f664 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -588,32 +588,26 @@ static void get_circle_yaw() // get_look_at_yaw - updates bearing to location held in look_at_yaw_WP and calls stabilize yaw controller // should be called at 100hz -static void get_look_at_yaw() +static float get_look_at_yaw() { static uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz look_at_yaw_counter++; - if( look_at_yaw_counter >= 10 ) { + if (look_at_yaw_counter >= 10) { look_at_yaw_counter = 0; yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP); } - // slew yaw and call stabilize controller - control_yaw = get_yaw_slew(control_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE); - get_stabilize_yaw(control_yaw); + return yaw_look_at_WP_bearing; } -static void get_look_ahead_yaw(int16_t pilot_yaw) +static float get_look_ahead_yaw() { // Commanded Yaw to automatically look ahead. if (g_gps->fix && g_gps->ground_speed_cm > YAW_LOOK_AHEAD_MIN_SPEED) { - control_yaw = get_yaw_slew(control_yaw, g_gps->ground_course_cd, AUTO_YAW_SLEW_RATE); - get_stabilize_yaw(wrap_360_cd(control_yaw + pilot_yaw)); // Allow pilot to "skid" around corners up to 45 degrees - }else{ - control_yaw += pilot_yaw * g.acro_yaw_p * G_Dt; - control_yaw = wrap_360_cd(control_yaw); - get_stabilize_yaw(control_yaw); + yaw_look_ahead_bearing = g_gps->ground_course_cd; } + return yaw_look_ahead_bearing; } /************************************************************* diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 8994cc8c04..f300901a93 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -238,7 +238,7 @@ static void do_takeoff() set_roll_pitch_mode(AUTO_RP); // set yaw mode - set_yaw_mode(YAW_HOLD); + set_auto_yaw_mode(AUTO_YAW_HOLD); // set throttle mode to AUTO although we should already be in this mode set_throttle_mode(AUTO_THR); @@ -286,7 +286,7 @@ static void do_nav_wp() } // set yaw_mode depending upon contents of WP_YAW_BEHAVIOR parameter - set_yaw_mode(get_wp_yaw_mode(false)); + set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } // do_land - initiate landing procedure @@ -304,7 +304,7 @@ static void do_land(const struct Location *cmd) set_roll_pitch_mode(AUTO_RP); // set yaw_mode depending upon contents of WP_YAW_BEHAVIOR parameter - set_yaw_mode(get_wp_yaw_mode(false)); + set_auto_yaw_mode(get_default_auto_yaw_mode(false)); // set throttle mode set_throttle_mode(AUTO_THR); @@ -340,7 +340,7 @@ static void do_land(const struct Location *cmd) } // hold yaw while landing - set_yaw_mode(YAW_HOLD); + set_auto_yaw_mode(AUTO_YAW_HOLD); // set throttle mode to land set_throttle_mode(THROTTLE_LAND); @@ -361,7 +361,7 @@ static void do_loiter_unlimited() set_throttle_mode(AUTO_THR); // hold yaw - set_yaw_mode(YAW_HOLD); + set_auto_yaw_mode(AUTO_YAW_HOLD); // get current position Vector3f curr_pos = inertial_nav.get_position(); @@ -407,7 +407,7 @@ static void do_circle() } // set yaw to point to center of circle - set_yaw_mode(CIRCLE_YAW); + set_auto_yaw_mode(CIRCLE_YAW); // set angle travelled so far to zero circle_angle_total = 0; @@ -429,7 +429,7 @@ static void do_loiter_time() set_throttle_mode(AUTO_THR); // hold yaw - set_yaw_mode(YAW_HOLD); + set_auto_yaw_mode(AUTO_YAW_HOLD); // get current position Vector3f curr_pos = inertial_nav.get_position(); @@ -491,7 +491,7 @@ static bool verify_land() set_roll_pitch_mode(ROLL_PITCH_LOITER); // give pilot control of yaw - set_yaw_mode(YAW_HOLD); + set_auto_yaw_mode(AUTO_YAW_HOLD); // set throttle mode to land set_throttle_mode(THROTTLE_LAND); @@ -587,7 +587,7 @@ static bool verify_RTL() // if we are below rtl alt do initial climb if( current_loc.alt < get_RTL_alt() ) { // first stage of RTL is the initial climb so just hold current yaw - set_yaw_mode(YAW_HOLD); + set_auto_yaw_mode(AUTO_YAW_HOLD); // use projection of safe stopping point based on current location and velocity Vector3f origin, dest; @@ -601,7 +601,7 @@ static bool verify_RTL() rtl_state = RTL_STATE_INITIAL_CLIMB; }else{ // point nose towards home (maybe) - set_yaw_mode(get_wp_yaw_mode(true)); + set_auto_yaw_mode(get_default_auto_yaw_mode(true)); // Set wp navigation target to above home wp_nav.set_wp_destination(Vector3f(0,0,get_RTL_alt())); @@ -628,7 +628,7 @@ static bool verify_RTL() original_wp_bearing = wp_bearing; // point nose towards home (maybe) - set_yaw_mode(get_wp_yaw_mode(true)); + set_auto_yaw_mode(get_default_auto_yaw_mode(true)); // advance to next rtl state rtl_state = RTL_STATE_RETURNING_HOME; @@ -644,10 +644,10 @@ static bool verify_RTL() rtl_loiter_start_time = millis(); // give pilot back control of yaw - if(get_wp_yaw_mode(true) != YAW_HOLD) { - set_yaw_mode(YAW_RESETTOARMEDYAW); // yaw back to initial yaw on take off + if(get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) { + set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW); // yaw back to initial yaw on take off } else { - set_yaw_mode(YAW_HOLD); + set_auto_yaw_mode(AUTO_YAW_HOLD); } // advance to next rtl state @@ -669,7 +669,7 @@ static bool verify_RTL() wp_nav.set_loiter_target(Vector3f(0,0,0)); // hold yaw while landing - set_yaw_mode(YAW_HOLD); + set_auto_yaw_mode(AUTO_YAW_HOLD); // set throttle mode to land set_throttle_mode(THROTTLE_LAND); @@ -770,7 +770,7 @@ static void do_yaw() } // set yaw mode - set_yaw_mode(YAW_LOOK_AT_HEADING); + set_auto_yaw_mode(AUTO_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 @@ -845,7 +845,7 @@ static void do_guided(const struct Location *cmd) wp_bearing = wp_nav.get_wp_bearing_to_destination(); // point nose at next waypoint if it is more than 10m away - if (yaw_mode == YAW_LOOK_AT_NEXT_WP) { + if (auto_yaw_mode == AUTO_YAW_LOOK_AT_NEXT_WP) { // get distance to new location wp_distance = wp_nav.get_wp_distance_to_destination(); // set original_wp_bearing to point at next waypoint @@ -911,7 +911,7 @@ static void do_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 ) { yaw_look_at_WP = pv_location_to_vector(command_cond_queue); - set_yaw_mode(YAW_LOOK_AT_LOCATION); + set_auto_yaw_mode(AUTO_YAW_LOOK_AT_LOCATION); } // send the command to the camera mount camera_mount.set_roi_cmd(&command_cond_queue); @@ -925,7 +925,7 @@ static void do_roi() #else // if we have no camera mount aim the quad at the location yaw_look_at_WP = pv_location_to_vector(command_cond_queue); - set_yaw_mode(YAW_LOOK_AT_LOCATION); + set_auto_yaw_mode(AUTO_YAW_LOOK_AT_LOCATION); #endif } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index bb3a80f4c5..9a9060cbe3 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -15,16 +15,16 @@ // Flight modes // ------------ -#define YAW_HOLD 0 // heading hold at heading in control_yaw but allow input from pilot +#define AUTO_YAW_HOLD 0 // pilot controls the heading +#define AUTO_YAW_LOOK_AT_NEXT_WP 1 // point towards next waypoint (no pilot input accepted) +#define AUTO_YAW_LOOK_AT_LOCATION 2 // point towards a location held in yaw_look_at_WP (no pilot input accepted) +#define AUTO_YAW_LOOK_AT_HEADING 3 // point towards a particular angle (not pilot input accepted) +#define AUTO_YAW_LOOK_AHEAD 4 // point in the direction the copter is moving +#define AUTO_YAW_RESETTOARMEDYAW 5 // point towards heading at time motors were armed + #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_CIRCLE 4 // point towards a location held in yaw_look_at_WP (no pilot input accepted) -#define YAW_LOOK_AT_HOME 5 // point towards home (no pilot input accepted) -#define YAW_LOOK_AT_HEADING 6 // point towards a particular angle (not pilot input accepted) -#define YAW_LOOK_AHEAD 7 // WARNING! CODE IN DEVELOPMENT NOT PROVEN #define YAW_DRIFT 8 // -#define YAW_RESETTOARMEDYAW 9 // point towards heading at time motors were armed #define ROLL_PITCH_STABLE 0 // pilot input roll, pitch angles #define ROLL_PITCH_ACRO 1 // pilot inputs roll, pitch rotation rates in body frame diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 6674f651c9..ff71ab2f6d 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -406,18 +406,12 @@ static bool set_mode(uint8_t mode) // check we have a GPS and at least one mission command (note the home position is always command 0) if ((GPS_ok() && g.command_total > 1) || ignore_checks) { success = auto_init(ignore_checks); - // roll-pitch, throttle and yaw modes will all be set by the first nav command - init_commands(); // clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function } break; case CIRCLE: if (GPS_ok() || ignore_checks) { success = circle_init(ignore_checks); - set_roll_pitch_mode(CIRCLE_RP); - set_throttle_mode(CIRCLE_THR); - set_nav_mode(CIRCLE_NAV); - set_yaw_mode(CIRCLE_YAW); } break; @@ -430,19 +424,19 @@ static bool set_mode(uint8_t mode) case POSITION: if (GPS_ok() || ignore_checks) { success = true; - set_yaw_mode(POSITION_YAW); - set_roll_pitch_mode(POSITION_RP); - set_throttle_mode(POSITION_THR); + //set_yaw_mode(POSITION_YAW); + //set_roll_pitch_mode(POSITION_RP); + //set_throttle_mode(POSITION_THR); } break; case GUIDED: if (GPS_ok() || ignore_checks) { success = guided_init(ignore_checks); - set_yaw_mode(get_wp_yaw_mode(false)); - set_roll_pitch_mode(GUIDED_RP); - set_throttle_mode(GUIDED_THR); - set_nav_mode(GUIDED_NAV); + //set_yaw_mode(get_wp_yaw_mode(false)); + //set_roll_pitch_mode(GUIDED_RP); + //set_throttle_mode(GUIDED_THR); + //set_nav_mode(GUIDED_NAV); } break; @@ -461,27 +455,27 @@ static bool set_mode(uint8_t mode) case OF_LOITER: if (g.optflow_enabled || ignore_checks) { success = ofloiter_init(ignore_checks); - set_yaw_mode(OF_LOITER_YAW); - set_roll_pitch_mode(OF_LOITER_RP); - set_throttle_mode(OF_LOITER_THR); - set_nav_mode(OF_LOITER_NAV); + //set_yaw_mode(OF_LOITER_YAW); + //set_roll_pitch_mode(OF_LOITER_RP); + //set_throttle_mode(OF_LOITER_THR); + //set_nav_mode(OF_LOITER_NAV); } break; case DRIFT: success = drift_init(ignore_checks); - set_yaw_mode(YAW_DRIFT); - set_roll_pitch_mode(ROLL_PITCH_DRIFT); - set_nav_mode(NAV_NONE); - set_throttle_mode(DRIFT_THR); + //set_yaw_mode(YAW_DRIFT); + //set_roll_pitch_mode(ROLL_PITCH_DRIFT); + //set_nav_mode(NAV_NONE); + //set_throttle_mode(DRIFT_THR); break; case SPORT: success = sport_init(ignore_checks); - set_yaw_mode(SPORT_YAW); - set_roll_pitch_mode(SPORT_RP); - set_throttle_mode(SPORT_THR); - set_nav_mode(NAV_NONE); + //set_yaw_mode(SPORT_YAW); + //set_roll_pitch_mode(SPORT_RP); + //set_throttle_mode(SPORT_THR); + //set_nav_mode(NAV_NONE); // reset acro angle targets to current attitude acro_roll = ahrs.roll_sensor; acro_pitch = ahrs.pitch_sensor;