continued APvar int

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

ArduCopterMega/ArduCopterMega.pde

@@ -510,17 +510,10 @@ void medium_loop()
 		case 2:
 			medium_loopCounter++;
 
-			// Read Baro pressure
-			// ------------------
-			read_barometer();
 
-			if(g.sonar_enabled){
+			// Read altitude from sensors
-				sonar.read();
+			// ------------------
-				update_alt();
+			update_alt();
-			}else{
-				// no sonar altitude
-				current_loc.alt = baro_alt;
-			}
 
 			// altitude smoothing
 			// ------------------
@@ -867,7 +860,7 @@ void update_current_flight_mode(void)
 
 				//if(g.rc_3.control_in)
 				// get desired height from the throttle
-				next_WP.alt 	= home.alt + (g.rc_3.control_in * 4); // 0 - 1000 (40 meters)
+				next_WP.alt 	= home.alt + (g.rc_3.control_in); // 0 - 1000 (40 meters)
 				next_WP.alt		= max(next_WP.alt, 30);
 
 				// !!! testing
@@ -998,14 +991,33 @@ void update_trig(void){
 }
 
 
-void
+void update_alt()
-update_alt(){
+{
+	altitude_sensor = BARO;
+	baro_alt 		= read_barometer();
 
-	altitude_sensor = (target_altitude > 500) ? BARO : SONAR;
+	if(g.sonar_enabled){
+		// decide which sensor we're usings
+		sonar_alt 		= sonar.read();
 
-	if(altitude_sensor == BARO){
+		if(baro_alt < 550){
-		current_loc.alt = baro_alt;
+			altitude_sensor = SONAR;
+		}
+
+		if(sonar_alt > 600){
+			altitude_sensor = BARO;
+		}
+
+		//altitude_sensor = (target_altitude > (home.alt + 500)) ? BARO : SONAR;
+
+		if(altitude_sensor == BARO){
+			current_loc.alt = baro_alt + home.alt;
+		}else{
+			sonar_alt		= min(sonar_alt, 600);
+			current_loc.alt = sonar_alt + home.alt;
+		}
 	}else{
-		current_loc.alt = sonar_alt;
+		// no sonar altitude
+		current_loc.alt = baro_alt + home.alt;
 	}
 }

ArduCopterMega/Attitude.pde

@@ -202,30 +202,30 @@ void calc_nav_throttle()
 {
 	// limit error
 	long error = constrain(altitude_error, -400, 400);
-	
+
 	if(altitude_sensor == BARO) {
 		float t = g.pid_baro_throttle.kP();
-		
+
 		if(error > 0){ 					// go up
 			g.pid_baro_throttle.kP(t);
 		}else{							// go down
 			g.pid_baro_throttle.kP(t/4.0);
 		}
-		
+
 		// limit output of throttle control
 		nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
 		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
-		
+
 		g.pid_baro_throttle.kP(t);
-		
+
 	} else {
 		// SONAR
 		nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
-	
+
 		// limit output of throttle control
 		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
 	}
-	
+
 	nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
 	nav_throttle_old = nav_throttle;
 }

ArduCopterMega/EEPROM.pde

@@ -205,7 +205,6 @@ void save_EEPROM_pressure(void)
 {
 	g.ground_pressure.save();
 	g.ground_temperature.save();
-
 }
 
 void read_EEPROM_pressure(void)

ArduCopterMega/commands.pde

@@ -216,9 +216,11 @@ void init_home()
 	home.id 	= MAV_CMD_NAV_WAYPOINT;
 	home.lng 	= g_gps->longitude;				// Lon * 10**7
 	home.lat 	= g_gps->latitude;				// Lat * 10**7
-	home.alt 	= g_gps->altitude;
+	home.alt 	= max(g_gps->altitude, 0);
 	home_is_set = true;
 
+	Serial.printf("gps alt: %ld", home.alt);
+
 	// ground altitude in centimeters for pressure alt calculations
 	// ------------------------------------------------------------
 	save_EEPROM_pressure();

ArduCopterMega/config.h

@@ -409,22 +409,20 @@
 #ifndef THROTTLE_BARO_IMAX
 # define THROTTLE_BARO_IMAX		50
 #endif
-# define THROTTLE_BARO_IMAX_CENTIDEGREE	THROTTLE_BARO_IMAX * 100
 
 
 #ifndef THROTTLE_SONAR_P
-# define THROTTLE_SONAR_P		.8
+# define THROTTLE_SONAR_P		.3
 #endif
 #ifndef THROTTLE_SONAR_I
-# define THROTTLE_SONAR_I		0.3
+# define THROTTLE_SONAR_I		0.01
 #endif
 #ifndef THROTTLE_SONAR_D
-# define THROTTLE_SONAR_D		0.7
+# define THROTTLE_SONAR_D		0.2
 #endif
 #ifndef THROTTLE_SONAR_IMAX
-# define THROTTLE_SONAR_IMAX	300
+# define THROTTLE_SONAR_IMAX	50
 #endif
-# define THROTTLE_SONAR_IMAX_CENTIDEGREE	THROTTLE_SONAR_IMAX * 100
 
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopterMega/control_modes.pde

@@ -2,11 +2,11 @@ void read_control_switch()
 {
 	byte switchPosition = readSwitch();
 	//motor_armed = (switchPosition < 5);
-	
+
 	if (oldSwitchPosition != switchPosition){
-				
+
 		set_mode(g.flight_modes[switchPosition]);
-		
+
 		oldSwitchPosition = switchPosition;
 
 		// reset navigation integrators
@@ -23,7 +23,7 @@ byte readSwitch(void){
 #elif FLIGHT_MODE_CHANNEL == CH_7
 	int pulsewidth = g.rc_7.radio_in;			//
 #elif FLIGHT_MODE_CHANNEL == CH_8
-	int pulsewidth = g.rc_8.radio_in;			// default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux! 
+	int pulsewidth = g.rc_8.radio_in;			// default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux!
 #else
 # error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8
 #endif
@@ -62,24 +62,26 @@ void read_trim_switch()
 			// called once
 			trim_timer = millis();
 		}
-		
+
 		trim_flag = true;
 		trim_accel();
-		
+
-		
+
 	}else{ // switch is disengaged
-	
+
 		if(trim_flag){
 			// switch was just released
 			if((millis() - trim_timer) > 2000){
 				imu.save();
-				
+
 			} else {
 				// set the throttle nominal
 				if(g.rc_3.control_in > 50){
 					g.throttle_cruise.set(g.rc_3.control_in);
 					Serial.printf("tnom %d\n", g.throttle_cruise.get());
-					save_EEPROM_throttle_cruise();
+					//save_EEPROM_throttle_cruise();
+					g.throttle_cruise.save();
+
 				}
 			}
 			trim_flag = false;

ArduCopterMega/motors.pde

@@ -60,7 +60,7 @@ set_servos_4()
 		//Serial.printf("yaw: %d ", g.rc_4.radio_out);
 
 		if(g.frame_type == PLUS_FRAME){
-			Serial.println("P_FRAME");
+			//Serial.println("P_FRAME");
 			motor_out[CH_1]		= g.rc_3.radio_out - g.rc_1.pwm_out;
 			motor_out[CH_2]		= g.rc_3.radio_out + g.rc_1.pwm_out;
 			motor_out[CH_3]		= g.rc_3.radio_out + g.rc_2.pwm_out;
@@ -73,7 +73,7 @@ set_servos_4()
 
 
 		}else if(g.frame_type == X_FRAME){
-			Serial.println("X_FRAME");
+			//Serial.println("X_FRAME");
 			int roll_out 	 	= g.rc_1.pwm_out / 2;
 			int pitch_out 	 	= g.rc_2.pwm_out / 2;
 
@@ -94,7 +94,7 @@ set_servos_4()
 
 		}else if(g.frame_type == TRI_FRAME){
 
-			Serial.println("TRI_FRAME");
+			//Serial.println("TRI_FRAME");
 			// Tri-copter power distribution
 
 			int roll_out 		= (float)g.rc_1.pwm_out * .866;
@@ -112,7 +112,7 @@ set_servos_4()
 
 
 		}else if (g.frame_type == HEXA_FRAME) {
-			Serial.println("6_FRAME");
+			//Serial.println("6_FRAME");
 
 			int roll_out 		= (float)g.rc_1.pwm_out * .866;
 			int pitch_out 		= g.rc_2.pwm_out / 2;
@@ -154,8 +154,18 @@ set_servos_4()
 		}
 
 		num++;
-		if (num > 10){
+		if (num > 50){
 			num = 0;
+			Serial.printf("t_alt:%ld, alt:%ld, thr: %d sen: ",
+						target_altitude,
+						current_loc.alt,
+						g.rc_3.servo_out);
+
+			if(altitude_sensor == BARO){
+				Serial.println("Baro");
+			}else{
+				Serial.println("Sonar");
+			}
 			//Serial.print("!");
 			//debugging with Channel 6
 
@@ -178,7 +188,7 @@ set_servos_4()
 			init_pids();
 			//*/
 
-			///*
+			/*
 			write_int(motor_out[CH_1]);
 			write_int(motor_out[CH_2]);
 			write_int(motor_out[CH_3]);

ArduCopterMega/sensors.pde

@@ -6,13 +6,15 @@ void init_pressure_ground(void)
 	for(int i = 0; i < 300; i++){		// We take some readings...
 		delay(20);
 		barometer.Read(); 	// Get initial data from absolute pressure sensor
-		ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
+		ground_pressure 	= (ground_pressure * 9l   + barometer.Press) / 10l;
 		ground_temperature 	= (ground_temperature * 9 + barometer.Temp) / 10;
 	}
 	abs_pressure  = barometer.Press;
 }
 
-void read_barometer(void){
+long
+read_barometer(void)
+{
  	float x, scaling, temp;
 
 	barometer.Read(); 	// Get new data from absolute pressure sensor
@@ -20,9 +22,9 @@ void read_barometer(void){
 	//abs_pressure 			= (abs_pressure + barometer.Press) >> 1;		// Small filtering
 	abs_pressure 			= ((float)abs_pressure * .7) + ((float)barometer.Press * .3);		// large filtering
 	scaling 				= (float)ground_pressure / (float)abs_pressure;
-	temp 					= ((float)ground_temperature / 10.f) + 273.15;
+	temp 					= ((float)ground_temperature / 10.0f) + 273.15f;
 	x 						= log(scaling) * temp * 29271.267f;
-	baro_alt 				= (long)(x / 10) + home.alt;// + baro_offset;		// Pressure altitude in centimeters
+	return 	(x / 10);
 }
 
 // in M/S * 100

ArduCopterMega/test.pde

@@ -762,7 +762,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 	Serial.printf_P(PSTR("\nCalibrating....\n"));
 	/*
 	for (int i = 1; i < 301; i++) {
-		read_barometer();
+		baro_alt = read_barometer();
 		if(i > 200)
 			sum += abs_pressure;
 		delay(10);
@@ -790,12 +790,14 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 
 			read_radio();			// read the radio first
 			next_WP.alt = home.alt + g.rc_6.control_in; // 0 - 2000 (20 meters)
-			read_trim_switch();
+			next_WP.alt =  max(next_WP.alt, 30);
-			read_barometer();
 
+			read_trim_switch();
+			baro_alt = read_barometer();
+			current_loc.alt = baro_alt + home.alt;
 			Serial.printf_P(PSTR("AP: %ld,\tAlt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \t out:%d\n"),
 						abs_pressure,
-						current_loc.alt,
+						baro_alt,
 						next_WP.alt,
 						altitude_error,
 						(int)g.throttle_cruise,