fun stuff

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1281 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArduCopterMega/ArduCopterMega.pde

@@ -773,11 +773,14 @@ void update_current_flight_mode(void)
 				//call at 5 hz
 				if(fbw_timer > 20){
 					fbw_timer = 0;
-					current_loc.lat = 0;
+
-					current_loc.lng = 0;
+					if(home_is_set == false){
+						current_loc.lat = home.lat = 0;
+						current_loc.lng = home.lng = 0;
+					}
 					
-					next_WP.lat = rc_1.control_in /5; // 10 meteres
+					next_WP.lat = home.lat + rc_1.control_in /5; // 10 meteres
-					next_WP.lng = -rc_2.control_in /5; // 10 meteres
+					next_WP.lng = home.lng -rc_2.control_in /5; // 10 meteres
 					
 					// waypoint distance from plane
 					// ----------------------------

ArduCopterMega/Attitude.pde

@@ -159,6 +159,7 @@ void set_servos_4(void)
 		int b_out = ((long)(motor_out[BACK]  - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min);
 		//*/
 				
+		//
 		/* debugging and dynamic kP
 		num++;
 		if (num > 50){
@@ -166,16 +167,18 @@ void set_servos_4(void)
 			//Serial.printf("control_in: %d ", rc_3.control_in);
 			//Serial.printf(" servo: %d %d %d %d\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out);
 			//Serial.printf(" cwm: %d %d %d %d, %d\t", rc_1.pwm_out, rc_2.pwm_out, rc_3.pwm_out, rc_4.pwm_out, rc_3.radio_out);
-			//Serial.printf(" pwm: %d %d %d %d\n", r_out, l_out, f_out, b_out);
+			//Serial.printf(" out: %d %d %d %d\n", r_out, l_out, f_out, b_out);
-			//Serial.printf(" pwm: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
+			//Serial.printf(" pwm: %d, %d %d %d %d\n",rc_3.pwm_out, motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
 			
 			pid_stabilize_roll.kP((float)rc_6.control_in / 1000);
-			stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.24;
+			stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.25;
 			init_pids();
 			
-			Serial.print("kP: ");
+			//Serial.print("kP: ");
-			Serial.println(pid_stabilize_roll.kP(),3);
+			//Serial.println(pid_stabilize_roll.kP(),3);
 		}
+		// out: 41 38 39 39
+ 		// pwm: 358, 1412 1380 1395 1389
 		//*/
 		
 		//Serial.printf("set: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);		

ArduCopterMega/config.h

@@ -283,7 +283,7 @@
 // STABILZE Angle Control
 //
 #ifndef STABILIZE_ROLL_P
-# define STABILIZE_ROLL_P 		0.44
+# define STABILIZE_ROLL_P 		0.7
 #endif
 #ifndef STABILIZE_ROLL_I
 # define STABILIZE_ROLL_I 		0.00
@@ -296,7 +296,7 @@
 #endif
 
 #ifndef STABILIZE_PITCH_P
-# define STABILIZE_PITCH_P		0.44
+# define STABILIZE_PITCH_P		0.7
 #endif
 #ifndef STABILIZE_PITCH_I
 # define STABILIZE_PITCH_I		0.0
@@ -311,7 +311,7 @@
 // STABILZE RATE Control
 //
 #ifndef STABILIZE_RATE_RP
-# define STABILIZE_RATE_RP		0.1
+# define STABILIZE_RATE_RP		0.175
 #endif
 
 
@@ -334,7 +334,7 @@
 // STABILZE YAW Control
 //
 #ifndef STABILIZE_RATE_YAW
-# define STABILIZE_RATE_YAW		0.008
+# define STABILIZE_RATE_YAW		0.000
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -356,7 +356,7 @@
 //
 // how much to we pitch towards the target
 #ifndef PITCH_MAX
-# define PITCH_MAX				12
+# define PITCH_MAX				8
 #endif
 
 
@@ -381,16 +381,16 @@
 // Throttle control gains
 //
 #ifndef THROTTLE_P
-# define THROTTLE_P		0.5
+# define THROTTLE_P		1.0
 #endif
 #ifndef THROTTLE_I
 # define THROTTLE_I		0.01
 #endif
 #ifndef THROTTLE_D
-# define THROTTLE_D		0.3
+# define THROTTLE_D		0.4
 #endif
 #ifndef THROTTLE_IMAX
-# define THROTTLE_IMAX	50
+# define THROTTLE_IMAX	20
 #endif
 
 

ArduCopterMega/control_modes.pde

@@ -81,11 +81,16 @@ void read_trim_switch()
 					imu.init_accel();
 					imu.print_accel_offsets();
 				}*/
-				Serial.printf("r: %d, p: %d\n", rc_1.control_in, rc_2.control_in);
+				if(rc_3.control_in == 0){
-				// set new accel offset values
+					imu.zero_accel();
-				imu.ax(((long)rc_1.control_in * -15) / 100);
+				}else{
-				imu.ay(((long)rc_2.control_in * -15) / 100);
+					Serial.printf("r: %d, p: %d\n", rc_1.control_in, rc_2.control_in);
-				imu.print_accel_offsets();
+					// set new accel offset values
+					imu.ax(((long)rc_1.control_in * -15) / 100);
+					imu.ay(((long)rc_2.control_in * -15) / 100);
+					imu.print_accel_offsets();
+				}
+
 			} else {
 				// set the throttle nominal
 				if(rc_3.control_in > 50){

ArduCopterMega/flight_control.pde

@@ -21,7 +21,7 @@ void output_auto_throttle()
 
 void calc_nav_throttle()
 {
-	long err = constrain (altitude_error, -300, 300); //+-3 meters
+	long err = constrain (altitude_error, -150, 150); //+-1.5 meters
 	long temp = pid_throttle.get_pid(err, deltaMiliSeconds, 1.0);
 	nav_throttle = (float)(throttle_cruise + temp) * angle_boost();
 }

ArduCopterMega/read_me.text

@@ -7,38 +7,40 @@ ch2 = pitch
 ch3 = throttle
 ch4 = yaw
 ch5 = mode switch - use your 3 position switch
-ch6 = used for tuning - not currently active
+ch6 = used for tuning - not currently active, search for "rc_6" to enable
 ch7 = use to set throttle hold value while hovering (quick toggle), hold > 5 seconds on ground to reset the accelerometer offsets.
 ch8 = not used
 
 setup:
-erase  = run this first, just in case
+erase  			- run this first, just in case
-reset  = run this second
+reset  			- run this second
-radio  = run this third
+radio  			- run this third
-esc = just ignore this for now
+esc 			- just ignore this for now
-level  = optional - sets accelerometer offsets
+level  			- optional - sets accelerometer offsets
-flat   = optional - sets accelerometer offsets to 0
+flat   			- optional - sets accelerometer offsets to 0
-modes  = interactive setup for flight modes
+modes  			- interactive setup for flight modes
-pid    = optional - writes default PID values to eeprom
+pid    			- optional - writes default PID values to eeprom
-frame  = optional - default is "+"
+frame  			- optional - default is "+"
-enable_mag = enables the compass
+enable_mag 		- enables the compass
-disable_mag = disables the compass
+disable_mag 	- disables the compass
-compass      = interactive setup for compass offsets
+compass      	- interactive setup for compass offsets
-declination  = usage: "declination 14.25"
+declination  	- usage: "declination 14.25"
-show = shows all values
+show 			- shows all values
 
 Flight modes to try:
-stabilize  - I'm having a little trouble inputting Yaw commands, would love some feedback on how to best handle it.
+stabilize  - yay
-alt_hold - You need to set your throttle_cruise value by toggling ch_7 for  a second. Mine is 330
+Alt_hold   - You need to set your throttle_cruise value by toggling ch_7 for less than 1 second. (Mine is 330), altitude is controlled by the throttle lever.
-- altitude is controlled by the throttle lever.
+FBW 	   - Simulates GPS Hold with the sticks being the position offset. Manual Throttle.
 position_hold - When selected, it will hold the current altitude, position and yaw. Yaw is user controllable. roll and pitch can be overridden with the radio.
-RTL - will try and fly back to home at the current altitude.
+RTL 		- not tested yet, will try and fly back to home at the current altitude.
-Auto - not tested yet, i'm finishing a waypoint writing sketch and will be ready to test soon
+Auto 		- not tested yet, i'm finishing a waypoint writing sketch and will be ready to test soon
+
 - what's new
 - CMD_ANGLE - will set the desired yaw with control of angle/second and direction.
 - CMD_ALTITUDE - will send the copter up from current position to desired altitude
 - CMD_R_WAYPOINT - is just like a waypoint but relative to home
 - CMD_TRACK_WAYPOINT - coming soon, will point the copter at the waypoint no matter the position
+
 Special note:
 Any mode other than stabilize will cause the props to spin once the control switch is engaged.
 The props will NOT spin in stabilize when throttle is in the off position, even when armed.

ArduCopterMega/test.pde

@@ -114,9 +114,26 @@ test_radio(uint8_t argc, const Menu::arg *argv)
 	while(1){
 		delay(20);
 		read_radio();
+		output_manual_throttle();
 		
-		Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), (rc_1.control_in), (rc_2.control_in), (rc_3.control_in), (rc_4.control_in), rc_5.control_in, rc_6.control_in, rc_7.control_in);
+		rc_1.calc_pwm();
+		rc_2.calc_pwm();
+		rc_3.calc_pwm();
+		rc_4.calc_pwm();
+		
+		//Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), (rc_1.control_in), (rc_2.control_in), (rc_3.control_in), (rc_4.control_in), rc_5.control_in, rc_6.control_in, rc_7.control_in);
 		//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (rc_1.servo_out / 100), (rc_2.servo_out / 100), rc_3.servo_out, (rc_4.servo_out / 100));
+		Serial.printf_P(PSTR(	"min: %d"
+								"\t in: %d"
+								"\t pwm_in: %d"
+								"\t sout: %d"
+								"\t pwm_out %d\n"),
+								rc_3.radio_min, 
+								rc_3.control_in,
+								rc_3.radio_in,
+								rc_3.servo_out,
+								rc_3.pwm_out
+								);
 
 		if(Serial.available() > 0){
 			return (0);