From f6f5a0c8ddddd9291a82ec6b33473f1f662f054f Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 17 Apr 2011 23:08:16 +0000 Subject: [PATCH] Added rudimentary tuning with CH6. Assign this to your Flaps knob on your radio. in APM_config.h set your desired option, then upload. The knob will adjust the value in real time. Enter the CLI and run test, tune to see the output value. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1902 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 32 ++++++++++++++++++++++++ ArduCopterMega/config.h | 6 ++++- ArduCopterMega/defines.h | 11 +++++++++ ArduCopterMega/motors.pde | 39 +---------------------------- ArduCopterMega/radio.pde | 13 ++++++++++ ArduCopterMega/test.pde | 41 +++++++++++++++++++++++++++++++ 6 files changed, 103 insertions(+), 39 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index a069f0cb26..37868eca2a 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -679,6 +679,7 @@ void medium_loop() // stuff that happens at 50 hz // --------------------------- + // use Yaw to find our bearing error calc_bearing_error(); @@ -758,6 +759,9 @@ void slow_loop() gcs.send_message(MSG_LOCATION); #endif + #if CHANNEL_6_TUNING != CH6_NONE + tuning(); + #endif break; @@ -1169,3 +1173,31 @@ adjust_altitude() } } +void tuning(){ + + #if CHANNEL_6_TUNING == CH6_STABLIZE_KP + g.pid_stabilize_roll.kP((float)g.rc_6.control_in / 1000.0); + g.pid_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0); + init_pids(); + + #elif CHANNEL_6_TUNING == CH6_STABLIZE_KD + // uncomment me out to try in flight dampening, 0 = unflyable, .2 = unfun, .13 worked for me. + // use test,radio to get the value to use in your config. + g.stabilize_dampener.set((float)g.rc_6.control_in / 1000.0); + + #elif CHANNEL_6_TUNING == CH6_BARO_KP + g.pid_baro_throttle.kP((float)g.rc_6.control_in / 1000.0); + + #elif CHANNEL_6_TUNING == CH6_BARO_KD + g.pid_baro_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1 + + #elif CHANNEL_6_TUNING == CH6_SONAR_KP + g.pid_sonar_throttle.kP((float)g.rc_6.control_in / 1000.0); + + #elif CHANNEL_6_TUNING == CH6_SONAR_KD + g.pid_sonar_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1 + + #elif CHANNEL_6_TUNING == CH6_Y6_SCALING + g.Y6_scaling.set((float)g.rc_6.control_in / 1000.0) + #endif +} \ No newline at end of file diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index eda88d3feb..0b45003432 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -399,7 +399,7 @@ #ifndef THROTTLE_SONAR_P -# define THROTTLE_SONAR_P .3 +# define THROTTLE_SONAR_P .5 #endif #ifndef THROTTLE_SONAR_I # define THROTTLE_SONAR_I 0.1 @@ -437,6 +437,10 @@ #endif +#ifndef CHANNEL_6_TUNING +# define CHANNEL_6_TUNING CH6_NONE +#endif + ////////////////////////////////////////////////////////////////////////////// // Dataflash logging control // diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 2167c64cdf..a474642256 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -101,6 +101,17 @@ #define YAW_BRAKE 1 #define YAW_RATE 2 +// CH_6 Tuning +#define CH6_NONE 0 +#define CH6_STABLIZE_KP 1 +#define CH6_STABLIZE_KD 2 +#define CH6_BARO_KP 3 +#define CH6_BARO_KD 4 +#define CH6_SONAR_KP 5 +#define CH6_SONAR_KD 6 +#define CH6_Y6_SCALING 7 + + // Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library #define CMD_BLANK 0 // there is no command stored in the mem location requested diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 6e3ab4c055..a903df325a 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -202,50 +202,13 @@ set_servos_4() motor_out[CH_8] = constrain(motor_out[CH_8], out_min, g.rc_3.radio_max.get()); } - num++; - if (num > 25){ + if (num++ > 25){ num = 0; - /* - Serial.printf("t_alt:%ld, alt:%ld, thr: %d sen: ", - target_altitude, - current_loc.alt, - g.rc_3.servo_out); - if(altitude_sensor == BARO){ - Serial.println("Baro"); - }else{ - Serial.println("Sonar"); - } - */ - - //Serial.print("!"); - //debugging with Channel 6 - - //g.pid_baro_throttle.kD((float)g.rc_6.control_in / 1000); // 0 to 1 - //g.pid_baro_throttle.kP((float)g.rc_6.control_in / 4000); // 0 to .25 - - - // uncomment me out to try in flight dampening, 0 = unflyable, .2 = unfun, .13 worked for me. - // use test,radio to get the value to use in your config. - //g.stabilize_dampener.set((float)g.rc_6.control_in / 1000.0); - - - - /* - // ROLL and PITCH - // make sure you init_pids() after changing the kP - g.pid_stabilize_roll.kP((float)g.rc_6.control_in / 1000); - init_pids(); //Serial.print("kP: "); //Serial.println(g.pid_stabilize_roll.kP(),3); //*/ - /* - // YAW - // make sure you init_pids() after changing the kP - g.pid_yaw.kP((float)g.rc_6.control_in / 1000); - init_pids(); - //*/ /* Serial.printf("yaw: %d, lat_e: %ld, lng_e: %ld, \tnlat: %ld, nlng: %ld,\tnrll: %ld, nptc: %ld, \tcx: %.2f, sy: %.2f, \ttber: %ld, \tnber: %ld\n", diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 0aab28b225..2581de5140 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -29,6 +29,19 @@ void init_rc_in() g.rc_7.set_range(0,1000); g.rc_8.set_range(0,1000); + #if CHANNEL_6_TUNING == CH6_STABLIZE_KD + g.rc_6.set_range(0,300); + + #elif CHANNEL_6_TUNING == CH6_BARO_KP + g.rc_6.set_range(0,500); + + #elif CHANNEL_6_TUNING == CH6_BARO_KD + g.rc_6.set_range(0,500); + + #elif CHANNEL_6_TUNING == CH6_Y6_SCALING + g.rc_6.set_range(800,1000); + #endif + //catch bad RC_3 min values } diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index c91ba90b93..69fb595885 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -12,6 +12,7 @@ static int8_t test_imu(uint8_t argc, const Menu::arg *argv); //static int8_t test_dcm(uint8_t argc, const Menu::arg *argv); //static int8_t test_omega(uint8_t argc, const Menu::arg *argv); static int8_t test_battery(uint8_t argc, const Menu::arg *argv); +static int8_t test_tuning(uint8_t argc, const Menu::arg *argv); static int8_t test_current(uint8_t argc, const Menu::arg *argv); static int8_t test_relay(uint8_t argc, const Menu::arg *argv); static int8_t test_wp(uint8_t argc, const Menu::arg *argv); @@ -55,6 +56,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { //{"dcm", test_dcm}, //{"omega", test_omega}, {"battery", test_battery}, + {"tune", test_tuning}, {"current", test_current}, {"relay", test_relay}, {"waypoints", test_wp}, @@ -575,6 +577,45 @@ test_battery(uint8_t argc, const Menu::arg *argv) return (0); } +static int8_t +test_tuning(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + + while(1){ + delay(200); + read_radio(); + + #if CHANNEL_6_TUNING == CH6_NONE + Serial.printf_P(PSTR("disabled\n")); + + #elif CHANNEL_6_TUNING == CH6_STABLIZE_KP + Serial.printf_P(PSTR("stab kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); + + #elif CHANNEL_6_TUNING == CH6_STABLIZE_KD + Serial.printf_P(PSTR("stab kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); + + #elif CHANNEL_6_TUNING == CH6_BARO_KP + Serial.printf_P(PSTR("baro kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); + + #elif CHANNEL_6_TUNING == CH6_BARO_KD + Serial.printf_P(PSTR("baro kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); + + #elif CHANNEL_6_TUNING == CH6_SONAR_KP + Serial.printf_P(PSTR("sonar kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); + + #elif CHANNEL_6_TUNING == CH6_SONAR_KD + Serial.printf_P(PSTR("sonar kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); + + #elif CHANNEL_6_TUNING == CH6_Y6_SCALING + Serial.printf_P(PSTR("Y6: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); + #endif + if(Serial.available() > 0){ + return (0); + } + } +} + static int8_t test_current(uint8_t argc, const Menu::arg *argv) {