From 2118931cdc26428314de4c4abae02d25219ec9ba Mon Sep 17 00:00:00 2001 From: jasonshort Date: Tue, 24 May 2011 02:14:18 +0000 Subject: [PATCH] =?UTF-8?q?Added=20resetting=20of=20Loiter=20location=20ba?= =?UTF-8?q?sed=20on=20stick=20input.=20Fly=20it=20to=20a=20location=20and?= =?UTF-8?q?=20let=20it=20stick.=20Added=20Octo=20Support=20Fixed=20Loiter?= =?UTF-8?q?=20issue=20regarding=20Yaw=20towards=20loiter=20location=20Adde?= =?UTF-8?q?d=20Yaw=20control=20alternative=20for=20testing=20decreased=20Y?= =?UTF-8?q?aw=20deadband=20to=20+-5=C2=B0=20revved=20to=202.0.10?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit git-svn-id: https://arducopter.googlecode.com/svn/trunk@2389 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 2 ++ ArduCopterMega/ArduCopterMega.pde | 11 ++++++----- ArduCopterMega/Attitude.pde | 33 +++++++++++++++++++++++++++++++ ArduCopterMega/config.h | 4 ++-- ArduCopterMega/defines.h | 13 +++--------- ArduCopterMega/radio.pde | 8 +++++++- ArduCopterMega/setup.pde | 8 ++++++++ ArduCopterMega/system.pde | 7 +++++-- ArduCopterMega/test.pde | 8 ++++---- 9 files changed, 70 insertions(+), 24 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 523adf8d1d..d6052c433c 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -7,6 +7,7 @@ //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD #define NAV_TEST 1 // 0 = traditional, 1 = rate controlled +#define YAW_OPTION 0 // hybrid rate approach, offset Yaw approach #define FRAME_CONFIG QUAD_FRAME /* @@ -15,6 +16,7 @@ TRI_FRAME HEXA_FRAME Y6_FRAME + OCTA_FRAME */ #define FRAME_ORIENTATION X_FRAME diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index a2c7a70d69..ef5398d300 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -820,7 +820,7 @@ void slow_loop() #if MOTOR_LEDS == 1 - update_motor_light(); + update_motor_leds(); #endif break; @@ -1101,7 +1101,9 @@ void update_current_flight_mode(void) #if AUTO_RESET_LOITER == 1 if((g.rc_2.control_in + g.rc_1.control_in) != 0){ // reset LOITER to current position + long temp = next_WP.alt; next_WP = current_loc; + next_WP.alt = temp; } #endif @@ -1152,13 +1154,12 @@ void update_navigation() update_nav_wp(); break; - case LOITER: case RTL: - // are we Traversing or Loitering? - wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE; - // calculates desired Yaw update_nav_yaw(); + case LOITER: + // are we Traversing or Loitering? + wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE; // calculates the desired Roll and Pitch update_nav_wp(); diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 9996d60f61..b9765efbf0 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -202,6 +202,7 @@ clear_yaw_control() yaw_debug = YAW_HOLD; //0 } +#if YAW_OPTION == 0 void output_yaw_with_hold(boolean hold) { @@ -271,3 +272,35 @@ output_yaw_with_hold(boolean hold) //Serial.printf("%d\n",g.rc_4.servo_out); } +#elif YAW_OPTION == 1 + +void +output_yaw_with_hold(boolean hold) +{ + long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 + rate = constrain(rate, -36000, 36000); // limit to something fun! + int dampener = rate * g.pid_yaw.kD(); // 34377 * .175 = 6000 + + // set nav_yaw + or - the current location + nav_yaw = (long)g.rc_4.control_in + dcm.yaw_sensor; + + // we need to wrap our value so we can be 0 to 360 (*100) + nav_yaw = wrap_360(nav_yaw); + + // how far off is nav_yaw from our current yaw? + yaw_error = nav_yaw - dcm.yaw_sensor; + + // we need to wrap our value so we can be -180 to 180 (*100) + yaw_error = wrap_180(yaw_error); + + // limit the error we're feeding to the PID + yaw_error = constrain(yaw_error, -4000, 4000); // limit error to 60 degees + + // Apply PID and save the new angle back to RC_Channel + g.rc_4.servo_out = g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600 + + // add in yaw dampener + g.rc_4.servo_out -= constrain(dampener, -1600, 1600); + yaw_debug = YAW_HOLD; //0 +} +#endif diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 2d38595f15..630a615274 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -350,13 +350,13 @@ # define YAW_P 0.4 // increase for more aggressive Yaw Hold, decrease if it's bouncy #endif #ifndef YAW_I -# define YAW_I 0.00 // increased to .1 to try and get over user's steady state error caused by poor balance +# define YAW_I 0.000 // set to .0001 to try and get over user's steady state error caused by poor balance #endif #ifndef YAW_D # define YAW_D 0.05 // Trying a lower value to prevent odd behavior #endif #ifndef YAW_IMAX -# define YAW_IMAX 0 // Always 0 +# define YAW_IMAX 1 // degrees * 100 #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index c71cd0d32d..91ff2c8d47 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -1,16 +1,6 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// ACM: -// Motors -#define RIGHT CH_1 -#define LEFT CH_2 -#define FRONT CH_3 -#define BACK CH_4 -#define RIGHTFRONT CH_7 -#define LEFTBACK CH_8 -#define MAX_SERVO_OUTPUT 2700 - // active altitude sensor // ---------------------- #define SONAR 0 @@ -21,6 +11,7 @@ #define TRI_FRAME 1 #define HEXA_FRAME 2 #define Y6_FRAME 3 +#define OCTA_FRAME 4 #define PLUS_FRAME 0 #define X_FRAME 1 @@ -61,6 +52,8 @@ #define CH_6 5 #define CH_7 6 #define CH_8 7 +#define CH_10 9 //PB5 +#define CH_11 10 //PE3 #define CH_ROLL CH_1 #define CH_PITCH CH_2 diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 0d2ec6ae6e..3878861295 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -22,7 +22,7 @@ void init_rc_in() g.rc_1.dead_zone = 60; // 60 = .6 degrees g.rc_2.dead_zone = 60; g.rc_3.dead_zone = 60; - g.rc_4.dead_zone = 1000; + g.rc_4.dead_zone = 500; //set auxiliary ranges g.rc_5.set_range(0,1000); @@ -93,6 +93,12 @@ void output_min() APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + + #if FRAME_CONFIG == OCTA_FRAME + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + #endif + } void read_radio() { diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index e50ccd6a6f..5055d6f75b 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -232,6 +232,12 @@ init_esc() APM_RC.OutputCh(CH_4, g.rc_3.radio_in); APM_RC.OutputCh(CH_7, g.rc_3.radio_in); APM_RC.OutputCh(CH_8, g.rc_3.radio_in); + + #if FRAME_CONFIG == OCTA_FRAME + APM_RC.OutputCh(CH_10, g.rc_3.radio_in); + APM_RC.OutputCh(CH_11, g.rc_3.radio_in); + #endif + } } @@ -495,6 +501,8 @@ void report_frame() Serial.printf_P(PSTR("Hexa frame\n")); #elif FRAME_CONFIG == Y6_FRAME Serial.printf_P(PSTR("Y6 frame\n")); +#elif FRAME_CONFIG == OCTA_FRAME + Serial.printf_P(PSTR("Octa frame\n")); #endif if(g.frame_orientation == X_FRAME) diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 91a4f0844e..e70dc1b2b7 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "AC 2.0.9 Beta", main_menu_commands); +MENU(main_menu, "AC 2.0.10 Beta", main_menu_commands); void init_ardupilot() { @@ -234,6 +234,9 @@ void init_ardupilot() // --------------------------- //reset_control_switch(); + // init the Yaw Hold output + clear_yaw_control(); + // Logging: // -------- if(g.log_bitmask != 0){ @@ -390,7 +393,7 @@ void set_failsafe(boolean mode) } #if MOTOR_LEDS == 1 -void update_motor_light(void) +void update_motor_leds(void) { // blink rear static bool blink; diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index a16c25ac4d..c363704b13 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -978,8 +978,8 @@ void fake_out_gps() void print_motor_out(){ Serial.printf("out: R: %d, L: %d F: %d B: %d\n", - (motor_out[RIGHT] - g.rc_3.radio_min), - (motor_out[LEFT] - g.rc_3.radio_min), - (motor_out[FRONT] - g.rc_3.radio_min), - (motor_out[BACK] - g.rc_3.radio_min)); + (motor_out[CH_1] - g.rc_3.radio_min), + (motor_out[CH_2] - g.rc_3.radio_min), + (motor_out[CH_3] - g.rc_3.radio_min), + (motor_out[CH_4] - g.rc_3.radio_min)); }