From 1b46daa12e31caf7943d5a950c881c1aa0b7523a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 18 Feb 2012 14:23:21 -0800 Subject: [PATCH 01/32] some better defaults for JDrones --- ArduCopter/config.h | 37 ++++++++++++++++++++++--------------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index aecf13b8fd..bdee40b2cc 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -554,20 +554,23 @@ #endif -#ifndef STABILIZE_D -# define STABILIZE_D .2 -#endif - - - #ifndef ACRO_P # define ACRO_P 4.5 #endif +#ifndef AXIS_LOCK_ENABLED +# define AXIS_LOCK_ENABLED DISABLED +#endif + +#ifndef AXIS_LOCK_P +# define AXIS_LOCK_P .02 +#endif + + // Good for smaller payload motors. #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 4 +# define STABILIZE_ROLL_P 4.5 #endif #ifndef STABILIZE_ROLL_I # define STABILIZE_ROLL_I 0.0 @@ -577,7 +580,7 @@ #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 4 +# define STABILIZE_PITCH_P 4.5 #endif #ifndef STABILIZE_PITCH_I # define STABILIZE_PITCH_I 0.0 @@ -601,26 +604,26 @@ // Stabilize Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P 0.12 +# define RATE_ROLL_P 0.14 #endif #ifndef RATE_ROLL_I -# define RATE_ROLL_I 0.02 +# define RATE_ROLL_I 0.0 #endif #ifndef RATE_ROLL_D -# define RATE_ROLL_D 0.008 +# define RATE_ROLL_D 0.002 #endif #ifndef RATE_ROLL_IMAX # define RATE_ROLL_IMAX 5 // degrees #endif #ifndef RATE_PITCH_P -# define RATE_PITCH_P 0.12 +# define RATE_PITCH_P 0.14 #endif #ifndef RATE_PITCH_I -# define RATE_PITCH_I 0.02 +# define RATE_PITCH_I 0.0 #endif #ifndef RATE_PITCH_D -# define RATE_PITCH_D 0.008 +# define RATE_PITCH_D 0.002 #endif #ifndef RATE_PITCH_IMAX # define RATE_PITCH_IMAX 5 // degrees @@ -633,13 +636,17 @@ # define RATE_YAW_I 0.0 #endif #ifndef RATE_YAW_D -# define RATE_YAW_D .002 +# define RATE_YAW_D .000 #endif #ifndef RATE_YAW_IMAX # define RATE_YAW_IMAX 50 // degrees #endif +#ifndef STABILIZE_D +# define STABILIZE_D 0.05 +#endif + ////////////////////////////////////////////////////////////////////////////// // Loiter control gains // From 1e37a0ec172ba62d4205a5a3745159bfa1814fa7 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 18 Feb 2012 21:08:17 -0800 Subject: [PATCH 02/32] added a limit to range output --- ArduCopter/Parameters.pde | 2 ++ libraries/RC_Channel/RC_Channel.cpp | 8 +++++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index aa1adcb440..a96133c922 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -111,6 +111,8 @@ static const AP_Param::Info var_info[] PROGMEM = { GSCALAR(camera_roll_gain, "CAM_R_G"), GSCALAR(stabilize_d, "STAB_D"), GSCALAR(acro_p, "ACRO_P"), + GSCALAR(axis_lock_p, "AXIS_P"), + GSCALAR(axis_enabled, "AXIS_ENABLE"), // PID controller //--------------- diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 9258928871..8ae480719e 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -108,6 +108,8 @@ RC_Channel::set_pwm(int pwm) if(_type == RC_CHANNEL_RANGE){ //Serial.print("range "); control_in = pwm_to_range(); + //control_in = constrain(control_in, _low, _high); + control_in = min(control_in, _high); control_in = (control_in < _dead_zone) ? 0 : control_in; if (fabs(scale_output) != 1){ @@ -211,9 +213,9 @@ RC_Channel::pwm_to_angle() { int radio_trim_high = radio_trim + _dead_zone; int radio_trim_low = radio_trim - _dead_zone; - + // prevent div by 0 - if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0) + if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0) return 0; if(radio_in > radio_trim_high){ @@ -269,7 +271,7 @@ float RC_Channel::norm_output() { int16_t mid = (radio_max + radio_min) / 2; - + if(radio_out < mid) return (float)(radio_out - mid) / (float)(mid - radio_min); else From ff2b4ecb7afaa6601d87515f8e9cb5979cd5a693 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 18 Feb 2012 21:11:06 -0800 Subject: [PATCH 03/32] updated throttle range --- ArduCopter/radio.pde | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 544e43f765..d119b3e76a 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -22,10 +22,7 @@ static void init_rc_in() // set rc channel ranges g.rc_1.set_angle(4500); g.rc_2.set_angle(4500); - g.rc_3.set_range(0, MAXIMUM_THROTTLE); - #if FRAME_CONFIG != HELI_FRAME - g.rc_3.scale_output = .9; - #endif + g.rc_3.set_range(MINIMUM_THROTTLE, MAXIMUM_THROTTLE); g.rc_4.set_angle(4500); // reverse: CW = left @@ -69,7 +66,7 @@ static void init_rc_out() } // we are full throttle - if(g.rc_3.control_in == 800){ + if(g.rc_3.control_in >= (MAXIMUM_THROTTLE - 50)){ if(g.esc_calibrate == 0){ // we will enter esc_calibrate mode on next reboot g.esc_calibrate.set_and_save(1); @@ -138,7 +135,7 @@ static void read_radio() #if FRAME_CONFIG != HELI_FRAME // limit our input to 800 so we can still pitch and roll - g.rc_3.control_in = min(g.rc_3.control_in, 800); + g.rc_3.control_in = min(g.rc_3.control_in, MAXIMUM_THROTTLE); #endif throttle_failsafe(g.rc_3.radio_in); From 04b70ffdc2fd68ad5a713a1aac987ff58787e34c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 18 Feb 2012 21:13:06 -0800 Subject: [PATCH 04/32] added Axis_Lock params --- ArduCopter/Parameters.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index b880130717..ea7f91f255 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -110,6 +110,7 @@ public: k_param_sonar_type, k_param_super_simple, //155 k_param_rtl_land_enabled, + k_param_axis_enabled, // // 160: Navigation parameters @@ -172,6 +173,7 @@ public: // k_param_stabilize_d = 220, k_param_acro_p, + k_param_axis_lock_p, k_param_pid_rate_roll, k_param_pid_rate_pitch, k_param_pid_rate_yaw, @@ -215,6 +217,8 @@ public: AP_Float low_voltage; AP_Int8 super_simple; AP_Int8 rtl_land_enabled; + AP_Int8 axis_enabled; + // Waypoints @@ -295,6 +299,7 @@ public: // PI/D controllers AP_Float acro_p; + AP_Float axis_lock_p; AC_PID pid_rate_roll; AC_PID pid_rate_pitch; @@ -339,6 +344,7 @@ public: low_voltage (LOW_VOLTAGE), super_simple (SUPER_SIMPLE), rtl_land_enabled (RTL_AUTO_LAND), + axis_enabled (AXIS_LOCK_ENABLED), waypoint_mode (0), command_total (0), @@ -397,6 +403,7 @@ public: camera_roll_gain (CAM_ROLL_GAIN), stabilize_d (STABILIZE_D), acro_p (ACRO_P), + axis_lock_p (AXIS_LOCK_P), // PID controller initial P initial I initial D initial imax //----------------------------------------------------------------------------------------------------- From 05024cc06c9c984f9ff75de69d9d0005255461af Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 18 Feb 2012 21:13:28 -0800 Subject: [PATCH 05/32] Change 800 to a define --- ArduCopter/motors_octa.pde | 2 +- ArduCopter/motors_octa_quad.pde | 2 +- ArduCopter/motors_quad.pde | 2 +- ArduCopter/motors_tri.pde | 2 +- ArduCopter/motors_y6.pde | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index 402d60da46..475c0a6768 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -29,7 +29,7 @@ static void output_motors_armed() int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 800); + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, MAXIMUM_THROTTLE); if(g.rc_3.servo_out > 0) out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index b8566f7f8e..0d22507c66 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -29,7 +29,7 @@ static void output_motors_armed() int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 800); + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, MAXIMUM_THROTTLE); if(g.rc_3.servo_out > 0) out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde index eb29581ea3..418da33d6b 100644 --- a/ArduCopter/motors_quad.pde +++ b/ArduCopter/motors_quad.pde @@ -24,7 +24,7 @@ static void output_motors_armed() int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 800); + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, MAXIMUM_THROTTLE); if(g.rc_3.servo_out > 0) out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; diff --git a/ArduCopter/motors_tri.pde b/ArduCopter/motors_tri.pde index 037ea1dc4f..b34fda09b5 100644 --- a/ArduCopter/motors_tri.pde +++ b/ArduCopter/motors_tri.pde @@ -23,7 +23,7 @@ static void output_motors_armed() int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 800); + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, MAXIMUM_THROTTLE); if(g.rc_3.servo_out > 0) out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; diff --git a/ArduCopter/motors_y6.pde b/ArduCopter/motors_y6.pde index c18c854fad..1c91b1fa70 100644 --- a/ArduCopter/motors_y6.pde +++ b/ArduCopter/motors_y6.pde @@ -28,7 +28,7 @@ static void output_motors_armed() int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 800); + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, MAXIMUM_THROTTLE); if(g.rc_3.servo_out > 0) out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; From 885710fce6cf14316a976fe857dccbef970267e9 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 18 Feb 2012 21:14:13 -0800 Subject: [PATCH 06/32] updated max throttle define --- ArduCopter/config.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index bdee40b2cc..6d79563a19 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -398,7 +398,7 @@ # define MINIMUM_THROTTLE 130 #endif #ifndef MAXIMUM_THROTTLE -# define MAXIMUM_THROTTLE 1000 +# define MAXIMUM_THROTTLE 850 #endif #ifndef AUTO_LAND_TIME @@ -697,8 +697,6 @@ #endif - - #ifndef WAYPOINT_SPEED_MAX # define WAYPOINT_SPEED_MAX 600 // 6m/s error = 13mph #endif From 028de5d2c5a062dd11869a9433ff7c3507beb621 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 18 Feb 2012 21:16:19 -0800 Subject: [PATCH 07/32] Axis lock beta (off by default) --- ArduCopter/ArduCopter.pde | 30 +++++++++++++++++++++++++++--- 1 file changed, 27 insertions(+), 3 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 21dfd0d919..3c05f4bc06 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -489,6 +489,12 @@ static float sin_pitch_y, sin_yaw_y, sin_roll_y; // or in SuperSimple mode when the copter leaves a 20m radius from home. static int32_t initial_simple_bearing; +//////////////////////////////////////////////////////////////////////////////// +// ACRO Mode +//////////////////////////////////////////////////////////////////////////////// +// Used to control Axis lock +int32_t roll_axis; +int32_t pitch_axis; //////////////////////////////////////////////////////////////////////////////// // Circle Mode / Loiter control @@ -1407,9 +1413,27 @@ void update_roll_pitch_mode(void) switch(roll_pitch_mode){ case ROLL_PITCH_ACRO: - // ACRO does not get SIMPLE mode ability - g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); - g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); + if(g.axis_enabled){ + roll_axis += (float)g.rc_1.control_in * g.axis_lock_p; + pitch_axis += (float)g.rc_2.control_in * g.axis_lock_p; + + roll_axis = wrap_360(roll_axis); + pitch_axis = wrap_360(pitch_axis); + + // in this mode, nav_roll and nav_pitch = the iterm + g.rc_1.servo_out = get_stabilize_roll(roll_axis); + g.rc_2.servo_out = get_stabilize_pitch(pitch_axis); + + if (g.rc_3.control_in == 0){ + roll_axis = 0; + pitch_axis = 0; + } + + }else{ + // ACRO does not get SIMPLE mode ability + g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); + g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); + } break; case ROLL_PITCH_STABLE: From 12327eb2a93d47d3ab20bcc1dc31718aefcccad7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Feb 2012 16:33:38 +1100 Subject: [PATCH 08/32] Heli: fixed some 'used before initialised' warnings --- ArduCopter/setup.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 4fe6e85be9..367761968f 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -457,7 +457,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv) int value = 0; int temp; int state = 0; // 0 = set rev+pos, 1 = capture min/max - int max_roll, max_pitch, min_collective, max_collective, min_tail, max_tail; + int max_roll=0, max_pitch=0, min_collective=0, max_collective=0, min_tail=0, max_tail=0; // initialise swash plate heli_init_swash(); From a0d4e940ef98bce4c1aefc2d14eb891b5c0cc656 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Feb 2012 16:34:37 +1100 Subject: [PATCH 09/32] Heli: added heli build to build_all.sh script ensure heli builds are tested --- Tools/scripts/build_all.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/scripts/build_all.sh b/Tools/scripts/build_all.sh index b61b6ef181..a1e28baa09 100755 --- a/Tools/scripts/build_all.sh +++ b/Tools/scripts/build_all.sh @@ -15,7 +15,7 @@ done popd pushd ArduCopter -for b in all apm2 apm2beta hil sitl; do +for b in all apm2 apm2beta hil sitl heli; do pwd make clean make $b From 53b87229ff36c2f71d74f01f545963273ee0def0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Feb 2012 16:57:49 +1100 Subject: [PATCH 10/32] AP_Param: added ParamToken type for variable list traversal --- libraries/AP_Common/AP_Param.cpp | 23 +++++++++++++---------- libraries/AP_Common/AP_Param.h | 14 ++++++++++---- 2 files changed, 23 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Common/AP_Param.cpp b/libraries/AP_Common/AP_Param.cpp index 17c2c28ada..7d07f227da 100644 --- a/libraries/AP_Common/AP_Param.cpp +++ b/libraries/AP_Common/AP_Param.cpp @@ -575,9 +575,10 @@ bool AP_Param::load_all(void) // return the first variable in _var_info -AP_Param *AP_Param::first(uint16_t *token, enum ap_var_type *ptype) +AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype) { - *token = 0; + token->key = 0; + token->group_element = 0; if (_num_vars == 0) { return NULL; } @@ -593,7 +594,7 @@ AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_inf bool *found_current, uint8_t group_base, uint8_t group_shift, - uint16_t *token, + ParamToken *token, enum ap_var_type *ptype) { uint8_t type; @@ -612,13 +613,14 @@ AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_inf } else { if (*found_current) { // got a new one - (*token) = ((uint16_t)GROUP_ID(group_info, group_base, i, group_shift)<<8) | vindex; + token->key = vindex; + token->group_element = GROUP_ID(group_info, group_base, i, group_shift); if (ptype != NULL) { *ptype = (enum ap_var_type)type; } return (AP_Param*)(PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset)); } - if (GROUP_ID(group_info, group_base, i, group_shift) == (uint8_t)((*token)>>8)) { + if (GROUP_ID(group_info, group_base, i, group_shift) == token->group_element) { *found_current = true; } } @@ -628,9 +630,9 @@ AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_inf /// Returns the next variable in _var_info, recursing into groups /// as needed -AP_Param *AP_Param::next(uint16_t *token, enum ap_var_type *ptype) +AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype) { - uint8_t i = (*token)&0xFF; + uint8_t i = token->key; bool found_current = false; if (i >= _num_vars) { // illegal token @@ -651,7 +653,8 @@ AP_Param *AP_Param::next(uint16_t *token, enum ap_var_type *ptype) } } else { // found the next one - (*token) = i; + token->key = i; + token->group_element = 0; if (ptype != NULL) { *ptype = (enum ap_var_type)type; } @@ -663,7 +666,7 @@ AP_Param *AP_Param::next(uint16_t *token, enum ap_var_type *ptype) /// Returns the next scalar in _var_info, recursing into groups /// as needed -AP_Param *AP_Param::next_scalar(uint16_t *token, enum ap_var_type *ptype) +AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype) { AP_Param *ap; enum ap_var_type type; @@ -696,7 +699,7 @@ float AP_Param::cast_to_float(enum ap_var_type type) // print the value of all variables void AP_Param::show_all(void) { - uint16_t token; + ParamToken token; AP_Param *ap; enum ap_var_type type; diff --git a/libraries/AP_Common/AP_Param.h b/libraries/AP_Common/AP_Param.h index b898a8a297..82e234a42f 100644 --- a/libraries/AP_Common/AP_Param.h +++ b/libraries/AP_Common/AP_Param.h @@ -74,6 +74,12 @@ public: const struct GroupInfo *group_info; }; + // a token used for first()/next() state + typedef struct { + uint8_t key; + uint8_t group_element; + } ParamToken; + // called once at startup to setup the _var_info[] table. This // will also check the EEPROM header and re-initialise it if the // wrong version is found @@ -138,15 +144,15 @@ public: /// @return The first variable in _var_info, or NULL if /// there are none. /// - static AP_Param *first(uint16_t *token, enum ap_var_type *ptype); + static AP_Param *first(ParamToken *token, enum ap_var_type *ptype); /// Returns the next variable in _var_info, recursing into groups /// as needed - static AP_Param *next(uint16_t *token, enum ap_var_type *ptype); + static AP_Param *next(ParamToken *token, enum ap_var_type *ptype); /// Returns the next scalar variable in _var_info, recursing into groups /// as needed - static AP_Param *next_scalar(uint16_t *token, enum ap_var_type *ptype); + static AP_Param *next_scalar(ParamToken *token, enum ap_var_type *ptype); /// cast a variable to a float given its type float cast_to_float(enum ap_var_type type); @@ -203,7 +209,7 @@ private: bool *found_current, uint8_t group_base, uint8_t group_shift, - uint16_t *token, + ParamToken *token, enum ap_var_type *ptype); static uint16_t _eeprom_size; From df8a557630a6b72ab3d31dd9f87b5e01e8757260 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Feb 2012 16:58:08 +1100 Subject: [PATCH 11/32] AP_Param: update users of AP_Param for ParamToken --- ArduCopter/GCS.h | 2 +- ArduCopter/GCS_Mavlink.pde | 2 +- ArduPlane/GCS.h | 2 +- ArduPlane/GCS_Mavlink.pde | 2 +- Tools/VARTest/VARTest.pde | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h index d378fcd8db..7047beaf09 100644 --- a/ArduCopter/GCS.h +++ b/ArduCopter/GCS.h @@ -127,7 +127,7 @@ private: /// AP_Param *_queued_parameter; ///< next parameter to be sent in queue enum ap_var_type _queued_parameter_type; ///< type of the next parameter - uint16_t _queued_parameter_token; ///AP_Param token for next() call + AP_Param::ParamToken _queued_parameter_token;///AP_Param token for next() call uint16_t _queued_parameter_index; ///< next queued parameter's index uint16_t _queued_parameter_count; ///< saved count of parameters for queued send diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 50ddf2febb..995ebaa9e4 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1617,7 +1617,7 @@ GCS_MAVLINK::_count_parameters() // if we haven't cached the parameter count yet... if (0 == _parameter_count) { AP_Param *vp; - uint16_t token; + AP_Param::ParamToken token; vp = AP_Param::first(&token, NULL); do { diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index d378fcd8db..7047beaf09 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -127,7 +127,7 @@ private: /// AP_Param *_queued_parameter; ///< next parameter to be sent in queue enum ap_var_type _queued_parameter_type; ///< type of the next parameter - uint16_t _queued_parameter_token; ///AP_Param token for next() call + AP_Param::ParamToken _queued_parameter_token;///AP_Param token for next() call uint16_t _queued_parameter_index; ///< next queued parameter's index uint16_t _queued_parameter_count; ///< saved count of parameters for queued send diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index e5ecb7cd9c..a301b9fa2c 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1970,7 +1970,7 @@ GCS_MAVLINK::_count_parameters() // if we haven't cached the parameter count yet... if (0 == _parameter_count) { AP_Param *vp; - uint16_t token; + AP_Param::ParamToken token; vp = AP_Param::first(&token, NULL); do { diff --git a/Tools/VARTest/VARTest.pde b/Tools/VARTest/VARTest.pde index dd98f0be55..1778b73a81 100644 --- a/Tools/VARTest/VARTest.pde +++ b/Tools/VARTest/VARTest.pde @@ -106,7 +106,7 @@ void setup() { compass.save_offsets(); // full testing of all variables - uint16_t token; + AP_Param::ParamToken token; for (AP_Param *ap = AP_Param::first(&token, &type); ap; ap=AP_Param::next(&token, &type)) { From 08d730c266e452e7051fa5a265440e161ac52dac Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Feb 2012 16:58:25 +1100 Subject: [PATCH 12/32] scripts: added some more builds to build_all.sh --- Tools/scripts/build_all.sh | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/Tools/scripts/build_all.sh b/Tools/scripts/build_all.sh index a1e28baa09..ee92b33505 100755 --- a/Tools/scripts/build_all.sh +++ b/Tools/scripts/build_all.sh @@ -6,6 +6,7 @@ set -e set -x +echo "Testing ArduPlane build" pushd ArduPlane for b in all apm2 apm2beta hil hilsensors mavlink10 sitl; do pwd @@ -14,6 +15,7 @@ for b in all apm2 apm2beta hil hilsensors mavlink10 sitl; do done popd +echo "Testing ArduCopter build" pushd ArduCopter for b in all apm2 apm2beta hil sitl heli; do pwd @@ -21,3 +23,13 @@ for b in all apm2 apm2beta hil sitl heli; do make $b done popd + +echo "Testing build of examples" + +examples="Tools/VARTest Tools/CPUInfo" +for d in $examples; do + pushd $d + make clean + make + popd +done From 66b95c14c5f63797923f54beb2f84660c6dfe29d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Feb 2012 17:05:21 +1100 Subject: [PATCH 13/32] autotest: added build_all.sh to autotest --- Tools/autotest/autotest.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 17cc5bf9b6..1887fa4181 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -67,6 +67,16 @@ def dump_logs(atype): print("Saved log for %s to %s" % (atype, logfile)) return True + +def build_all(): + '''run the build_all.sh script''' + print("Running build_all.sh") + if not util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh')): + print("Failed build_all.sh") + return False + return True + + def convert_gpx(): '''convert any mavlog files to GPX and KML''' import glob @@ -108,6 +118,7 @@ steps = [ 'prerequesites', 'build1280.ArduPlane', 'build2560.ArduPlane', + 'build.All', 'build.ArduPlane', 'defaults.ArduPlane', 'fly.ArduPlane', @@ -171,6 +182,9 @@ def run_step(step): if step == 'fly.ArduPlane': return arduplane.fly_ArduPlane(viewerip=opts.viewerip) + if step == 'build.All': + return build_all() + if step == 'convertgpx': return convert_gpx() From e146f225b1d931fbe26a6a5cc89a48357f961cd9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 19 Feb 2012 15:38:40 +0900 Subject: [PATCH 14/32] ArduCopter - remove unnecessary block for next GPS value before saving home location --- ArduCopter/ArduCopter.pde | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 3c05f4bc06..c7534e4413 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1339,14 +1339,7 @@ static void update_GPS(void) ground_start_count = 5; }else{ - // block until we get a good fix - // ----------------------------- - while (!g_gps->new_data || !g_gps->fix) { - g_gps->update(); - // we need GCS update while waiting for GPS, to ensure - // we react to HIL mavlink - gcs_update(); - } + // save home to eeprom (we must have a good fix to have reached this point) init_home(); ground_start_count = 0; } From 9700afaf102e15ee2e8395b33913e96bbc923861 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Feb 2012 22:51:52 +1100 Subject: [PATCH 15/32] autotest: fixed failsafe test increased timeout, added check for home location, and fixed failure testing on timeout --- Tools/autotest/arducopter.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index bfe1930485..68b6c617a9 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -193,15 +193,17 @@ def fly_failsafe(mavproxy, mav, side=60): print("# Enter Failsafe") mavproxy.send('rc 3 900\n') tstart = time.time() - while time.time() < tstart + 120: + while time.time() < tstart + 250: m = mav.recv_match(type='VFR_HUD', blocking=True) pos = current_location(mav) - #delta = get_distance(start, pos) - print("Alt: %u" % m.alt) - if(m.alt <= 1): + home_distance = get_distance(HOME, pos) + print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) + if m.alt <= 1 and home_distance < 10: + print("Reached failsafe home OK") mavproxy.send('rc 3 1100\n') return True - return True + print("Failed to land on failsafe RTL - timed out after 120 seconds") + return False def fly_simple(mavproxy, mav, side=60, timeout=120): From 4a766e1abd387d6916cb5a56fa468db65c720973 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Feb 2012 23:21:00 +1100 Subject: [PATCH 16/32] autotest: lower throttle level for lowering altitude in loiter we were not dropping altitude in the change_alt test --- Tools/autotest/arducopter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 68b6c617a9..bfe4b54dfe 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -90,7 +90,7 @@ def change_alt(mavproxy, mav, alt_min): wait_altitude(mav, alt_min, (alt_min + 5)) else: print("Lower to alt:%u from %u" % (alt_min, m.alt)) - mavproxy.send('rc 3 1100\n') + mavproxy.send('rc 3 1070\n') wait_altitude(mav, (alt_min -5), alt_min) mavproxy.send('rc 3 1430\n') return True From 6cf7f45f2e609e007d74013ba3269b019049a302 Mon Sep 17 00:00:00 2001 From: ellissu Date: Sun, 19 Feb 2012 21:36:04 +0800 Subject: [PATCH 17/32] Edit and Add some Traditional Chinese translation --- .../GCSViews/FlightPlanner.zh-TW.resx | 97 ++++++++----------- 1 file changed, 42 insertions(+), 55 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.zh-TW.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.zh-TW.resx index be4ff4cb18..2fb779ae23 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.zh-TW.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.zh-TW.resx @@ -151,6 +151,15 @@ 參數4 + + 緯度 + + + 經度 + + + 高度 + 删除 @@ -161,13 +170,13 @@ 上移 - 向上移動本行 + 向上移动本行 下移 - 向下移动本行 + 向下移動本行 @@ -185,7 +194,7 @@ Microsoft Sans Serif, 8.25pt - 66, 31 + 24, 40 @@ -194,7 +203,7 @@ Microsoft Sans Serif, 8.25pt - 278, 31 + 162, 40 @@ -227,7 +236,7 @@ Microsoft Sans Serif, 8.25pt - 170, 31 + 90, 40 @@ -239,7 +248,7 @@ 55, 13 - 盤旋半径 + 盤旋半徑 @@ -431,7 +440,7 @@ - 更改目前地圖類型 + 更改當前地圖類型 Microsoft Sans Serif, 8.25pt @@ -449,7 +458,7 @@ 1. 連接 2. 讀取航路（如果需要） 3. 確保起始位置和高度都已設置 -4. 在地图上點擊，增加航點 +4. 在地圖上點擊，增加航點 @@ -457,6 +466,24 @@ + + KML疊加 + + + + + + 縮放至 + + + + + + 相機 + + + + 網格 @@ -467,7 +494,7 @@ 預取 - 預先缓存選中區域的地圖 + 預先緩存選中區域的地圖 海拔圖 @@ -518,19 +545,19 @@ 删除航點 - 152, 22 + 98, 22 永久 - 152, 22 + 98, 22 時間 - 152, 22 + 98, 22 圈數 @@ -541,12 +568,6 @@ 盤旋 - - 152, 22 - - - 152, 22 - 122, 22 @@ -569,20 +590,17 @@ 旋轉地圖 - 152, 22 + 122, 22 增加頂點 - 152, 22 + 122, 22 清除區域 - - 122, 22 - 122, 22 @@ -750,7 +768,7 @@ 31, 13 - 缩放 + 縮放 @@ -764,35 +782,4 @@ - - - ..\Resources\MAVCmd.zh-Hans.txt;System.String, mscorlib, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089;utf-8 - - - 高度 - - - 相機 - - - - - - KML疊加 - - - - - - 缩放至 - - - - - - 緯度 - - - 經度 - \ No newline at end of file From 9fabdb5797de36a2d59814ac50dd2db3745a42b0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 20 Feb 2012 00:24:10 +0900 Subject: [PATCH 18/32] TradHeli - change throttle range back to 0~1000 (was temporarily changed to be 1250~850 which limited the range of the swash plate's vertical movements) --- ArduCopter/radio.pde | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index d119b3e76a..7e9ec6de27 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -22,7 +22,12 @@ static void init_rc_in() // set rc channel ranges g.rc_1.set_angle(4500); g.rc_2.set_angle(4500); - g.rc_3.set_range(MINIMUM_THROTTLE, MAXIMUM_THROTTLE); + #if FRAME_CONFIG == HELI_FRAME + // we do not want to limit the movment of the heli's swash plate + g.rc_3.set_range(0, 1000); + #else + g.rc_3.set_range(MINIMUM_THROTTLE, MAXIMUM_THROTTLE); + #endif g.rc_4.set_angle(4500); // reverse: CW = left @@ -32,13 +37,6 @@ static void init_rc_in() g.rc_2.set_type(RC_CHANNEL_ANGLE_RAW); g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW); - // set rc dead zones - /*g.rc_1.dead_zone = 60; - g.rc_2.dead_zone = 60; - g.rc_3.dead_zone = 60; - g.rc_4.dead_zone = 300; - */ - //set auxiliary ranges g.rc_5.set_range(0,1000); g.rc_6.set_range(0,1000); From 1c9bb25f0971c99b6fb0898cedbd1e069215f87c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 12:38:57 -0800 Subject: [PATCH 19/32] AP TEST fixes for new throttle control --- Tools/autotest/arducopter.py | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index bfe4b54dfe..f6a64ba194 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -14,6 +14,10 @@ HOME=location(-35.362938,149.165085,584,270) homeloc = None num_wp = 0 +def hover(mavproxy, mav): + mavproxy.send('rc 3 1385\n') + return True + def calibrate_level(mavproxy, mav): '''init the accelerometers''' print("Initialising accelerometers") @@ -54,7 +58,7 @@ def takeoff(mavproxy, mav, alt_min = 30): m = mav.recv_match(type='VFR_HUD', blocking=True) if (m.alt < alt_min): wait_altitude(mav, alt_min, (alt_min + 5)) - mavproxy.send('rc 3 1430\n') + hover(mavproxy, mav) print("TAKEOFF COMPLETE") return True @@ -86,13 +90,13 @@ def change_alt(mavproxy, mav, alt_min): m = mav.recv_match(type='VFR_HUD', blocking=True) if(m.alt < alt_min): print("Rise to alt:%u from %u" % (alt_min, m.alt)) - mavproxy.send('rc 3 1800\n') + mavproxy.send('rc 3 1920\n') wait_altitude(mav, alt_min, (alt_min + 5)) else: print("Lower to alt:%u from %u" % (alt_min, m.alt)) - mavproxy.send('rc 3 1070\n') + mavproxy.send('rc 3 1120\n') wait_altitude(mav, (alt_min -5), alt_min) - mavproxy.send('rc 3 1430\n') + hover(mavproxy, mav) return True @@ -107,7 +111,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120): save_wp(mavproxy, mav) print("turn") - mavproxy.send('rc 3 1430\n') + hover(mavproxy, mav) mavproxy.send('rc 4 1610\n') if not wait_heading(mav, 0): return False @@ -156,7 +160,7 @@ def fly_RTL(mavproxy, mav, side=60): '''Fly, return, land''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') - mavproxy.send('rc 3 1430\n') + hover(mavproxy, mav) failed = False print("# Going forward %u meters" % side) @@ -181,7 +185,7 @@ def fly_failsafe(mavproxy, mav, side=60): '''Fly, Failsafe, return, land''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') - mavproxy.send('rc 3 1430\n') + hover(mavproxy, mav) failed = False print("# Going forward %u meters" % side) @@ -210,7 +214,7 @@ def fly_simple(mavproxy, mav, side=60, timeout=120): '''fly Simple, flying N then E''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') - mavproxy.send('rc 3 1440\n') + hover(mavproxy, mav) tstart = time.time() failed = False @@ -236,7 +240,7 @@ def fly_simple(mavproxy, mav, side=60, timeout=120): mavproxy.send('rc 2 1500\n') #restore to default mavproxy.send('param set SIMPLE 0\n') - mavproxy.send('rc 3 1430\n') + hover(mavproxy, mav) return not failed From fa32b084bd516bf73c6c090f110188b13dd94c2a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 12:39:40 -0800 Subject: [PATCH 20/32] Added SW test for Manual Boost --- ArduCopter/test.pde | 37 +++++++++++++------------------------ 1 file changed, 13 insertions(+), 24 deletions(-) diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index dd6d019515..74037faf08 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -17,7 +17,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_nav(uint8_t argc, const Menu::arg *argv); +//static int8_t test_boost(uint8_t argc, const Menu::arg *argv); //static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv); //static int8_t test_reverse(uint8_t argc, const Menu::arg *argv); static int8_t test_tuning(uint8_t argc, const Menu::arg *argv); @@ -70,7 +70,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { //{"tri", test_tri}, {"relay", test_relay}, {"wp", test_wp}, - //{"nav", test_nav}, +// {"boost", test_boost}, #if HIL_MODE != HIL_MODE_ATTITUDE {"altitude", test_baro}, {"sonar", test_sonar}, @@ -179,37 +179,26 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv) }*/ /* -//static int8_t -//test_nav(uint8_t argc, const Menu::arg *argv) +static int8_t +//test_boost(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); delay(1000); + int16_t temp = MINIMUM_THROTTLE; while(1){ - delay(1000); - g_gps->ground_course = 19500; - calc_nav_rate2(g.waypoint_speed_max); - calc_nav_pitch_roll2(); + delay(20); + g.rc_3.control_in = temp; + adjust_altitude(); + Serial.printf("tmp:%d, boost: %d\n", temp, manual_boost); + temp++; - g_gps->ground_course = 28500; - calc_nav_rate2(g.waypoint_speed_max); - calc_nav_pitch_roll2(); - - g_gps->ground_course = 1500; - calc_nav_rate2(g.waypoint_speed_max); - calc_nav_pitch_roll2(); - - g_gps->ground_course = 10500; - calc_nav_rate2(g.waypoint_speed_max); - calc_nav_pitch_roll2(); - - - //if(Serial.available() > 0){ + if(temp > MAXIMUM_THROTTLE){ return (0); - //} + } } } -*/ +//*/ static int8_t test_radio(uint8_t argc, const Menu::arg *argv) From bcd5129f9b173d9c6b212e69d440582e7f04ee4e Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 12:40:08 -0800 Subject: [PATCH 21/32] added workaround for resetting alt. --- ArduCopter/navigation.pde | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index d7e9ce933c..2afa10bd34 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -326,6 +326,13 @@ static void clear_new_altitude() static void set_new_altitude(int32_t _new_alt) { + if(_new_alt == current_loc.alt){ + next_WP.alt = _new_alt; + target_altitude = _new_alt; + alt_change_flag = REACHED_ALT; + return; + } + // just to be clear next_WP.alt = current_loc.alt; From b54d08f4107ab0e143b5fdbc01402a4410ea9699 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 12:40:51 -0800 Subject: [PATCH 22/32] Updated manual throttle control during throttle hold --- ArduCopter/ArduCopter.pde | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c7534e4413..91c5ea52aa 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1602,9 +1602,12 @@ void update_throttle_mode(void) manual_boost, iterm); //*/ + // this lets us know we need to update the altitude after manual throttle control reset_throttle_flag = true; }else{ + // we are under automatic throttle control + // --------------------------------------- if(reset_throttle_flag) { set_new_altitude(max(current_loc.alt, 100)); reset_throttle_flag = false; @@ -1920,15 +1923,16 @@ static void update_altitude() static void adjust_altitude() { - if(g.rc_3.control_in <= 180){ + if(g.rc_3.control_in <= (MINIMUM_THROTTLE + 100)){ // we remove 0 to 100 PWM from hover - manual_boost = g.rc_3.control_in - 180; - manual_boost = max(-120, manual_boost); + manual_boost = (g.rc_3.control_in - MINIMUM_THROTTLE) -100; + manual_boost = max(-100, manual_boost); update_throttle_cruise(); - }else if (g.rc_3.control_in >= 650){ + }else if (g.rc_3.control_in >= (MAXIMUM_THROTTLE - 100)){ // we add 0 to 100 PWM to hover - manual_boost = g.rc_3.control_in - 650; + manual_boost = g.rc_3.control_in - (MAXIMUM_THROTTLE - 100); + manual_boost = min(100, manual_boost); update_throttle_cruise(); }else { manual_boost = 0; From cae49b354352107079463cb9713899d37e5b7bce Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 13:11:59 -0800 Subject: [PATCH 23/32] Added "slow WP" option for RTL. --- ArduCopter/ArduCopter.pde | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 91c5ea52aa..68bcde7f26 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -666,9 +666,10 @@ static int16_t nav_lat; // The desired bank towards East (Positive) or West (Negative) static int16_t nav_lon; // The Commanded ROll from the autopilot based on optical flow sensor. -static int32_t of_roll = 0; +static int32_t of_roll; // The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward. -static int32_t of_pitch = 0; +static int32_t of_pitch; +static bool slow_wp = false; //////////////////////////////////////////////////////////////////////////////// @@ -1655,8 +1656,8 @@ void update_throttle_mode(void) // called after a GPS read static void update_navigation() { - // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS - // ------------------------------------------------------------------------ + // wp_distance is in CM + // -------------------- switch(control_mode){ case AUTO: // note: wp_control is handled by commands_logic @@ -1696,6 +1697,7 @@ static void update_navigation() } wp_control = WP_MODE; + slow_wp = true; // calculates desired Yaw #if FRAME_CONFIG == HELI_FRAME @@ -2109,7 +2111,7 @@ static void update_nav_wp() // calc error to target calc_location_error(&next_WP); - int16_t speed = calc_desired_speed(g.waypoint_speed_max); + int16_t speed = calc_desired_speed(g.waypoint_speed_max, slow_wp); // use error as the desired rate towards the target calc_nav_rate(speed); // rotate pitch and roll to the copter frame of reference From 33d4d52a6b9515f1d483413edba06fef04504e50 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 13:12:23 -0800 Subject: [PATCH 24/32] set Slow WP option for RTL in Auto mode --- ArduCopter/commands_logic.pde | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 2cf2c5ee04..ab0a4e8617 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -208,6 +208,10 @@ static void do_RTL(void) // -------------------- set_next_WP(&temp); + + // We want to come home and stop on a dime + slow_wp = true; + // output control mode to the ground station // ----------------------------------------- gcs_send_message(MSG_HEARTBEAT); From 81a861c8ac6c7836eaaed0e8b0e38c6c2b2fbfa2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 13:13:21 -0800 Subject: [PATCH 25/32] Added slow wp option in calc_desired_speed --- ArduCopter/navigation.pde | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 2afa10bd34..ef534425fc 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -265,7 +265,7 @@ static void calc_loiter_pitch_roll() auto_pitch = -auto_pitch; } -static int16_t calc_desired_speed(int16_t max_speed) +static int16_t calc_desired_speed(int16_t max_speed, bool _slow) { /* |< WP Radius @@ -277,14 +277,13 @@ static int16_t calc_desired_speed(int16_t max_speed) */ // max_speed is default 600 or 6m/s - // (wp_distance * .5) = 1/2 of the distance converted to speed - // wp_distance is always in m/s and not cm/s - I know it's stupid that way - // for example 4m from target = 200cm/s speed - // we choose the lowest speed based on disance - max_speed = min(max_speed, wp_distance); - - // go at least 100cm/s - max_speed = max(max_speed, WAYPOINT_SPEED_MIN); + if(_slow){ + max_speed = min(max_speed, wp_distance / 2); + max_speed = max(max_speed, 0); + }else{ + max_speed = min(max_speed, wp_distance); + max_speed = max(max_speed, WAYPOINT_SPEED_MIN); // go at least 100cm/s + } // limit the ramp up of the speed // waypoint_speed_gov is reset to 0 at each new WP command From 336441b109a123e93ac1357367ce6816fc11afd0 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 13:13:42 -0800 Subject: [PATCH 26/32] reset slow_WP option at mode switch --- ArduCopter/system.pde | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index e6637a1fe0..5f1f2593e7 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -419,6 +419,9 @@ static void set_mode(byte mode) // clearing value used to force the copter down in landing mode landing_boost = 0; + // do we want to come to a stop or pass a WP? + slow_wp = false; + // do not auto_land if we are leaving RTL auto_land_timer = 0; From 265c5255a3108618a12f7053d49b10c42511bd5c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 13:15:40 -0800 Subject: [PATCH 27/32] Added slow_wp default in AP mode --- ArduCopter/Attitude.pde | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 80af18322c..eaf4d75b38 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -245,6 +245,9 @@ static void reset_nav_params(void) // Will be set by new command, used by loiter next_WP.alt = 0; + + // We want to by default pass WPs + slow_wp = false; } /* From c3d6e64efb5ed46bda926f1f7e383dd5ad2a3b0d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Feb 2012 08:40:16 +1100 Subject: [PATCH 28/32] autotest: fixed directory for build_all.sh --- Tools/autotest/autotest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 1887fa4181..08bca099f7 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -71,7 +71,7 @@ def dump_logs(atype): def build_all(): '''run the build_all.sh script''' print("Running build_all.sh") - if not util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh')): + if not util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh'), dir=util.reltopdir('.')): print("Failed build_all.sh") return False return True From e5ec810c9273631961d93b6ae836ec7bdc47c094 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Feb 2012 09:16:57 +1100 Subject: [PATCH 29/32] autotest: fixed check of return code for build_all.sh --- Tools/autotest/autotest.py | 2 +- Tools/scripts/build_all.sh | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 08bca099f7..16543603d2 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -71,7 +71,7 @@ def dump_logs(atype): def build_all(): '''run the build_all.sh script''' print("Running build_all.sh") - if not util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh'), dir=util.reltopdir('.')): + if util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh'), dir=util.reltopdir('.')) != 0: print("Failed build_all.sh") return False return True diff --git a/Tools/scripts/build_all.sh b/Tools/scripts/build_all.sh index ee92b33505..e32924b562 100755 --- a/Tools/scripts/build_all.sh +++ b/Tools/scripts/build_all.sh @@ -33,3 +33,5 @@ for d in $examples; do make popd done + +exit 0 From 11dd9564d9d8a81aca1a0770538b395f33146fac Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 14:20:51 -0800 Subject: [PATCH 30/32] SIMPLE mode throttle fix --- Tools/autotest/arducopter.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f6a64ba194..33fb3bb698 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -214,7 +214,8 @@ def fly_simple(mavproxy, mav, side=60, timeout=120): '''fly Simple, flying N then E''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') - hover(mavproxy, mav) + mavproxy.send('rc 3 1395\n') + tstart = time.time() failed = False From 30efbae0e9cbb93bb4f3c3dd8bdb595537650c68 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 14:21:22 -0800 Subject: [PATCH 31/32] removed old comments --- ArduCopter/ArduCopter.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 68bcde7f26..4c0071aab8 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1383,6 +1383,7 @@ void update_yaw_mode(void) case YAW_AUTO: nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -20, 20); // 40 deg a second + //Serial.printf("nav_yaw %d ", nav_yaw); nav_yaw = wrap_360(nav_yaw); break; } @@ -1958,7 +1959,6 @@ static void tuning(){ break; case CH6_STABILIZE_KP: - //g.rc_6.set_range(0,8000); // 0 to 8 g.pi_stabilize_roll.kP(tuning_value); g.pi_stabilize_pitch.kP(tuning_value); break; @@ -1983,7 +1983,6 @@ static void tuning(){ break; case CH6_YAW_RATE_KP: - //g.rc_6.set_range(0,1000); g.pid_rate_yaw.kP(tuning_value); break; @@ -2144,5 +2143,6 @@ static void update_auto_yaw() // Point towards next WP auto_yaw = target_bearing; } + //Serial.printf("auto_yaw %d ", auto_yaw); // MAV_ROI_NONE = basic Yaw hold } From c10778d685022c79a069bfa2ce66c299cb1df593 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 15:06:54 -0800 Subject: [PATCH 32/32] Better throttle for tests --- Tools/autotest/arducopter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 33fb3bb698..80de97e541 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -15,7 +15,7 @@ homeloc = None num_wp = 0 def hover(mavproxy, mav): - mavproxy.send('rc 3 1385\n') + mavproxy.send('rc 3 1395\n') return True def calibrate_level(mavproxy, mav): @@ -214,7 +214,7 @@ def fly_simple(mavproxy, mav, side=60, timeout=120): '''fly Simple, flying N then E''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') - mavproxy.send('rc 3 1395\n') + mavproxy.send('rc 3 1400\n') tstart = time.time() failed = False