Fixed CIRCLE mode trig error

made loiter_radius * 100 in Params
updated gains

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

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:

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")),

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

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;