Better sonar hold ++

added mag calibration - still not sure if needed.
better sonar spike filter
2.0.19

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

ArduCopterMega/APM_Config.h

@@ -7,7 +7,7 @@
 //#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 
 #define NAV_TEST 1	// 0 = traditional, 1 = rate controlled
-#define YAW_OPTION 0	// 0 = hybrid rate approach, 1 = offset Yaw approach
+#define YAW_OPTION 1	// 0 = hybrid rate approach, 1 = offset Yaw approach
 #define AUTO_RESET_LOITER 0 // enables Loiter to reset it's current location based on stick input.
 
 #define FRAME_CONFIG QUAD_FRAME

ArduCopterMega/ArduCopterMega.pde

@@ -642,7 +642,7 @@ void medium_loop()
 			if (g.log_bitmask & MASK_LOG_NTUN)
 				Log_Write_Nav_Tuning();
 
-			if (g.log_bitmask & MASK_LOG_GPS){
+			if (GPS_enabled && g.log_bitmask & MASK_LOG_GPS){
 				Log_Write_GPS();
 			}
 
@@ -1230,13 +1230,11 @@ void update_alt()
 		// filter out offset
 
 		// read barometer
-		baro_alt 		= read_barometer();
-
-		// XXX temp removed fr debugging
-		//filter out bad sonar reads
+		baro_alt 			= read_barometer();
 		int temp_sonar 		= sonar.read();
 
-		if(abs(temp_sonar - sonar_alt) < 300){
+		// spike filter
+		if((temp_sonar - sonar_alt) < 50){
 			sonar_alt = temp_sonar;
 		}
 

ArduCopterMega/Attitude.pde

@@ -132,15 +132,15 @@ void calc_nav_throttle()
 
 	if(error < 0){
 		// try and prevent rapid fall
-		//scaler = (altitude_sensor == BARO) ? 1 : 1;
+		scaler = (altitude_sensor == BARO) ? .8 : .8;
 	}
 
 	if(altitude_sensor == BARO){
 		nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_medium_loop, scaler);	// .2
-		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -35, 80);
+		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
 	}else{
 		nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_medium_loop, scaler);	// .5
-		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 100);
+		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -40, 100);
 	}
 
 	// simple filtering

ArduCopterMega/GCS.h

@@ -110,8 +110,10 @@ public:
     // used by data_stream_send
     static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
     {
-        if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
-        else return false;
+        if (freq != 0 && freq >= freqMin && freq < freqMax)
+        	return true;
+        else
+        	return false;
     }
 
     // send streams which match frequency range

ArduCopterMega/GCS_Mavlink.pde

@@ -87,40 +87,42 @@ void
 GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
 {
 	if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
-		if (freqLoopMatch(streamRateRawSensors,freqMin,freqMax))
-    	send_message(MSG_RAW_IMU);
-
-		if (freqLoopMatch(streamRateExtendedStatus,freqMin,freqMax)) {
-        send_message(MSG_EXTENDED_STATUS);
-        send_message(MSG_GPS_STATUS);
-        send_message(MSG_CURRENT_WAYPOINT);
-        send_message(MSG_GPS_RAW);            // TODO - remove this message after location message is working
+
+		if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
+    		send_message(MSG_RAW_IMU);
+    	}
+
+		if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
+	        send_message(MSG_EXTENDED_STATUS);
+	        send_message(MSG_GPS_STATUS);
+	        send_message(MSG_CURRENT_WAYPOINT);
+	        send_message(MSG_GPS_RAW);            // TODO - remove this message after location message is working
 			send_message(MSG_NAV_CONTROLLER_OUTPUT);
-    }
+		}
 
-		if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) {
-		send_message(MSG_LOCATION);
-	}
+		if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
+			send_message(MSG_LOCATION);
+		}
 
-		if (freqLoopMatch(streamRateRawController,freqMin,freqMax)) {
-        // This is used for HIL.  Do not change without discussing with HIL maintainers
-        send_message(MSG_SERVO_OUT);
-    }
+		if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
+			// This is used for HIL.  Do not change without discussing with HIL maintainers
+			send_message(MSG_SERVO_OUT);
+		}
 
-		if (freqLoopMatch(streamRateRCChannels,freqMin,freqMax)) {
-        send_message(MSG_RADIO_OUT);
-        send_message(MSG_RADIO_IN);
-    }
+		if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
+			send_message(MSG_RADIO_OUT);
+			send_message(MSG_RADIO_IN);
+		}
 
-		if (freqLoopMatch(streamRateExtra1,freqMin,freqMax)){   // Use Extra 1 for AHRS info
-	    send_message(MSG_ATTITUDE);
-    }
+		if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){	 // Use Extra 1 for AHRS info
+			send_message(MSG_ATTITUDE);
+		}
 
-		if (freqLoopMatch(streamRateExtra2,freqMin,freqMax)){    // Use Extra 2 for additional HIL info
-    	send_message(MSG_VFR_HUD);
-    }
+		if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){		// Use Extra 2 for additional HIL info
+			send_message(MSG_VFR_HUD);
+		}
 
-		if (freqLoopMatch(streamRateExtra3,freqMin,freqMax)){
+		if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
         // Available datastream
 		}
     }

ArduCopterMega/Log.pde

@@ -482,15 +482,15 @@ void Log_Write_Control_Tuning()
 	DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
 
 	// Control
-	DataFlash.WriteInt((int)(g.rc_4.control_in));
-	DataFlash.WriteInt((int)(g.rc_4.servo_out));
+	DataFlash.WriteInt((int)(g.rc_4.control_in/100));
+	DataFlash.WriteInt((int)(g.rc_4.servo_out/100));
 
 	// yaw
 	DataFlash.WriteByte(yaw_debug);
 	DataFlash.WriteInt((int)(dcm.yaw_sensor/100));
 	DataFlash.WriteInt((int)(nav_yaw/100));
 	DataFlash.WriteInt((int)yaw_error/100);
-	DataFlash.WriteInt((int)(omega.z * 1000));
+	DataFlash.WriteInt((int)(omega.z * 100));
 
 	// Alt hold
 	DataFlash.WriteInt((int)(g.rc_3.servo_out));
@@ -499,7 +499,7 @@ void Log_Write_Control_Tuning()
 
 	DataFlash.WriteInt((int)next_WP.alt);				//
 	DataFlash.WriteInt((int)altitude_error);			//
-	DataFlash.WriteInt((int)g.pid_baro_throttle.get_integrator());
+	DataFlash.WriteInt((int)g.pid_sonar_throttle.get_integrator());
 
 	DataFlash.WriteByte(END_BYTE);
 }
@@ -521,7 +521,7 @@ void Log_Read_Control_Tuning()
 				DataFlash.ReadInt(),
 				DataFlash.ReadInt(),
 				DataFlash.ReadInt(),
-				(float)DataFlash.ReadInt() / 1000.0,// Gyro Rate
+				(float)DataFlash.ReadInt(),// Gyro Rate
 
 				// Alt Hold
 				DataFlash.ReadInt(),

ArduCopterMega/config.h

@@ -360,7 +360,7 @@
 # define YAW_I					0.01		// set to .0001 to try and get over user's steady state error caused by poor balance
 #endif
 #ifndef  YAW_D
-# define YAW_D					0.05		// Trying a lower value to prevent odd behavior
+# define YAW_D					0.08		// .7 = almost no yaw	Trying a lower value to prevent odd behavior
 #endif
 #ifndef  YAW_IMAX
 # define YAW_IMAX				1			// degrees * 100
@@ -438,16 +438,16 @@
 
 
 #ifndef THROTTLE_SONAR_P
-# define THROTTLE_SONAR_P		0.5			//
+# define THROTTLE_SONAR_P		0.35			// lowering P by .15
 #endif
 #ifndef THROTTLE_SONAR_I
-# define THROTTLE_SONAR_I		0.1
+# define THROTTLE_SONAR_I		0.05
 #endif
 #ifndef THROTTLE_SONAR_D
-# define THROTTLE_SONAR_D		0.06
+# define THROTTLE_SONAR_D		0.3				// increasing D by .5
 #endif
 #ifndef THROTTLE_SONAR_IMAX
-# define THROTTLE_SONAR_IMAX	60
+# define THROTTLE_SONAR_IMAX	20
 #endif
 
 

ArduCopterMega/setup.pde

@@ -409,6 +409,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
 
 	if (!strcmp_P(argv[1].str, PSTR("c"))) {
 		compass.set_offsets(_offsets);
+		compass.save_offsets();
 		report_compass();
 		return (0);
 	}

ArduCopterMega/system.pde

@@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "AC 2.0.18 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.19 Beta", main_menu_commands);
 
 void init_ardupilot()
 {