From 1516972eaa3d2a552407891dd557b8ad0a08f443 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 22 Mar 2013 17:38:07 +0900 Subject: [PATCH] Copter: add panorama to CIRCLE mode Yaw will slowly rotate if CIRCLE_RADIUS is set to zero Circle center is projected forward CIRCLE_RADIUS from current position and heading --- ArduCopter/ArduCopter.pde | 24 +++++++++++++++--- ArduCopter/Attitude.pde | 41 ++++++++++++++++++++++++++++++ ArduCopter/commands_logic.pde | 1 - ArduCopter/config.h | 2 +- ArduCopter/defines.h | 9 ++++--- ArduCopter/navigation.pde | 47 +++++++++++++++++++---------------- ArduCopter/system.pde | 3 +-- 7 files changed, 94 insertions(+), 33 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 981f2d5ce7..c4919fc1e6 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1442,6 +1442,15 @@ bool set_yaw_mode(uint8_t new_yaw_mode) 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; @@ -1502,9 +1511,18 @@ void update_yaw_mode(void) 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); + // 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( g.rc_4.control_in != 0 ) { + set_yaw_mode(YAW_HOLD); + } + break; + + case YAW_CIRCLE: + // 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( g.rc_4.control_in != 0 ) { diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index feece45c89..81f18e7c0c 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -682,6 +682,47 @@ get_of_pitch(int32_t input_pitch) * yaw controllers *************************************************************/ + // get_look_at_yaw - updates bearing to look at center of circle or do a panorama +// should be called at 100hz +static void get_circle_yaw() +{ + static uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz + + // if circle radius is zero do panorama + if( g.circle_radius == 0 ) { + // slew yaw towards circle angle + nav_yaw = get_yaw_slew(nav_yaw, ToDeg(circle_angle)*100, AUTO_YAW_SLEW_RATE); + }else{ + look_at_yaw_counter++; + 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 + nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE); + } + + // call stabilize yaw controller + get_stabilize_yaw(nav_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 uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz + + look_at_yaw_counter++; + 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 + nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE); + get_stabilize_yaw(nav_yaw); +} + static void get_look_ahead_yaw(int16_t pilot_yaw) { // Commanded Yaw to automatically look ahead. diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 510cfb1997..900bb67fa7 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -372,7 +372,6 @@ static void do_circle() } // set yaw to point to center of circle - yaw_look_at_WP = circle_center; set_yaw_mode(CIRCLE_YAW); // set angle travelled so far to zero diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 008c3370b0..ce7bc6ebdb 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -593,7 +593,7 @@ // CIRCLE Mode #ifndef CIRCLE_YAW - # define CIRCLE_YAW YAW_LOOK_AT_LOCATION + # define CIRCLE_YAW YAW_CIRCLE #endif #ifndef CIRCLE_RP diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 84843c1863..4c3bc14911 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -19,10 +19,11 @@ #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 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_TOY 8 // THOR This is the Yaw mode #define ROLL_PITCH_STABLE 0 // pilot input roll, pitch angles diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 3e0255e8ad..24d5ed6151 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -81,12 +81,6 @@ static void calc_distance_and_bearing() home_distance = 0; home_bearing = 0; } - - // calculate bearing to target (used when yaw_mode = YAW_LOOK_AT_LOCATION) - // To-Do: move this to the look-at-waypoint yaw controller - if( yaw_mode == YAW_LOOK_AT_LOCATION ) { - yaw_look_at_WP_bearing = pv_get_bearing_cd(curr, yaw_look_at_WP); - } } // run_autopilot - highest level call to process mission commands @@ -279,25 +273,31 @@ static bool waypoint_valid(Location &wp) // circle_set_center -- set circle controller's center position and starting angle static void -circle_set_center(const Vector3f pos_vec, float heading_in_radians) +circle_set_center(const Vector3f current_position, float heading_in_radians) { - // set circle center - circle_center = pos_vec; + // set circle center to circle_radius ahead of current position + circle_center.x = current_position.x + (float)g.circle_radius * 100 * sin_yaw_y; + circle_center.y = current_position.y + (float)g.circle_radius * 100 * cos_yaw_x; - // set starting angle to current heading - 180 degrees - circle_angle = heading_in_radians-ToRad(180); - if( circle_angle > 180 ) { - circle_angle -= 180; - } - if( circle_angle < -180 ) { - circle_angle -= 180; + // if we are doing a panorama set the circle_angle to the current heading + if( g.circle_radius == 0 ) { + circle_angle = heading_in_radians; + }else{ + // set starting angle to current heading - 180 degrees + circle_angle = heading_in_radians-ToRad(180); + if( circle_angle > 180 ) { + circle_angle -= 180; + } + if( circle_angle < -180 ) { + circle_angle -= 180; + } } // initialise other variables circle_angle_total = 0; } -// circle_get_pos - circle position controller's main call which in turn calls loiter controller with updated target position +// update_circle - circle position controller's main call which in turn calls loiter controller with updated target position static void update_circle(float dt) { @@ -317,12 +317,15 @@ update_circle(float dt) // update the total angle travelled circle_angle_total += angle_delta; - // calculate target position - circle_target.x = circle_center.x + cir_radius * sinf(1.57f - circle_angle); - circle_target.y = circle_center.y + cir_radius * cosf(1.57f - circle_angle); + // if the circle_radius is zero we are doing panorama so no need to update loiter target + if( g.circle_radius != 0.0 ) { + // calculate target position + circle_target.x = circle_center.x + cir_radius * sinf(1.57f - circle_angle); + circle_target.y = circle_center.y + cir_radius * cosf(1.57f - circle_angle); - // re-use loiter position controller - wp_nav.set_loiter_target(circle_target); + // re-use loiter position controller + wp_nav.set_loiter_target(circle_target); + } // call loiter controller wp_nav.update_loiter(); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index aeedc36373..56e82466bb 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -426,11 +426,10 @@ static void set_mode(uint8_t mode) case CIRCLE: ap.manual_throttle = false; ap.manual_attitude = false; - // set yaw to point to center of circle - set_yaw_mode(CIRCLE_YAW); set_roll_pitch_mode(CIRCLE_RP); set_throttle_mode(CIRCLE_THR); set_nav_mode(CIRCLE_NAV); + set_yaw_mode(CIRCLE_YAW); break; case LOITER: