diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 62b3d06bec..0691b88635 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -345,6 +345,7 @@ static float cos_pitch_x = 1; static float cos_yaw_x = 1; static float sin_pitch_y, sin_yaw_y, sin_roll_y; static long initial_simple_bearing; // used for Simple mode +static float simple_sin_y, simple_cos_x; static float boost; // used to give a little extra to maintain altitude // Acro @@ -1043,17 +1044,29 @@ void update_roll_pitch_mode(void) if(do_simple && new_radio_frame){ new_radio_frame = false; - //Serial.printf("1: %d, 2: %d ",g.rc_1.control_in, g.rc_2.control_in); + simple_timer++; + + int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; + + if (simple_timer == 1){ + // roll + simple_cos_x = sin(radians(90 - delta)); + + }else if (simple_timer > 2){ + // pitch + simple_sin_y = cos(radians(90 - delta)); + simple_timer = 0; + } // Rotate input by the initial bearing - control_roll = g.rc_1.control_in * sin_yaw_y + g.rc_2.control_in * cos_yaw_x; - control_pitch = g.rc_2.control_in * sin_yaw_y - g.rc_1.control_in * cos_yaw_x; + control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y; + control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x); g.rc_1.control_in = control_roll; g.rc_2.control_in = control_pitch; - //Serial.printf("\t1: %d, 2: %d\n",g.rc_1.control_in, g.rc_2.control_in); } + switch(roll_pitch_mode){ case ROLL_PITCH_ACRO: // Roll control @@ -1085,13 +1098,16 @@ void update_throttle_mode(void) { switch(throttle_mode){ case THROTTLE_MANUAL: - g.rc_3.servo_out = g.rc_3.control_in + boost; + if (g.rc_3.control_in > 0){ + g.rc_3.servo_out = g.rc_3.control_in + boost; + }else{ + g.rc_3.servo_out = 0; + } break; case THROTTLE_HOLD: // allow interactive changing of atitude adjust_altitude(); - // fall through case THROTTLE_AUTO: @@ -1170,10 +1186,10 @@ static void update_navigation() update_auto_yaw(); { //circle_angle += dTnav; //1000 * (dTnav/1000); - circle_angle = wrap_360(target_bearing + 2000 + 18000); + circle_angle = wrap_360(target_bearing + 3000 + 18000); - target_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(radians(90 - circle_angle))); - target_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(radians(90 - circle_angle))); + target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle/100))); + target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle/100))); } // calc the lat and long error to the target diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index d5a71cc5a9..98dbf41998 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -558,7 +558,7 @@ static void Log_Write_Nav_Tuning() static void Log_Read_Nav_Tuning() { - Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d\n"), + Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d\n"), DataFlash.ReadInt(), // distance DataFlash.ReadByte(), // wp_verify_byte DataFlash.ReadInt(), // target_bearing @@ -591,8 +591,6 @@ static void Log_Read_Nav_Tuning() DataFlash.ReadInt(), DataFlash.ReadInt()); //nav bearing */ - - } @@ -630,8 +628,8 @@ static void Log_Read_Control_Tuning() "%d, %d\n"), // Control - DataFlash.ReadInt(), - DataFlash.ReadInt(), + //DataFlash.ReadInt(), + //DataFlash.ReadInt(), // yaw DataFlash.ReadInt(), @@ -760,7 +758,7 @@ static void Log_Write_Attitude() // Read an attitude packet static void Log_Read_Attitude() { - Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, $d\n"), + Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"), DataFlash.ReadInt(), DataFlash.ReadInt(), (uint16_t)DataFlash.ReadInt(), @@ -821,15 +819,15 @@ static void Log_Read(int start_page, int end_page) case 0: if(data == HEAD_BYTE1) // Head byte 1 log_step++; - else - Serial.println("."); break; case 1: if(data == HEAD_BYTE2) // Head byte 2 log_step++; - else + else{ log_step = 0; + Serial.println("."); + } break; case 2: diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index c6e6d7cc67..d026d30063 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -306,7 +306,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("WP_LOITER_RAD")), + loiter_radius (LOITER_RADIUS * 100, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")), throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 79e19c1b05..75cbfd2c03 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -392,7 +392,7 @@ // Attitude Control // #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 3.6 +# define STABILIZE_ROLL_P 4.0 #endif #ifndef STABILIZE_ROLL_I # define STABILIZE_ROLL_I 0.02 @@ -402,7 +402,7 @@ #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 3.6 +# define STABILIZE_PITCH_P 4.0 #endif #ifndef STABILIZE_PITCH_I # define STABILIZE_PITCH_I 0.02 @@ -474,7 +474,7 @@ # define LOITER_P .4 // #endif #ifndef LOITER_I -# define LOITER_I 0.01 // +# define LOITER_I 0.10 // #endif #ifndef LOITER_IMAX # define LOITER_IMAX 12 // degrees° @@ -484,7 +484,7 @@ # define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch #endif #ifndef NAV_I -# define NAV_I 0.12 // this +# define NAV_I 0.15 // this #endif #ifndef NAV_IMAX # define NAV_IMAX 20 // degrees @@ -654,8 +654,8 @@ # define WP_RADIUS_DEFAULT 3 #endif -#ifndef LOITER_RADIUS_DEFAULT -# define LOITER_RADIUS_DEFAULT 10 +#ifndef LOITER_RADIUS +# define LOITER_RADIUS 10 #endif #ifndef ALT_HOLD_HOME diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde index d5af159ad4..5344a08f63 100644 --- a/ArduCopter/motors_quad.pde +++ b/ArduCopter/motors_quad.pde @@ -4,6 +4,7 @@ static void output_motors_armed() { + int roll_out, pitch_out; int out_min = g.rc_3.radio_min; int out_max = g.rc_3.radio_max;