From 659b52091248d81fe1811e4ae9f2a176b44394dd Mon Sep 17 00:00:00 2001 From: jasonshort Date: Mon, 4 Jul 2011 06:37:29 +0000 Subject: [PATCH] AC 2.0.33 Beta Slighted tuned down D term in Loiter PIDs after testing, tuned up P slightly fixed alt hold bug cause when gaining 3D lock in alt hold. Mavlink now reports correct throttle scaling value in mavlink git-svn-id: https://arducopter.googlecode.com/svn/trunk@2745 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 11 ++++++----- ArduCopterMega/Mavlink_Common.h | 2 +- ArduCopterMega/Parameters.h | 7 +++---- ArduCopterMega/commands.pde | 2 +- ArduCopterMega/config.h | 8 ++++---- ArduCopterMega/system.pde | 2 +- 6 files changed, 16 insertions(+), 16 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 20cbf06655..3d5424492c 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -852,11 +852,11 @@ void slow_loop() #if AUTO_RESET_LOITER == 1 if(control_mode == LOITER){ - if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500){ + if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){ // reset LOITER to current position - long temp = next_WP.alt; + //long temp = next_WP.alt; next_WP = current_loc; - next_WP.alt = temp; + //next_WP.alt = temp; } } #endif @@ -981,13 +981,14 @@ void update_GPS(void) init_home(); // init altitude - current_loc.alt = home.alt; + // commented out because we aren't using absolute altitude + // current_loc.alt = home.alt; ground_start_count = 0; } } current_loc.lng = g_gps->longitude; // Lon * 10 * *7 - current_loc.lat = g_gps->latitude; // Lat * 10 * *7 + current_loc.lat = g_gps->latitude; // Lat * 10 * *7 if (g.log_bitmask & MASK_LOG_GPS){ Log_Write_GPS(); diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 4a41929bee..1f5167ab59 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -214,7 +214,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui (float)airspeed / 100.0, (float)g_gps->ground_speed / 100.0, (dcm.yaw_sensor / 100) % 360, - g.rc_3.servo_out, + g.rc_3.servo_out/10, current_loc.alt / 100.0, climb_rate); break; diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index a7e94dbd37..5cf6a69118 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -262,8 +262,8 @@ public: sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")), sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")), - crosstrack_gain (XTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN")), - crosstrack_entry_angle (XTRACK_ENTRY_ANGLE * 100, k_param_crosstrack_entry_angle, PSTR("XTRACK_ANGLE")), + crosstrack_gain (XTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), + crosstrack_entry_angle (XTRACK_ENTRY_ANGLE * 100, k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")), sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")), battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), @@ -275,7 +275,7 @@ public: waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")), waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), - loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("LOITER_RADIUS")), + loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")), throttle_max (1000, k_param_throttle_max, PSTR("THR_MAX")), @@ -291,7 +291,6 @@ public: log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")), RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")), - frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), #if FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index fc0cb460af..cc025f9453 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -233,7 +233,7 @@ void init_home() print_wp(&home, 0); // Save prev loc this makes the calcs look better before commands are loaded - next_WP = prev_WP = home; + prev_WP = home; } diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index a3dc1d46a1..feef7f1ed7 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -403,16 +403,16 @@ // Navigation control gains // #ifndef NAV_LOITER_P -# define NAV_LOITER_P 1.2 // upped to be a bit more aggressive +# define NAV_LOITER_P 1.4 // #endif #ifndef NAV_LOITER_I -# define NAV_LOITER_I 0.08 // upped a bit to deal with wind faster +# define NAV_LOITER_I 0.1 // #endif #ifndef NAV_LOITER_D -# define NAV_LOITER_D 0.8 // +# define NAV_LOITER_D 0.4 // #endif #ifndef NAV_LOITER_IMAX -# define NAV_LOITER_IMAX 15 // 20° +# define NAV_LOITER_IMAX 15 // degrees° #endif diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 6e91221792..b2f31f4554 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -40,7 +40,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "AC 2.0.32 Beta", main_menu_commands); +MENU(main_menu, "AC 2.0.33 Beta", main_menu_commands); void init_ardupilot() {