AC 2.0.32 Beta

new Loiter PIDs
tuned down control on standard frame
shunk the mixing range for sonar from 3 meters to 2 meters
added framework for Guided mode - not tested!
removed pitch and roll deadzone
decoupled SIMPLE mode from LOITER PIDs
Synced Mavlink with APM
Logs report correct WP number now.
disabled Sonar spike filter.


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

ArduCopterMega/APM_Config.h

@@ -5,6 +5,7 @@
 //#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 
 
+#define BROKEN_SLIDER		0		// 1 = yes (use Yaw to enter CLI mode)
 
 #define FRAME_CONFIG QUAD_FRAME
 	/*

ArduCopterMega/ArduCopterMega.pde

@@ -227,10 +227,6 @@ GPS         *g_gps;
 ////////////////////////////////////////////////////////////////////////////////
 // Global variables
 ////////////////////////////////////////////////////////////////////////////////
-
-byte 	control_mode		= STABILIZE;
-byte 	oldSwitchPosition;					// for remembering the control mode switch
-
 const char *comma = ",";
 
 const char* flight_mode_strings[] = {
@@ -239,7 +235,7 @@ const char* flight_mode_strings[] = {
 	"SIMPLE",
 	"ALT_HOLD",
 	"AUTO",
-	"GCS_AUTO",
+	"GUIDED",
 	"LOITER",
 	"RTL"};
 
@@ -257,8 +253,9 @@ const char* flight_mode_strings[] = {
 
 // Radio
 // -----
+byte 		control_mode		= STABILIZE;
+byte 		oldSwitchPosition;					// for remembering the control mode switch
 int 		motor_out[8];
-Vector3f omega;
 
 // Heli
 // ----
@@ -280,6 +277,7 @@ boolean		motor_auto_armed;				// if true,
 boolean rate_yaw_flag;						// used to transition yaw control from Rate control to Yaw hold
 byte 	yaw_debug;
 bool 	did_clear_yaw_control;
+Vector3f omega;
 
 // LED output
 // ----------
@@ -335,7 +333,7 @@ float	crosstrack_error;					// meters we are off trackline
 long 	distance_error;						// distance to the WP
 long 	yaw_error;							// how off are we pointed
 long	long_error, lat_error;				// temp for debugging
-
+int		loiter_error_max;
 // Battery Sensors
 // ---------------
 float	battery_voltage		= LOW_VOLTAGE * 1.05;		// Battery Voltage of total battery, initialized above threshold for filter
@@ -445,6 +443,7 @@ struct 	Location next_WP;					// next waypoint
 struct 	Location target_WP;					// where do we want to you towards?
 struct 	Location simple_WP;					//
 struct 	Location next_command;				// command preloaded
+struct  Location guided_WP;					// guided mode waypoint
 long 	target_altitude;					// used for
 boolean	home_is_set; 						// Flag for if we have g_gps lock and have set the home location
 
@@ -1094,14 +1093,17 @@ void update_current_flight_mode(void)
 					simple_WP.lat = 0;
 					simple_WP.lng = 0;
 
-					next_WP.lng =   (float)g.rc_1.control_in *.4;  // X: 4500 / 2 = 2250 = 25 meteres
+					next_WP.lng =  (float)g.rc_1.control_in * .9;  // X: 4500 * .7 = 2250 = 25 meteres
-					next_WP.lat = -((float)g.rc_2.control_in *.4); // Y: 4500 / 2 = 2250 = 25 meteres
+					next_WP.lat = -(float)g.rc_2.control_in * .9; // Y: 4500 * .7 = 2250 = 25 meteres
+					//next_WP.lng =  g.rc_1.control_in;  // X: 4500 * .7 = 2250 = 25 meteres
+					//next_WP.lat = -g.rc_2.control_in; // Y: 4500 * .7 = 2250 = 25 meteres
 
 					// calc a new bearing
 					nav_bearing 	= get_bearing(&simple_WP, &next_WP) + initial_simple_bearing;
 					nav_bearing 	= wrap_360(nav_bearing);
 					wp_distance 	= get_distance(&simple_WP, &next_WP);
 					calc_bearing_error();
+
 					/*
 					Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ",
 							next_WP.lat,
@@ -1179,6 +1181,7 @@ void update_current_flight_mode(void)
 				output_stabilize_pitch();
 				break;
 
+			case GUIDED:
 			case RTL:
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
@@ -1252,6 +1255,10 @@ void update_navigation()
 		case RTL:
 			// calculates desired Yaw
 			update_nav_yaw();
+
+		case GUIDED:
+			update_nav_yaw();
+			// switch passthrough to LOITER
 		case LOITER:
 			// are we Traversing or Loitering?
 			wp_control = (wp_distance < 50) ? LOITER_MODE : WP_MODE;
@@ -1325,9 +1332,11 @@ void update_alt()
 		int temp_sonar 		= sonar.read();
 
 		// spike filter
-		if((temp_sonar - sonar_alt) < 50){
+		//if((temp_sonar - sonar_alt) < 50){
+		//	sonar_alt = temp_sonar;
+		//}
+
 		sonar_alt = temp_sonar;
-		}
 
 		/*
 		doesn't really seem to be a need for this using EZ0:
@@ -1337,7 +1346,7 @@ void update_alt()
 		*/
 
 		if(baro_alt < 800){
-			scale = (sonar_alt - 300) / 300;
+			scale = (sonar_alt - 400) / 200;
 			scale = constrain(scale, 0, 1);
 
 			current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
@@ -1430,6 +1439,7 @@ void tuning(){
 
 void update_nav_wp()
 {
+	// XXX Guided mode!!!
 	if(wp_control == LOITER_MODE){
 		// calc a pitch to the target
 		calc_loiter_nav();

ArduCopterMega/GCS_Mavlink.pde

@@ -39,16 +39,6 @@ GCS_MAVLINK::init(BetterStream * port)
 		chan = MAVLINK_COMM_1;
 	}
 	_queued_parameter = NULL;
-
-	// temporary
-	streamRateRawSensors.set_and_save(0);
-	streamRateExtendedStatus.set_and_save(0);
-	streamRateRCChannels.set_and_save(0);
-	streamRateRawController.set_and_save(0);
-	streamRatePosition.set_and_save(0);
-	streamRateExtra1.set_and_save(0);
-	streamRateExtra2.set_and_save(0);
-	streamRateExtra3.set_and_save(0);
 }
 
 void
@@ -88,18 +78,6 @@ GCS_MAVLINK::update(void)
 	}
 }
 
-/*
-mav2 1
-mav3 3
-mav5 3
-mav6 10
-mav7 10
-mav6 10
-mav7 10
-mav6 10
-mav7 10
-*/
-
 void
 GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
 {
@@ -186,7 +164,9 @@ GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
 
 void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 {
-	struct 	Location tell_command;				// command for telemetry
+	struct Location tell_command = {};				// command for telemetry
+	static uint8_t mav_nav = 255;							// For setting mode (some require receipt of 2 messages...)
+
 	switch (msg->msgid) {
 
 	case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
@@ -219,7 +199,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					streamRateExtra2			= freq;
 					//streamRateExtra3.set_and_save(freq);	// We just do set and save on the last as it takes care of the whole group.
 					streamRateExtra3			= freq;	// Don't save!!
-
 					break;
 
 				case MAV_DATA_STREAM_RAW_SENSORS:
@@ -238,9 +217,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 				case MAV_DATA_STREAM_RAW_CONTROLLER:
 					streamRateRawController = freq;
 					break;
+
 				//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
 				//	streamRateRawSensorFusion.set_and_save(freq);
 				//	break;
+
 				case MAV_DATA_STREAM_POSITION:
 					streamRatePosition = freq;
 					break;
@@ -251,7 +232,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 				case MAV_DATA_STREAM_EXTRA2:
 					streamRateExtra2 = freq;
-					//streamRateExtra2.set_and_save(freq);
 					break;
 
 				case MAV_DATA_STREAM_EXTRA3:
@@ -339,10 +319,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 				case MAV_ACTION_CALIBRATE_MAG:
 				case MAV_ACTION_CALIBRATE_ACC:
 				case MAV_ACTION_CALIBRATE_PRESSURE:
-				case MAV_ACTION_REBOOT:  // this is a rough interpretation
+				//case MAV_ACTION_REBOOT:  // this is a rough interpretation
 					//startup_IMU_ground();
 					//result=1;
-					break;
+				//	break;
 
 				/*	For future implemtation
 				case MAV_ACTION_REC_START: break;
@@ -384,6 +364,45 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			break;
 		}
 
+	case MAVLINK_MSG_ID_SET_MODE:
+		{
+			// decode
+			mavlink_set_mode_t packet;
+			mavlink_msg_set_mode_decode(msg, &packet);
+
+			switch(packet.mode){
+
+				case MAV_MODE_MANUAL:
+					set_mode(STABILIZE);
+					break;
+
+				case MAV_MODE_GUIDED:
+					set_mode(GUIDED);
+					break;
+
+				case MAV_MODE_AUTO:
+					if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) 	set_mode(AUTO);
+					if(mav_nav == MAV_NAV_RETURNING)					set_mode(RTL);
+					if(mav_nav == MAV_NAV_LOITER)						set_mode(LOITER);
+					mav_nav = 255;
+					break;
+
+				case MAV_MODE_TEST1:
+					set_mode(STABILIZE);
+					break;
+			}
+		}
+
+	/*case MAVLINK_MSG_ID_SET_NAV_MODE:
+		{
+			// decode
+			mavlink_set_nav_mode_t packet;
+			mavlink_msg_set_nav_mode_decode(msg, &packet);
+			// To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message
+			mav_nav = packet.nav_mode;
+			break;
+		}
+	*/
 	case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
 		{
 			//send_text_P(SEVERITY_LOW,PSTR("waypoint request list"));
@@ -415,8 +434,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			//send_text_P(SEVERITY_LOW,PSTR("waypoint request"));
 
 			// Check if sending waypiont
-			if (!waypoint_sending)
+			//if (!waypoint_sending) break;
-				break;
+			// 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request.  DEW
 
 			// decode
 			mavlink_waypoint_request_t packet;
@@ -442,7 +461,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			// time that the mav should loiter in milliseconds
 			uint8_t current = 0; // 1 (true), 0 (false)
 
-			if (packet.seq == g.waypoint_index)
+			if (packet.seq == (uint16_t)g.waypoint_index)
 				current = 1;
 
 			uint8_t autocontinue = 1; // 1 (true), 0 (false)
@@ -471,7 +490,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					break;
 
 				case MAV_CMD_NAV_LOITER_TIME:
-					param1 = tell_command.p1;	// APM loiter time is in ten second increments
+					param1 = tell_command.p1;	// ACM loiter time is in 1 second increments
 					break;
 
 				case MAV_CMD_CONDITION_DELAY:
@@ -534,9 +553,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			mavlink_msg_waypoint_ack_decode(msg, &packet);
 			if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
-			// check for error
-			uint8_t type = packet.type; // ok (0), error(1)
-
 			// turn off waypoint send
 			waypoint_sending = false;
 			break;
@@ -621,20 +637,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 	// XXX receive a WP from GCS and store in EEPROM
 	case MAVLINK_MSG_ID_WAYPOINT:
 		{
-			// Check if receiving waypiont
-			if (!waypoint_receiving) break;
-
 			// decode
 			mavlink_waypoint_t packet;
 			mavlink_msg_waypoint_decode(msg, &packet);
 			if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
-			// check if this is the requested waypoint
-			if (packet.seq != waypoint_request_i) break;
-
-			// store waypoint
-			uint8_t loadAction = 0; // 0 insert in list, 1 exec now
-
 			// defaults
 			tell_command.id = packet.command;
 
@@ -728,13 +735,39 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					break;
 			}
 
+			if(packet.current == 2){ 				//current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
+				guided_WP = tell_command;
+
+				// add home alt if needed
+				if (guided_WP.options & WP_OPTION_ALT_RELATIVE){
+					guided_WP.alt += home.alt;
+				}
+
+				set_mode(GUIDED);
+
+				// make any new wp uploaded instant (in case we are already in Guided mode)
+				set_next_WP(&guided_WP);
+
+				// verify we recevied the command
+				mavlink_msg_waypoint_ack_send(
+						chan,
+						msg->sysid,
+						msg->compid,
+						0);
+
+			} else {
+				// Check if receiving waypoints (mission upload expected)
+				if (!waypoint_receiving) break;
+
+				// check if this is the requested waypoint
+				if (packet.seq != waypoint_request_i) break;
 					set_command_with_index(tell_command, packet.seq);
 
 			// update waypoint receiving state machine
 			waypoint_timelast_receive = millis();
 			waypoint_request_i++;
 
-			if (waypoint_request_i > g.waypoint_total){
+			if (waypoint_request_i > (uint16_t)g.waypoint_total){
 				uint8_t type = 0; // ok (0), error(1)
 
 				mavlink_msg_waypoint_ack_send(
@@ -748,6 +781,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 				// XXX ignores waypoint radius for individual waypoints, can
 				// only set WP_RADIUS parameter
 				}
+			}
 			break;
 		}
 
@@ -759,6 +793,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			// decode
 			mavlink_param_set_t packet;
 			mavlink_msg_param_set_decode(msg, &packet);
+
 			if (mavlink_check_target(packet.target_system, packet.target_component))
 				break;
 
@@ -777,7 +812,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 				// add a small amount before casting parameter values
 				// from float to integer to avoid truncating to the
 				// next lower integer value.
-				const float rounding_addition = 0.01;
+				float rounding_addition = 0.01;
 
 				// fetch the variable type ID
 				var_type = vp->meta_type_id();
@@ -790,12 +825,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					((AP_Float16 *)vp)->set_and_save(packet.param_value);
 
 				} else if (var_type == AP_Var::k_typeid_int32) {
+					if (packet.param_value < 0) rounding_addition = -rounding_addition;
 					((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
-
 				} else if (var_type == AP_Var::k_typeid_int16) {
+					if (packet.param_value < 0) rounding_addition = -rounding_addition;
 					((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
-
 				} else if (var_type == AP_Var::k_typeid_int8) {
+					if (packet.param_value < 0) rounding_addition = -rounding_addition;
 					((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
 				} else {
 					// we don't support mavlink set on this parameter
@@ -816,16 +852,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 			break;
 		} // end case
-
+/*
-#if ALLOW_RC_OVERRIDE == ENABLED
+	case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
-	case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
 		{
 			// allow override of RC channel values for HIL
 			// or for complete GCS control of switch position
 			// and RC PWM values.
-			mavlink_rc_channels_raw_t packet;
+			if(msg->sysid != g.sysid_my_gcs) break;		// Only accept control from our gcs
+			mavlink_rc_channels_override_t packet;
 			int16_t v[8];
-			mavlink_msg_rc_channels_raw_decode(msg, &packet);
+			mavlink_msg_rc_channels_override_decode(msg, &packet);
+
+			if (mavlink_check_target(packet.target_system,packet.target_component))
+				break;
+
 			v[0] = packet.chan1_raw;
 			v[1] = packet.chan2_raw;
 			v[2] = packet.chan3_raw;
@@ -834,12 +874,109 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			v[5] = packet.chan6_raw;
 			v[6] = packet.chan7_raw;
 			v[7] = packet.chan8_raw;
-			APM_RC.setHIL(v);
+			rc_override_active = APM_RC.setHIL(v);
+			rc_override_fs_timer = millis();
+			break;
+		}
+
+	case MAVLINK_MSG_ID_HEARTBEAT:
+		{
+			// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
+			if(msg->sysid != g.sysid_my_gcs) break;
+			rc_override_fs_timer = millis();
+			//pmTest1++;
+			break;
+		}
+
+	#if HIL_MODE != HIL_MODE_DISABLED
+		// This is used both as a sensor and to pass the location
+		// in HIL_ATTITUDE mode.
+	case MAVLINK_MSG_ID_GPS_RAW:
+		{
+			// decode
+			mavlink_gps_raw_t packet;
+			mavlink_msg_gps_raw_decode(msg, &packet);
+
+			// set gps hil sensor
+			g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt,
+			packet.v,packet.hdg,0,0);
+			break;
+		}
+
+		//	Is this resolved? - MAVLink protocol change.....
+	case MAVLINK_MSG_ID_VFR_HUD:
+		{
+			// decode
+			mavlink_vfr_hud_t packet;
+			mavlink_msg_vfr_hud_decode(msg, &packet);
+
+			// set airspeed
+			airspeed = 100*packet.airspeed;
+			break;
+		}
+
+#endif
+#if HIL_MODE == HIL_MODE_ATTITUDE
+	case MAVLINK_MSG_ID_ATTITUDE:
+		{
+			// decode
+			mavlink_attitude_t packet;
+			mavlink_msg_attitude_decode(msg, &packet);
+
+			// set dcm hil sensor
+			dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
+			packet.pitchspeed,packet.yawspeed);
 			break;
 		}
 #endif
+#if HIL_MODE == HIL_MODE_SENSORS
 
+    case MAVLINK_MSG_ID_RAW_IMU:
+        {
+            // decode
+            mavlink_raw_imu_t packet;
+            mavlink_msg_raw_imu_decode(msg, &packet);
 
+            // set imu hil sensors
+            // TODO: check scaling for temp/absPress
+            float temp = 70;
+            float absPress = 1;
+            //      Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc);
+            //      Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
+
+            // rad/sec
+            Vector3f gyros;
+            gyros.x = (float)packet.xgyro / 1000.0;
+            gyros.y = (float)packet.ygyro / 1000.0;
+            gyros.z = (float)packet.zgyro / 1000.0;
+            // m/s/s
+            Vector3f accels;
+            accels.x = (float)packet.xacc / 1000.0;
+            accels.y = (float)packet.yacc / 1000.0;
+            accels.z = (float)packet.zacc / 1000.0;
+
+            imu.set_gyro(gyros);
+
+            imu.set_accel(accels);
+
+            compass.setHIL(packet.xmag,packet.ymag,packet.zmag);
+            break;
+        }
+
+    case MAVLINK_MSG_ID_RAW_PRESSURE:
+        {
+            // decode
+            mavlink_raw_pressure_t packet;
+            mavlink_msg_raw_pressure_decode(msg, &packet);
+
+            // set pressure hil sensor
+            // TODO: check scaling
+            float temp = 70;
+            barometer.setHIL(temp,packet.press_diff1);
+            break;
+        }
+#endif // HIL_MODE
+*/
 	} // end switch
 } // end handle mavlink
 

ArduCopterMega/Log.pde

@@ -73,7 +73,7 @@ print_log_menu(void)
 	Serial.println();
 
 	if (last_log_num == 0) {
-		Serial.printf_P(PSTR("\nNo logs\n"));
+		Serial.printf_P(PSTR("\nNo logs\nType 'dump 0'.\n\n"));
 	}else{
 		Serial.printf_P(PSTR("\n%d logs\n"), last_log_num);
 
@@ -107,6 +107,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 	if (/*(argc != 2) || */ (dump_log < 1) || (dump_log > last_log_num)) {
 		Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
 		Log_Read(1, 4095);
+		erase_logs(NULL, NULL);
 		return(-1);
 	}
 

ArduCopterMega/Mavlink_Common.h

@@ -56,6 +56,9 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 					mode 		= MAV_MODE_AUTO;
 					nav_mode 	= MAV_NAV_RETURNING;
 					break;
+				case GUIDED:
+					mode 		= MAV_MODE_GUIDED;
+					break;
 				default:
 					mode 		= control_mode + 100;
 
@@ -63,7 +66,6 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 
 			uint8_t status 		= MAV_STATE_ACTIVE;
 			uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity;	//Mavlink scaling 100% = 1000
-			uint8_t motor_block = false;
 
 			mavlink_msg_sys_status_send(
 					chan,
@@ -199,9 +201,9 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 					motor_out[2],
 					motor_out[3],
 					motor_out[4],
-					g.rc_5.radio_out,
+					motor_out[7],
-					g.rc_6.radio_out,
+					motor_out[8],
-					motor_out[7]);
+					0);
 			break;
 		}
 
@@ -238,13 +240,14 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 					compass.mag_y,
 					compass.mag_z);
 
-			mavlink_msg_raw_pressure_send(
+			/*  This message is not working.  Why?
+			mavlink_msg_scaled_pressure_send(
 					chan,
 					timeStamp,
-					adc.Ch(AIRSPEED_CH),
+					(float)barometer.Press/100.,
-					barometer.RawPress,
+					(float)adc.Ch(AIRSPEED_CH),		// We don't calculate the differential pressure value anywhere, so use raw
-					0,
+					(int)(barometer.Temp*100));
-					0);
+			*/
 			break;
 		}
 		#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE

ArduCopterMega/commands.pde

@@ -164,8 +164,8 @@ It precalculates all the necessary stuff.
 
 void set_next_WP(struct Location *wp)
 {
-	SendDebug("MSG <set_next_wp> wp_index: ");
+	//SendDebug("MSG <set_next_wp> wp_index: ");
-	SendDebugln(g.waypoint_index, DEC);
+	//SendDebugln(g.waypoint_index, DEC);
 	gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
 
 	// copy the current WP into the OldWP slot
@@ -180,11 +180,6 @@ void set_next_WP(struct Location *wp)
 	// -----------------------------------------------------------------
 	target_altitude = current_loc.alt;
 
-	// zero out our loiter vals to watch for missed waypoints
-	//loiter_delta 		= 0;
-	//loiter_sum 			= 0;
-	//loiter_total 		= 0;
-
 	// this is used to offset the shrinking longitude as we go towards the poles
 	float rads 			= (abs(next_WP.lat)/t7) * 0.0174532925;
 	scaleLongDown 		= cos(rads);
@@ -192,8 +187,6 @@ void set_next_WP(struct Location *wp)
 
 	// this is handy for the groundstation
 	wp_totalDistance 	= get_distance(&current_loc, &next_WP);
-
-	// this makes the data not log for a GPS read
 	wp_distance 		= wp_totalDistance;
 	target_bearing 		= get_bearing(&current_loc, &next_WP);
 	nav_bearing 		= target_bearing;
@@ -209,6 +202,7 @@ void set_next_WP(struct Location *wp)
 	gcs.print_current_waypoints();
 }
 
+
 // run this at setup on the ground
 // -------------------------------
 void init_home()

ArduCopterMega/commands_process.pde

@@ -94,7 +94,7 @@ process_next_command()
 
 			// Save CMD to Log
 			if (g.log_bitmask & MASK_LOG_CMD)
-				Log_Write_Cmd(g.waypoint_index, &next_command);
+				Log_Write_Cmd(g.waypoint_index + 1, &next_command);
 
 			// Act on the new command
 			process_must();
@@ -117,7 +117,7 @@ process_next_command()
 
 			// Save CMD to Log
 			if (g.log_bitmask & MASK_LOG_CMD)
-				Log_Write_Cmd(g.waypoint_index, &next_command);
+				Log_Write_Cmd(g.waypoint_index + 1, &next_command);
 
 			process_may();
 			return true;
@@ -130,7 +130,7 @@ process_next_command()
 			//SendDebugln(next_command.id,DEC);
 
 			if (g.log_bitmask & MASK_LOG_CMD)
-				Log_Write_Cmd(g.waypoint_index, &next_command);
+				Log_Write_Cmd(g.waypoint_index + 1, &next_command);
 			process_now();
 			return true;
 		}

ArduCopterMega/config.h

@@ -329,26 +329,26 @@
 // STABILZE Angle Control
 //
 #ifndef STABILIZE_ROLL_P
-# define STABILIZE_ROLL_P 		0.54 // .48 = 4.0 NG, .54 = 4.5 NG
+# define STABILIZE_ROLL_P 		0.48 // .48 = 4.0 NG, .54 = 4.5 NG
 #endif
 #ifndef STABILIZE_ROLL_I
 # define STABILIZE_ROLL_I 		0.025		//
 #endif
 #ifndef STABILIZE_ROLL_D
-# define STABILIZE_ROLL_D 		0.12
+# define STABILIZE_ROLL_D 		0.18
 #endif
 #ifndef STABILIZE_ROLL_IMAX
 # define STABILIZE_ROLL_IMAX 	.5		// degrees * 100
 #endif
 
 #ifndef STABILIZE_PITCH_P
-# define STABILIZE_PITCH_P		0.54
+# define STABILIZE_PITCH_P		0.48
 #endif
 #ifndef STABILIZE_PITCH_I
 # define STABILIZE_PITCH_I		0.025
 #endif
 #ifndef STABILIZE_PITCH_D
-# define STABILIZE_PITCH_D		0.12
+# define STABILIZE_PITCH_D		0.18
 #endif
 #ifndef STABILIZE_PITCH_IMAX
 # define STABILIZE_PITCH_IMAX	.5		// degrees * 100
@@ -403,16 +403,16 @@
 // Navigation control gains
 //
 #ifndef NAV_LOITER_P
-# define NAV_LOITER_P			2.5			// upped to be a bit more aggressive
+# define NAV_LOITER_P			1.2			// upped to be a bit more aggressive
 #endif
 #ifndef NAV_LOITER_I
 # define NAV_LOITER_I			0.08		// upped a bit to deal with wind faster
 #endif
 #ifndef NAV_LOITER_D
-# define NAV_LOITER_D			0.15		//
+# define NAV_LOITER_D			0.8		//
 #endif
 #ifndef NAV_LOITER_IMAX
-# define NAV_LOITER_IMAX		20			// 20°
+# define NAV_LOITER_IMAX		15			// 20°
 #endif
 
 

ArduCopterMega/control_modes.pde

@@ -56,6 +56,7 @@ void read_trim_switch()
 	}else{ // switch is disengaged
 
 		if(trim_flag){
+
 			// set the throttle nominal
 			if(g.rc_3.control_in > 150){
 				g.throttle_cruise.set_and_save(g.rc_3.control_in);
@@ -69,11 +70,16 @@ void read_trim_switch()
 void auto_trim()
 {
 	if(auto_level_counter > 0){
+		g.rc_1.dead_zone = 60;		// 60 = .6 degrees
+		g.rc_2.dead_zone = 60;
+
 		auto_level_counter--;
 		trim_accel();
 		led_mode = AUTO_TRIM_LEDS;
 
 		if(auto_level_counter == 1){
+			g.rc_1.dead_zone = 0;		// 60 = .6 degrees
+			g.rc_2.dead_zone = 0;
 			led_mode = NORMAL_LEDS;
 			clear_leds();
 			imu.save();

ArduCopterMega/defines.h

@@ -115,7 +115,7 @@
 #define SIMPLE 2			//
 #define ALT_HOLD 3			// AUTO control
 #define AUTO 4				// AUTO control
-#define GCS_AUTO 5			// AUTO control
+#define GUIDED 5			// AUTO control
 #define LOITER 6			// Hold a single location
 #define RTL 7				// AUTO control
 #define NUM_MODES 8

ArduCopterMega/navigation.pde

@@ -62,15 +62,16 @@ void calc_loiter_nav()
 	// Y PITCH
 	lat_error	= current_loc.lat - next_WP.lat;							// 0 - 500 = -500 pitch NORTH
 
-	long_error	= constrain(long_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
+	// constrain input, not output to let I term ramp up and do it's job again wind
-	lat_error	= constrain(lat_error,  -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
+	long_error	= constrain(long_error, -loiter_error_max, loiter_error_max); // +- 20m max error
+	lat_error	= constrain(lat_error,  -loiter_error_max, loiter_error_max); // +- 20m max error
 
 	nav_lon		= g.pid_nav_lon.get_pid(long_error, dTnav, 1.0);		// X 700 * 2.5 = 1750,
 	nav_lat 	= g.pid_nav_lat.get_pid(lat_error,  dTnav, 1.0);		// Y invert lat (for pitch)
 
-	long pmax 	= g.pitch_max.get();
+	//long pmax 	= g.pitch_max.get();
-	nav_lon 	= constrain(nav_lon, -pmax, pmax);
+	//nav_lon 	= constrain(nav_lon, -pmax, pmax);
-	nav_lat 	= constrain(nav_lat, -pmax, pmax);
+	//nav_lat 	= constrain(nav_lat, -pmax, pmax);
 }
 
 void calc_loiter_output()
@@ -110,9 +111,9 @@ void calc_loiter_output()
 void calc_simple_nav()
 {
 	// no dampening here in SIMPLE mode
-	nav_lat	= constrain((wp_distance * 100), -1800, 1800); // +- 20m max error
+	nav_lat	= constrain((wp_distance * 100), -4500, 4500); // +- 20m max error
 	// Scale response by kP
-	nav_lat	*= g.pid_nav_lat.kP();	// 1800 * 2 = 3600 or 36°
+	//nav_lat	*= g.pid_nav_lat.kP();	// 1800 * 2 = 3600 or 36°
 }
 
 void calc_nav_output()

ArduCopterMega/radio.pde

@@ -19,9 +19,10 @@ void init_rc_in()
 	g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW);
 
 	// set rc dead zones
-	g.rc_1.dead_zone = 60;		// 60 = .6 degrees
+	g.rc_1.dead_zone = 0;		// 60 = .6 degrees
-	g.rc_2.dead_zone = 60;
+	g.rc_2.dead_zone = 0;
 	g.rc_3.dead_zone = 60;
+
 	#if YAW_OPTION == 1
 	g.rc_4.dead_zone = 500;// 1 = offset Yaw approach
 	#else

ArduCopterMega/read_me.text

@@ -57,14 +57,14 @@ Set these up in 'setup'/'modes'. Use your three position switch (channel 5) to s
 All of the modes allow the user to control the copter with the normal controls. You can get yourself out of a jam sometimes by simply nudging the copter while in AUTO or LOITER modes.
 Options include:
 ACRO		- rate control only. not for beginners
-STABILIZE	- copter will hold -45 to 45° angle, throttle is manual.
+STABILIZE	- copter will hold any angle from -45 to 45° based on roll and pitch stick input. Manual throttle.
-SIMPLE		- Remembers the orientation of the copter when first entering SIMPLE mode, allowing the user to fly more intuitivey. Manual Throttle.
+SIMPLE		- Remembers the yaw orientation of the copter when first entering SIMPLE mode, allowing the user to fly more intuitivey. Manual throttle.
 ALT_HOLD	- altitude is controlled by the throttle lever. Middle is hold, high = rise, low = fall.
 LOITER 		- When selected, it will hold the current altitude, position and yaw. Yaw is user controllable. roll and pitch can be overridden temporarily with the radio.
 				altitude is controlled by the throttle lever. Middle is hold, high = rise, low = fall.
 RTL 		- Will try and fly back to home at the current altitude.
 AUTO 		- Will fly the mission loaded by the Waypoint writer
-GCS_AUTO	- A future mode where the copter can be flown interactively from the GCS
+GUIDED	- A future mode where the copter can be flown interactively from the GCS
 
 
 Special note:

ArduCopterMega/system.pde

@@ -40,7 +40,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "AC 2.0.31 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.32 Beta", main_menu_commands);
 
 void init_ardupilot()
 {
@@ -332,6 +332,10 @@ void startup_ground(void)
 	report_gps();
 	g_gps->idleTimeout = 20000;
 
+	// used to limit the input of error for loiter
+	loiter_error_max = (float)g.pitch_max.get() / (float)g.pid_nav_lat.kP();
+	//Serial.printf_P(PSTR("\nloiter: %d\n"), loiter_error_max);
+
 	Log_Write_Startup();
 
 	SendDebug("\nReady to FLY ");
@@ -391,6 +395,10 @@ void set_mode(byte mode)
 			do_loiter_at_location();
 			break;
 
+		case GUIDED:
+			set_next_WP(&guided_WP);
+			break;
+
 		case RTL:
 			//init_throttle_cruise();
 			do_RTL();
@@ -485,11 +493,17 @@ init_throttle_cruise()
 boolean
 check_startup_for_CLI()
 {
-	if(abs(g.rc_4.control_in) > 3000){
+	//return true;
-		// startup to fly
+	if((g.rc_4.radio_max) < 1600){
-		return true;
-	}else{
 		// CLI mode
+		return true;
+
+	}else if(abs(g.rc_4.control_in) > 3000){
+		// CLI mode
+		return true;
+
+	}else{
+		// startup to fly
 		return false;
 	}
 }