From 1b714defcc782557933fcd8837324bf7dd417a32 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Jan 2014 14:14:58 +0900 Subject: [PATCH] Copter: move auto to control_auto.pde and fix yaw --- ArduCopter/control_auto.pde | 206 +++++++++++++++++++++++++++++++ ArduCopter/control_stabilize.pde | 94 +++----------- 2 files changed, 224 insertions(+), 76 deletions(-) create mode 100644 ArduCopter/control_auto.pde diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde new file mode 100644 index 0000000000..613b74c444 --- /dev/null +++ b/ArduCopter/control_auto.pde @@ -0,0 +1,206 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + * control_auto.pde - init and run calls for auto, guided, land and RTL flight modes + */ + +// auto_init - initialise auto controller +static bool auto_init(bool ignore_checks) +{ + if (GPS_ok() || ignore_checks) { + // set target to current position + wp_nav.init_loiter_target(); + // initialise auto_yaw_mode + set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + cliSerial->printf_P(PSTR("\nYM:%d\n"),(int)auto_yaw_mode); + // clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function + init_commands(); + return true; + }else{ + return false; + } +} + +// auto_run - runs the auto controller +// should be called at 100hz or more +// relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands +static void auto_run() +{ + float target_yaw_rate = 0; + + // if not auto armed set throttle to zero and exit immediately + if(!ap.auto_armed || !inertial_nav.position_ok()) { + wp_nav.init_loiter_target(); + attitude_control.init_targets(); + attitude_control.set_throttle_out(0, false); + return; + } + + // process pilot's yaw input + if (!failsafe.radio) { + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + if (target_yaw_rate != 0) { + set_auto_yaw_mode(AUTO_YAW_HOLD); + } + } + + // run waypoint controller + wp_nav.update_wpnav(); + + // call z-axis position controller (wpnav should have already updated it's alt target) + pos_control.update_z_controller(); + + // call attitude controller + if (auto_yaw_mode == AUTO_YAW_HOLD) { + // roll & pitch from waypoint controller, yaw rate from pilot + attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + }else{ + // roll, pitch from waypoint controller, yaw heading from auto_heading() + attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading()); + } + + // body-frame rate controller is run directly from 100hz loop + + // refetch angle targets for reporting + const Vector3f angle_target = attitude_control.angle_ef_targets(); + control_roll = angle_target.x; + control_pitch = angle_target.y; + control_yaw = angle_target.z; +} + +// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter +// set rtl parameter to true if this is during an RTL +uint8_t get_default_auto_yaw_mode(bool rtl) +{ + switch (g.wp_yaw_behavior) { + + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL: + if (rtl) { + return AUTO_YAW_HOLD; + }else{ + return AUTO_YAW_LOOK_AT_NEXT_WP; + } + break; + + case WP_YAW_BEHAVIOR_LOOK_AHEAD: + return AUTO_YAW_LOOK_AHEAD; + break; + + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: + default: + return AUTO_YAW_LOOK_AT_NEXT_WP; + break; + } +} + +// set_auto_yaw_mode - sets the yaw mode for auto +void set_auto_yaw_mode(uint8_t yaw_mode) +{ + // return immediately if no change + if (auto_yaw_mode == yaw_mode) { + return; + } + auto_yaw_mode = yaw_mode; + + // perform initialisation + switch (auto_yaw_mode) { + + case AUTO_YAW_LOOK_AT_NEXT_WP: + // original_wp_bearing will be set by do_nav_wp or other nav command initialisatoin functions so no init required + break; + + case AUTO_YAW_LOOK_AT_LOCATION: + // point towards a location held in yaw_look_at_WP + yaw_look_at_WP_bearing = ahrs.yaw_sensor; + break; + + case AUTO_YAW_LOOK_AT_HEADING: + // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed + yaw_look_at_heading = ahrs.yaw_sensor; + break; + + case AUTO_YAW_LOOK_AHEAD: + // Commanded Yaw to automatically look ahead. + yaw_look_ahead_bearing = ahrs.yaw_sensor; + break; + + case AUTO_YAW_RESETTOARMEDYAW: + // initial_armed_bearing will be set during arming so no init required + break; + } +} + +// get_auto_heading - returns target heading depending upon auto_yaw_mode +// 100hz update rate +float get_auto_heading(void) +{ + switch(auto_yaw_mode) { + + case AUTO_YAW_LOOK_AT_LOCATION: + // point towards a location held in yaw_look_at_WP + return get_look_at_yaw(); + break; + + case AUTO_YAW_LOOK_AT_HEADING: + // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed + return yaw_look_at_heading; + break; + + case AUTO_YAW_LOOK_AHEAD: + // Commanded Yaw to automatically look ahead. + return get_look_ahead_yaw(); + break; + + case AUTO_YAW_RESETTOARMEDYAW: + // changes yaw to be same as when quad was armed + control_yaw = initial_armed_bearing; + break; + + case AUTO_YAW_LOOK_AT_NEXT_WP: + default: + // point towards next waypoint. + // we don't use wp_bearing because we don't want the copter to turn too much during flight + return original_wp_bearing; + break; + } +} + +// guided_init - initialise guided controller +static bool guided_init(bool ignore_checks) +{ + return true; +} + +// guided_run - runs the guided controller +// should be called at 100hz or more +static void guided_run() +{ +} + +// land_init - initialise land controller +static bool land_init(bool ignore_checks) +{ + return true; +} + +// land_run - runs the land controller +// should be called at 100hz or more +static void land_run() +{ + verify_land(); +} + +// rtl_init - initialise rtl controller +static bool rtl_init(bool ignore_checks) +{ + return true; +} + +// rtl_run - runs the return-to-launch controller +// should be called at 100hz or more +static void rtl_run() +{ + verify_RTL(); +} + diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index 2518da4b9c..48d7e45ac3 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -30,6 +30,8 @@ static void acro_run() attitude_control.rate_stab_bf_to_rate_bf_roll(); attitude_control.rate_stab_bf_to_rate_bf_pitch(); attitude_control.rate_stab_bf_to_rate_bf_yaw(); + // pilot controlled yaw using rate controller + //get_yaw_rate_stabilized_bf(pilot_yaw); // call get_acro_level_rates() here? @@ -182,46 +184,14 @@ static void althold_run() control_yaw = angle_target.z; } -// auto_init - initialise auto controller -static bool auto_init(bool ignore_checks) -{ - return true; -} - -// auto_run - runs the auto controller -// should be called at 100hz or more -static void auto_run() -{ - Vector3f angle_target; - - // run way point controller - - // copy latest output from nav controller to stabilize controller - angle_target.x = wp_nav.get_roll(); - angle_target.y = wp_nav.get_pitch(); - - // To-Do: handle pilot input for yaw and different methods to update yaw (ROI, face next wp) - angle_target.z = control_yaw; - - // To-Do: shorten below by moving these often used steps into a single function in the AC_AttitudeControl lib - - // set earth-frame angular targets - attitude_control.angle_ef_targets(angle_target); - - // convert earth-frame angle targets to earth-frame rate targets - attitude_control.angle_to_rate_ef_roll(); - attitude_control.angle_to_rate_ef_pitch(); - attitude_control.angle_to_rate_ef_yaw(); - - // convert earth-frame rates to body-frame rates - attitude_control.rate_ef_targets_to_bf(); - - // body-frame rate controller is run directly from 100hz loop -} - // circle_init - initialise circle controller static bool circle_init(bool ignore_checks) { + // 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; return true; } @@ -229,6 +199,12 @@ static bool circle_init(bool ignore_checks) // should be called at 100hz or more static void circle_run() { + // 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(); } // loiter_init - initialise loiter controller @@ -236,7 +212,6 @@ static bool loiter_init(bool ignore_checks) { if (GPS_ok() || ignore_checks) { // set target to current position - // To-Do: supply zero velocity below? wp_nav.init_loiter_target(); return true; }else{ @@ -314,44 +289,6 @@ static void loiter_run() control_yaw = angle_target.z; } -// guided_init - initialise guided controller -static bool guided_init(bool ignore_checks) -{ - return true; -} - -// guided_run - runs the guided controller -// should be called at 100hz or more -static void guided_run() -{ -} - -// land_init - initialise land controller -static bool land_init(bool ignore_checks) -{ - return true; -} - -// land_run - runs the land controller -// should be called at 100hz or more -static void land_run() -{ - verify_land(); -} - -// rtl_init - initialise rtl controller -static bool rtl_init(bool ignore_checks) -{ - return true; -} - -// rtl_run - runs the return-to-launch controller -// should be called at 100hz or more -static void rtl_run() -{ - verify_RTL(); -} - // ofloiter_init - initialise ofloiter controller static bool ofloiter_init(bool ignore_checks) { @@ -374,6 +311,11 @@ static bool drift_init(bool ignore_checks) // should be called at 100hz or more static void drift_run() { + // if we have landed reset yaw target to current heading + //if (ap.land_complete) { + // control_yaw = ahrs.yaw_sensor; + //} + //get_yaw_drift(); } // sport_init - initialise sport controller