diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 21dfd0d919..4c0071aab8 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
@@ -660,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;
////////////////////////////////////////////////////////////////////////////////
@@ -1333,14 +1340,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;
}
@@ -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;
}
@@ -1407,9 +1408,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:
@@ -1585,9 +1604,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;
@@ -1635,8 +1657,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
@@ -1676,6 +1698,7 @@ static void update_navigation()
}
wp_control = WP_MODE;
+ slow_wp = true;
// calculates desired Yaw
#if FRAME_CONFIG == HELI_FRAME
@@ -1903,15 +1926,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;
@@ -1935,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;
@@ -1960,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;
@@ -2088,7 +2110,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
@@ -2121,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
}
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;
}
/*
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/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
//-----------------------------------------------------------------------------------------------------
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/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);
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index aecf13b8fd..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
@@ -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
//
@@ -690,8 +697,6 @@
#endif
-
-
#ifndef WAYPOINT_SPEED_MAX
# define WAYPOINT_SPEED_MAX 600 // 6m/s error = 13mph
#endif
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;
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index d7e9ce933c..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
@@ -326,6 +325,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;
diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde
index 544e43f765..7e9ec6de27 100644
--- a/ArduCopter/radio.pde
+++ b/ArduCopter/radio.pde
@@ -22,9 +22,11 @@ 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;
+ #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);
@@ -35,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);
@@ -69,7 +64,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 +133,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);
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();
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;
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)
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/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
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)) {
diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index bfe1930485..80de97e541 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 1395\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 1100\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)
@@ -193,22 +197,25 @@ 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):
'''fly Simple, flying N then E'''
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
- mavproxy.send('rc 3 1440\n')
+ mavproxy.send('rc 3 1400\n')
+
tstart = time.time()
failed = False
@@ -234,7 +241,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
diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py
index 17cc5bf9b6..16543603d2 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 util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh'), dir=util.reltopdir('.')) != 0:
+ 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()
diff --git a/Tools/scripts/build_all.sh b/Tools/scripts/build_all.sh
index b61b6ef181..e32924b562 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,10 +15,23 @@ 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; do
+for b in all apm2 apm2beta hil sitl heli; do
pwd
make clean
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
+
+exit 0
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;
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