Merge branch 'master' of https://code.google.com/p/ardupilot-mega

ArduCopter/APM_Config.h

@@ -64,4 +64,10 @@
 #define USERHOOK_VARIABLES "UserVariables.h"
 
 
+// to enable, set to 1
+// to disable, set to 0
+#define AUTO_THROTTLE_HOLD 1
+
+
+
 //# define LOGGING_ENABLED		DISABLED

ArduCopter/ArduCopter.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V2.2"
+#define THISFIRMWARE "ArduCopter V2.2 b2"
 /*
 ArduCopter Version 2.2
 Authors:	Jason Short
@@ -858,6 +858,12 @@ void loop()
 		// -------------------------------------------------
 		estimate_velocity();
 
+		// check for new GPS messages
+		// --------------------------
+		if(GPS_enabled){
+			update_GPS();
+		}
+
 		// perform 10hz tasks
 		// ------------------
 		medium_loop();
@@ -879,7 +885,7 @@ void loop()
 				Log_Write_Performance();
 
 			gps_fix_count 		= 0;
-			perf_mon_counter 		= 0;
+			perf_mon_counter 	= 0;
         }
 		//PORTK &= B10111111;
 	}
@@ -901,10 +907,6 @@ static void fast_loop()
 	// IMU DCM Algorithm
 	read_AHRS();
 
-	if(GPS_enabled){
-		update_GPS();
-	}
-
 	// custom code/exceptions for flight modes
 	// ---------------------------------------
 	update_yaw_mode();
@@ -1287,13 +1289,13 @@ static void update_GPS(void)
 	//current_loc.lat = -1224318000;		// Lat * 10 * *7
 	//current_loc.alt = 100;				// alt * 10 * *7
 	//return;
-	if(gps_watchdog < 12){
+	if(gps_watchdog < 30){
 		gps_watchdog++;
 	}else{
 		// after 12 reads we guess we may have lost GPS signal, stop navigating
 		// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
-		nav_roll  = 0;
+		nav_roll  >>= 1;
-		nav_pitch = 0;
+		nav_pitch >>= 1;
 	}
 
     if (g_gps->new_data && g_gps->fix) {
@@ -1355,8 +1357,6 @@ static void update_GPS(void)
 			update_altitude();
 		#endif
 
-	} else {
-		g_gps->new_data = false;
 	}
 }
 
@@ -1460,7 +1460,7 @@ void update_roll_pitch_mode(void)
 // new radio frame is used to make sure we only call this at 50hz
 void update_simple_mode(void)
 {
-	float simple_sin_y=0, simple_cos_x=0;
+	static float simple_sin_y=0, simple_cos_x=0;
 
 	// used to manage state machine
 	// which improves speed of function
@@ -1494,6 +1494,10 @@ void update_throttle_mode(void)
 {
 	int16_t throttle_out;
 
+	#if AUTO_THROTTLE_HOLD != 0
+	static float throttle_avg = THROTTLE_CRUISE;
+	#endif
+
 	switch(throttle_mode){
 		case THROTTLE_MANUAL:
 			if (g.rc_3.control_in > 0){
@@ -1508,11 +1512,13 @@ void update_throttle_mode(void)
 					}
 				#endif
 
+				#if AUTO_THROTTLE_HOLD != 0
 				// calc average throttle
-				if ((g.rc_3.control_in > MINIMUM_THROTTLE)){
+				if ((g.rc_3.control_in > MINIMUM_THROTTLE) && abs(climb_rate) < 60){
-					//throttle_avg = throttle_avg * .98 + rc_3.control_in * .02;
+					throttle_avg = throttle_avg * .98 + (float)g.rc_3.control_in * .02;
-					//g.throttle_cruise = throttle_avg;
+					g.throttle_cruise = throttle_avg;
 				}
+				#endif
 
 				// Code to manage the Copter state
 				if ((millis() - takeoff_timer) > 5000){
@@ -1875,17 +1881,11 @@ adjust_altitude()
 		// we remove 0 to 100 PWM from hover
 		manual_boost = g.rc_3.control_in - 180;
 		manual_boost = max(-120, manual_boost);
-		g.throttle_cruise += g.pi_alt_hold.get_integrator();
+		update_throttle_cruise();
-		g.pi_alt_hold.reset_I();
-		g.pi_throttle.reset_I();
-
 	}else if  (g.rc_3.control_in >= 650){
 		// we add 0 to 100 PWM to hover
 		manual_boost = g.rc_3.control_in - 650;
-		g.throttle_cruise += g.pi_alt_hold.get_integrator();
+		update_throttle_cruise();
-		g.pi_alt_hold.reset_I();
-		g.pi_throttle.reset_I();
-
 	}else {
 		manual_boost = 0;
 	}

ArduCopter/Attitude.pde

@@ -166,7 +166,7 @@ static int16_t
 get_nav_throttle(int32_t z_error)
 {
 	static int16_t old_output = 0;
-	static int16_t rate_d = 0;
+	//static int16_t rate_d = 0;
 
 	int16_t rate_error;
 	int16_t output;
@@ -187,10 +187,10 @@ get_nav_throttle(int32_t z_error)
 	output =  constrain((int)g.pi_throttle.get_p(rate_error), -160, 180);
 
 	// a positive climb rate means we're going up
-	rate_d =  ((rate_d + climb_rate)>>1) * .1; // replace with gain
+	//rate_d =  ((rate_d + climb_rate)>>1) * .1; // replace with gain
 
 	// slight adjustment to alt hold output
-	output -= constrain(rate_d, -25, 25);
+	//output -= constrain(rate_d, -25, 25);
 
 	// light filter of output
 	output = (old_output * 3 + output) / 4;

ArduCopter/commands.pde

@@ -101,16 +101,19 @@ static void set_cmd_with_index(struct Location temp, int i)
 	eeprom_write_dword((uint32_t *)	mem, temp.lng); // Long is stored in decimal degrees * 10^7
 
 	// Make sure our WP_total
-	if(g.command_total <= i)
+	if(g.command_total < (i+1))
 		g.command_total.set_and_save(i+1);
 }
 
 static int32_t read_alt_to_hold()
 {
+	return current_loc.alt;
+	/*
 	if(g.RTL_altitude <= 0)
 		return current_loc.alt;
 	else
 		return g.RTL_altitude;// + home.alt;
+	*/
 }
 
 

ArduCopter/commands_logic.pde

@@ -271,7 +271,7 @@ static void do_land()
 	set_next_WP(&current_loc);
 
 	// Set a new target altitude
-	set_new_altitude(0);
+	set_new_altitude(-200);
 }
 
 static void do_loiter_unlimited()
@@ -290,7 +290,7 @@ static void do_loiter_turns()
 	wp_control = CIRCLE_MODE;
 
 	// reset desired location
-	circle_angle = 0;
+
 
 	if(command_nav_queue.lat == 0){
 		// allow user to specify just the altitude
@@ -305,6 +305,10 @@ static void do_loiter_turns()
 	loiter_total = command_nav_queue.p1 * 360;
 	loiter_sum	 = 0;
 	old_target_bearing = target_bearing;
+
+	circle_angle = target_bearing + 18000;
+	circle_angle = wrap_360(circle_angle);
+	circle_angle *= RADX100;
 }
 
 static void do_loiter_time()
@@ -364,7 +368,7 @@ static bool verify_land()
 		wp_control = NO_NAV_MODE;
 		// try and come down faster
 		landing_boost++;
-		landing_boost 	= min(landing_boost, 20);
+		landing_boost 	= min(landing_boost, 30);
 	}else{
 		landing_boost 	= 0;
 		wp_control 		= LOITER_MODE;

ArduCopter/config.h

@@ -614,7 +614,7 @@
 # define LOITER_P			2.0		// was .25 in previous
 #endif
 #ifndef LOITER_I
-# define LOITER_I			0.05	// Wind control
+# define LOITER_I			0.04	// Wind control
 #endif
 #ifndef LOITER_IMAX
 # define LOITER_IMAX		30		// degrees°

ArduCopter/control_modes.pde

@@ -111,6 +111,7 @@ static void read_trim_switch()
 				trim_flag = false;
 
 				if(control_mode == AUTO){
+					// reset the mission
 					CH7_wp_index = 0;
 					g.command_total.set_and_save(1);
 					return;
@@ -118,7 +119,7 @@ static void read_trim_switch()
 
 				if(CH7_wp_index == 0){
 					// this is our first WP, let's save WP 1 as a takeoff
-					// increment index
+					// increment index to WP index of 1 (home is stored at 0)
 					CH7_wp_index = 1;
 
 					// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
@@ -134,7 +135,7 @@ static void read_trim_switch()
 				// increment index
 				CH7_wp_index++;
 
-				// set the next_WP, 0 is Home so we don't set that
+				// set the next_WP (home is stored at 0)
 				// max out at 100 since I think we need to stay under the EEPROM limit
 				CH7_wp_index = constrain(CH7_wp_index, 1, 100);
 
@@ -149,8 +150,11 @@ static void read_trim_switch()
 				// save command
 				set_cmd_with_index(current_loc, CH7_wp_index);
 
-				// save the index
+				// 0 = home
-				g.command_total.set_and_save(CH7_wp_index + 1);
+				// 1 = takeoff
+				// 2 = WP 2
+				// 3 = command total
+
 			}
 		}
 	}

ArduCopter/motors.pde

@@ -121,6 +121,8 @@ static void init_disarm_motors()
 	motor_armed 	= false;
 	compass.save_offsets();
 
+	g.throttle_cruise.save();
+
 	// we are not in the air
 	takeoff_complete = false;
 

ArduCopter/navigation.pde

@@ -46,25 +46,26 @@ static void calc_XY_velocity(){
 	static int32_t last_longitude = 0;
 	static int32_t last_latitude  = 0;
 
-	static int16_t x_speed_old = 0;
+	//static int16_t x_speed_old = 0;
-	static int16_t y_speed_old = 0;
+	//static int16_t y_speed_old = 0;
 
 	// y_GPS_speed positve = Up
 	// x_GPS_speed positve = Right
 
 	// this speed is ~ in cm because we are using 10^7 numbers from GPS
 	float tmp = 1.0/dTnav;
-	//float tmp = 5;
 
 	// straightforward approach:
-	/*
+	///*
 	int16_t	x_estimate	= (float)(g_gps->longitude - last_longitude) * tmp;
 	int16_t	y_estimate	= (float)(g_gps->latitude  - last_latitude)  * tmp;
+
 	// slight averaging filter
 	x_GPS_speed = (x_GPS_speed  + x_estimate) >> 1;
 	y_GPS_speed = (y_GPS_speed  + y_estimate) >> 1;
-	*/
+	//*/
 
+	/*
 	// Ryan Beall's forward estimator:
 	int16_t	x_speed_new = (float)(g_gps->longitude - last_longitude) * tmp;
 	int16_t	y_speed_new = (float)(g_gps->latitude  - last_latitude)  * tmp;
@@ -74,6 +75,7 @@ static void calc_XY_velocity(){
 
 	x_speed_old 	= x_speed_new;
 	y_speed_old 	= y_speed_new;
+	*/
 
 	last_longitude 	= g_gps->longitude;
 	last_latitude 	= g_gps->latitude;

ArduCopter/system.pde

@@ -558,9 +558,7 @@ static void set_mode(byte mode)
 
 	if(throttle_mode == THROTTLE_MANUAL){
 		// reset all of the throttle iterms
-		g.pi_alt_hold.reset_I();
+		update_throttle_cruise();
-		g.pi_throttle.reset_I();
-
 	}else {
 		// an automatic throttle
 		// todo: replace with a throttle cruise estimator
@@ -605,15 +603,27 @@ init_simple_bearing()
 	initial_simple_bearing = dcm.yaw_sensor;
 }
 
+static void update_throttle_cruise()
+{
+	int16_t tmp = g.pi_alt_hold.get_integrator();
+	if(tmp != 0){
+		g.throttle_cruise += tmp;
+		g.pi_alt_hold.reset_I();
+		g.pi_throttle.reset_I();
+	}
+}
+
 static void
 init_throttle_cruise()
 {
+#if AUTO_THROTTLE_HOLD == 0
 	// are we moving from manual throttle to auto_throttle?
 	if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
 		g.pi_throttle.reset_I();
 		g.pi_alt_hold.reset_I();
 		g.throttle_cruise.set_and_save(g.rc_3.control_in);
 	}
+#endif
 }
 
 #if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED

libraries/AP_Baro/AP_Baro_BMP085.cpp

@@ -56,6 +56,8 @@ extern "C" {
 // is not available to the arduino digitalRead function.
 #define BMP_DATA_READY() (_apm2_hardware?(PINE&0x80):digitalRead(BMP085_EOC))
 
+// oversampling 3 gives highest resolution
+#define OVERSAMPLING 3
 
 // Public Methods //////////////////////////////////////////////////////////////
 bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
@@ -64,7 +66,6 @@ bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
 	
 	pinMode(BMP085_EOC, INPUT);	 // End Of Conversion (PC7) input
 
-	oss = 3;					 // Over Sampling setting 3 = High resolution
 	BMP085_State = 0;		 // Initial state
 
 	// We read the calibration data registers
@@ -142,7 +143,7 @@ int32_t AP_Baro_BMP085::get_raw_temp() {
 // Send command to Read Pressure
 void AP_Baro_BMP085::Command_ReadPress()
 {
-	if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(oss << 6)) != 0) {
+	if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(OVERSAMPLING << 6)) != 0) {
 		healthy = false;
 	}
 }
@@ -163,14 +164,15 @@ void AP_Baro_BMP085::ReadPress()
 		return;
 	}
 
-	RawPress = (((long)buf[0] << 16) | ((long)buf[1] << 8) | ((long)buf[2])) >> (8 - oss);
+	RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
 
-	if(_offset_press == 0){
+	if (_offset_press == 0){
 		_offset_press = RawPress;
 		RawPress = 0;
 	} else{
 		RawPress -= _offset_press;
 	}
+
 	// filter
 	_press_filter[_press_index++] = RawPress;
 
@@ -186,7 +188,6 @@ void AP_Baro_BMP085::ReadPress()
 
 	// grab result
 	RawPress /= PRESS_FILTER_SIZE;
-	//RawPress >>= 3;
 	RawPress += _offset_press;
 }
 
@@ -261,16 +262,16 @@ void AP_Baro_BMP085::Calculate()
 	x1 = (b2 * (b6 * b6 >> 12)) >> 11;
 	x2 = ac2 * b6 >> 11;
 	x3 = x1 + x2;
-	//b3 = (((int32_t) ac1 * 4 + x3)<<oss + 2) >> 2; // BAD
+	//b3 = (((int32_t) ac1 * 4 + x3)<<OVERSAMPLING + 2) >> 2; // BAD
-	//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2;  //OK for oss=0
+	//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2;  //OK for OVERSAMPLING=0
 	tmp = ac1;
-	tmp = (tmp*4 + x3)<<oss;
+	tmp = (tmp*4 + x3)<<OVERSAMPLING;
 	b3 = (tmp+2)/4;
 	x1 = ac3 * b6 >> 13;
 	x2 = (b1 * (b6 * b6 >> 12)) >> 16;
 	x3 = ((x1 + x2) + 2) >> 2;
 	b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
-	b7 = ((uint32_t) RawPress - b3) * (50000 >> oss);
+	b7 = ((uint32_t) RawPress - b3) * (50000 >> OVERSAMPLING);
 	p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
 
 	x1 = (p >> 8) * (p >> 8);

libraries/AP_Baro/AP_Baro_BMP085.h

@@ -28,14 +28,11 @@ class AP_Baro_BMP085 : public AP_Baro
 
   private:
 	int32_t RawPress;
-	int32_t	_offset_press;
+	int32_t _offset_press;
 	int32_t RawTemp;
 	int16_t Temp;
-	int32_t Press;
+	uint32_t Press;
-	//int Altitude;
-	uint8_t oss;
 	bool _apm2_hardware;
-	//int32_t Press0;  // Pressure at sea level
 
 
     // State machine
@@ -44,9 +41,9 @@ class AP_Baro_BMP085 : public AP_Baro
 	int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
     uint16_t ac4, ac5, ac6;
 
-	int	 	_temp_filter[TEMP_FILTER_SIZE];
+	int16_t _temp_filter[TEMP_FILTER_SIZE];
-	int	 	_press_filter[PRESS_FILTER_SIZE];
+	int32_t _press_filter[PRESS_FILTER_SIZE];
-	long	_offset_temp;
+	int32_t	_offset_temp;
 
 	uint8_t	_temp_index;
 	uint8_t	_press_index;

libraries/AP_Baro/AP_Baro_BMP085_hil.h

@@ -17,7 +17,6 @@ public:
 	//int Altitude;
     bool healthy;
 
-	uint8_t oss;
 	bool init(AP_PeriodicProcess * scheduler);
 	uint8_t read();
 	int32_t get_pressure();
@@ -26,7 +25,6 @@ public:
 	int32_t get_raw_pressure();
 	int32_t get_raw_temp();
 	void setHIL(float Temp, float Press);
-	int32_t _offset_press;
 };
 
 #endif //  __AP_BARO_BMP085_HIL_H__

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp

@@ -237,17 +237,17 @@ AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn )
 	write_register(ADNS3080_CONFIGURATION_BITS, regVal);
 }
 
-// returns resolution (either 400 or 1200 counts per inch)
+// returns resolution (either 400 or 1600 counts per inch)
 int
 AP_OpticalFlow_ADNS3080::get_resolution()
 {
     if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
 	    return 400;
 	else
-	    return 1200;
+	    return 1600;
 }
 
-// set parameter to 400 or 1200 counts per inch
+// set parameter to 400 or 1600 counts per inch
 void
 AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
 {
@@ -255,12 +255,17 @@ AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
 
     if( resolution == ADNS3080_RESOLUTION_400 )	{
 	    regVal &= ~0x10;
-	}else if( resolution == ADNS3080_RESOLUTION_1200) {
+		scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
+	}else if( resolution == ADNS3080_RESOLUTION_1600) {
 	    regVal |= 0x10;
+		scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
 	}
 
 	delayMicroseconds(50);  // small delay
 	write_register(ADNS3080_CONFIGURATION_BITS, regVal);
+
+	// this will affect conversion factors so update them
+	update_conversion_factors();
 }
 
 // get_frame_rate_auto - return whether frame rate is set to "auto" or manual

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h

@@ -77,7 +77,7 @@
 #define ADNS3080_LED_MODE_WHEN_REQUIRED    0x01
 
 #define ADNS3080_RESOLUTION_400			400
-#define ADNS3080_RESOLUTION_1200		1200
+#define ADNS3080_RESOLUTION_1600		1600
 
 // Extended Configuration bits
 #define ADNS3080_SERIALNPU_OFF	0x02
@@ -118,8 +118,8 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
 	bool get_led_always_on();                    // returns true if LED is always on, false if only on when required
 	void set_led_always_on( bool alwaysOn );     // set parameter to true if you want LED always on, otherwise false for only when required
 
-	int get_resolution();							// returns resolution (either 400 or 1200 counts per inch)
+	int get_resolution();							// returns resolution (either 400 or 1600 counts per inch)
-	void set_resolution(int resolution);            // set parameter to 400 or 1200 counts per inch
+	void set_resolution(int resolution);            // set parameter to 400 or 1600 counts per inch
 
 	bool get_frame_rate_auto();                      // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual
 	void set_frame_rate_auto(bool auto_frame_rate);  // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false)

libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde

@@ -201,7 +201,7 @@ void set_resolution()
     Serial.print("resolution: ");
 	Serial.println(resolution);
 	Serial.println("Choose new value:");
-	Serial.println("    1) 1200");
+	Serial.println("    1) 1600");
 	Serial.println("    4) 400");
 	Serial.println("    x) exit");
 	Serial.println();
@@ -215,10 +215,9 @@ void set_resolution()
 	
 	// update resolution
 	if( value == '1' ) {
-	    flowSensor.set_resolution(ADNS3080_RESOLUTION_1200);
+	    flowSensor.set_resolution(ADNS3080_RESOLUTION_1600);
 	}
 	if( value == '4' ) {
-	    Serial.println("you want 1200!");
 	    flowSensor.set_resolution(ADNS3080_RESOLUTION_400);
 	}