mavlink: fixed build with MAVLink and HIL

this fixes the build with HIL_MODE_ATTITUDE and MAVLink enabled. Basic
MAVLink operation works.

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

ArduCopterMega/ArduCopterMega.pde

@@ -360,7 +360,7 @@ struct 	Location home;						// home location
 struct 	Location prev_WP;					// last waypoint
 struct 	Location current_loc;				// current location
 struct 	Location next_WP;					// next waypoint
-//struct 	Location tell_command;				// command for telemetry
+struct 	Location tell_command;				// command for telemetry
 struct 	Location next_command;				// command preloaded
 long 	target_altitude;					// used for
 long 	offset_altitude;					// used for
@@ -559,8 +559,10 @@ void medium_loop()
 			if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && (g.log_bitmask & MASK_LOG_ATTITUDE_FAST == 0))
 				Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
 
+#if HIL_MODE != HIL_MODE_ATTITUDE
 			if (g.log_bitmask & MASK_LOG_CTUN)
 				Log_Write_Control_Tuning();
+#endif
 
 			if (g.log_bitmask & MASK_LOG_NTUN)
 				Log_Write_Nav_Tuning();
@@ -608,8 +610,10 @@ void medium_loop()
 	if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
 		Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
 
+#if HIL_MODE != HIL_MODE_ATTITUDE
 	if (g.log_bitmask & MASK_LOG_RAW)
 		Log_Write_Raw();
+#endif
 
 	#if GCS_PROTOCOL == 6		// This is here for Benjamin Pelletier.	Please do not remove without checking with me.	Doug W
 		readgcsinput();
@@ -998,6 +1002,9 @@ void update_trig(void){
 
 void update_alt()
 {
+#if HIL_MODE == HIL_MODE_ATTITUDE
+	current_loc.alt = g_gps->altitude;
+#else
 	altitude_sensor = BARO;
 	baro_alt 		= read_barometer();
 	//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
@@ -1029,6 +1036,7 @@ void update_alt()
 		current_loc.alt = baro_alt + home.alt;
 	}
 	//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
+#endif
 
 	// altitude smoothing
 	// ------------------
@@ -1037,4 +1045,4 @@ void update_alt()
 	// Amount of throttle to apply for hovering
 	// ----------------------------------------
 	calc_nav_throttle();
-}
+}

ArduCopterMega/GCS_Mavlink.pde

@@ -223,7 +223,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             case MAV_ACTION_EMCY_KILL:
             case MAV_ACTION_MOTORS_STOP:
             case MAV_ACTION_SHUTDOWN:
-                set_mode(MANUAL);
+                set_mode(ACRO);
                 break;
 
             case MAV_ACTION_CONTINUE:
@@ -231,7 +231,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 break;
 
             case MAV_ACTION_SET_MANUAL:
-                set_mode(MANUAL);
+                set_mode(ACRO);
                 break;
 
             case MAV_ACTION_SET_AUTO:
@@ -265,7 +265,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             case MAV_ACTION_CALIBRATE_ACC:
             case MAV_ACTION_CALIBRATE_PRESSURE:
             case MAV_ACTION_REBOOT:  // this is a rough interpretation
-                startup_IMU_ground();
+                startup_ground();
                 break;
 
             case MAV_ACTION_REC_START: break;

ArduCopterMega/Mavlink_Common.h

@@ -1,3 +1,5 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
 #ifndef Mavlink_Common_H
 #define Mavlink_Common_H
 
@@ -39,40 +41,37 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
         uint8_t nav_mode = MAV_NAV_VECTOR;
 
 		switch(control_mode) {
-		case MANUAL:
-			mode 		= MAV_MODE_MANUAL;
-			break;
-		case CIRCLE:
-			mode 		= MAV_MODE_TEST3;
+		case ACRO:
+			mode = MAV_MODE_MANUAL;
 			break;
 		case STABILIZE:
-			mode 		= MAV_MODE_GUIDED;
+			mode = MAV_MODE_GUIDED;
+			break;
+		case FBW:
+			mode = MAV_MODE_TEST1;
 			break;
-		case FLY_BY_WIRE_A:
-			mode 		= MAV_MODE_TEST1;
+		case ALT_HOLD:
+			mode = MAV_MODE_TEST2;
 			break;
-		case FLY_BY_WIRE_B:
-			mode 		= MAV_MODE_TEST2;
+		case POSITION_HOLD:
+			mode = MAV_MODE_AUTO;
+			nav_mode = MAV_NAV_HOLD;
 			break;
 		case AUTO:
-			mode 		= MAV_MODE_AUTO;
-			nav_mode 	= MAV_NAV_WAYPOINT;
+			mode = MAV_MODE_AUTO;
+			nav_mode = MAV_NAV_WAYPOINT;
 			break;
 		case RTL:
-			mode 		= MAV_MODE_AUTO;
-			nav_mode 	= MAV_NAV_RETURNING;
-			break;
-		case LOITER:
-			mode 		= MAV_MODE_AUTO;
-			nav_mode 	= MAV_NAV_HOLD;
+			mode = MAV_MODE_AUTO;
+			nav_mode = MAV_NAV_RETURNING;
 			break;
 		case TAKEOFF:
-			mode 		= MAV_MODE_AUTO;
-			nav_mode 	= MAV_NAV_LIFTOFF;
+			mode = MAV_MODE_AUTO;
+			nav_mode = MAV_NAV_LIFTOFF;
 			break;
 		case LAND:
-			mode 		= MAV_MODE_AUTO;
-			nav_mode 	= MAV_NAV_LANDING;
+			mode = MAV_MODE_AUTO;
+			nav_mode = MAV_NAV_LANDING;
 			break;
 		}
 		uint8_t status 		= MAV_STATE_ACTIVE;
@@ -155,50 +154,40 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
         uint8_t rssi = 1;
 		// normalized values scaled to -10000 to 10000
 		// This is used for HIL.  Do not change without discussing with HIL maintainers
-        mavlink_msg_rc_channels_scaled_send(
-				chan,
-				10000 * g.channel_roll.norm_output(),
-				10000 * g.channel_pitch.norm_output(),
-				10000 * g.channel_throttle.norm_output(),
-				10000 * g.channel_rudder.norm_output(),
-				0,
-				0,
-				0,
-				0,
-				rssi);
+        mavlink_msg_rc_channels_scaled_send(chan,
+											g.rc_1.norm_output(),
+											g.rc_2.norm_output(),
+											g.rc_3.norm_output(),
+											g.rc_4.norm_output(),
+											0,0,0,0,rssi);
         break;
     }
 
     case MSG_RADIO_IN:
     {
         uint8_t rssi = 1;
-        mavlink_msg_rc_channels_raw_send(
-				chan,
-				g.channel_roll.radio_in,
-				g.channel_pitch.radio_in,
-				g.channel_throttle.radio_in,
-				g.channel_rudder.radio_in,
-				g.rc_5.radio_in,       // XXX currently only 4 RC channels defined
-				g.rc_6.radio_in,
-				g.rc_7.radio_in,
-				g.rc_8.radio_in,
-				rssi);
+        mavlink_msg_rc_channels_raw_send(chan,
+										 g.rc_1.radio_in,
+										 g.rc_2.radio_in,
+										 g.rc_3.radio_in,
+										 g.rc_4.radio_in,
+										 g.rc_5.radio_in,
+										 g.rc_6.radio_in,
+										 g.rc_7.radio_in,
+										 g.rc_8.radio_in,
+										 rssi);
         break;
     }
 
     case MSG_RADIO_OUT:
     {
-        mavlink_msg_servo_output_raw_send(
-				chan,
-				g.channel_roll.radio_out,
-				g.channel_pitch.radio_out,
-				g.channel_throttle.radio_out,
-				g.channel_rudder.radio_out,
-				g.rc_5.radio_out,       // XXX currently only 4 RC channels defined
-				g.rc_6.radio_out,
-				g.rc_7.radio_out,
-				g.rc_8.radio_out);
-        break;
+	    mavlink_msg_servo_output_raw_send(chan,
+										  motor_out[0],
+										  motor_out[1],
+										  motor_out[2],
+										  motor_out[3],
+										  0, 0, 0, 0);
+	    break;
     }
 
     case MSG_VFR_HUD:
@@ -210,7 +199,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 				dcm.yaw_sensor,
 				current_loc.alt / 100.0,
 				climb_rate,
-				(int)g.channel_throttle.servo_out);
+				nav_throttle);
         break;
     }
 

ArduCopterMega/commands_logic.pde

@@ -189,7 +189,7 @@ verify_nav_wp()
 	// add in a more complex case
 	// Doug to do
 	if(loiter_sum > 300){
-		send_message(SEVERITY_MEDIUM,"Missed WP");
+		gcs.send_text(SEVERITY_MEDIUM,"Missed WP");
 		return true;
 	}
 	return false;

ArduCopterMega/control_modes.pde

@@ -1,3 +1,5 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
 void read_control_switch()
 {
 	byte switchPosition = readSwitch();
@@ -73,8 +75,9 @@ void read_trim_switch()
 		if(trim_flag){
 			// switch was just released
 			if((millis() - trim_timer) > 2000){
+#if HIL_MODE != HIL_MODE_ATTITUDE
 				imu.save();
-
+#endif
 			}else{
 				// set the throttle nominal
 				if(g.rc_3.control_in > 50){

ArduCopterMega/motors.pde

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
 #define ARM_DELAY 10
 #define DISARM_DELAY 10

ArduCopterMega/system.pde

@@ -115,8 +115,10 @@ void init_ardupilot()
 	init_rc_in();		// sets up rc channels from radio
 	init_rc_out();		// sets up the timer libs
 	init_camera();
+#if HIL_MODE != HIL_MODE_ATTITUDE
 	adc.Init();	 		// APM ADC library initialization
 	barometer.Init();	// APM Abs Pressure sensor initialization
+#endif
 	DataFlash.Init(); 	// DataFlash log initialization
 
 	// Do GPS init
@@ -137,9 +139,11 @@ void init_ardupilot()
 	if(g.compass_enabled)
 		init_compass();
 
+#if HIL_MODE != HIL_MODE_ATTITUDE
 	if(g.sonar_enabled){
-	  sonar.init(SONAR_PIN, &adc);
+		sonar.init(SONAR_PIN, &adc);
 	}
+#endif
 
 	pinMode(C_LED_PIN, OUTPUT);			// GPS status LED
 	pinMode(A_LED_PIN, OUTPUT);			// GPS status LED
@@ -213,10 +217,12 @@ void startup_ground(void)
 		gcs.send_message(MSG_COMMAND_LIST, i);
 	}
 
+#if HIL_MODE != HIL_MODE_ATTITUDE
 	// Warm up and read Gyro offsets
 	// -----------------------------
 	imu.init_gyro();
 	report_imu();
+#endif
 
 	// read the radio to set trims
 	// ---------------------------
@@ -226,9 +232,11 @@ void startup_ground(void)
 		Log_Write_Startup(TYPE_GROUNDSTART_MSG);
 
 
+#if HIL_MODE != HIL_MODE_ATTITUDE
 	// read Baro pressure at ground
 	//-----------------------------
 	init_barometer();
+#endif
 
 	// initialize commands
 	// -------------------
@@ -253,7 +261,7 @@ void set_mode(byte mode)
 		// disarm motors temp
 		motor_auto_safe = false;
 	}
-	//send_message(SEVERITY_LOW,"control mode");
+	//send_text(SEVERITY_LOW,"control mode");
 	//Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode);
 	switch(control_mode)
 	{

ArduCopterMega/test.pde

@@ -47,7 +47,9 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
 	{"stabilize",	test_stabilize},
 	{"fbw",			test_fbw},
 	{"gps",			test_gps},
+#if HIL_MODE != HIL_MODE_ATTITUDE
 	{"adc", 		test_adc},
+#endif
 	{"imu",			test_imu},
 	//{"dcm",			test_dcm},
 	//{"omega",		test_omega},
@@ -55,7 +57,9 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
 	{"current",		test_current},
 	{"relay",		test_relay},
 	{"waypoints",	test_wp},
+#if HIL_MODE != HIL_MODE_ATTITUDE
 	{"airpressure",	test_pressure},
+#endif
 	{"compass",		test_mag},
 	{"xbee",		test_xbee},
 	{"eedump",		test_eedump},
@@ -396,6 +400,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
 	}
 }
 
+#if HIL_MODE != HIL_MODE_ATTITUDE
 static int8_t
 test_adc(uint8_t argc, const Menu::arg *argv)
 {
@@ -416,6 +421,8 @@ test_adc(uint8_t argc, const Menu::arg *argv)
 		}
 	}
 }
+#endif
+
 static int8_t
 test_imu(uint8_t argc, const Menu::arg *argv)
 {
@@ -769,6 +776,7 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
 	}
 }
 
+#if HIL_MODE != HIL_MODE_ATTITUDE
 static int8_t
 test_pressure(uint8_t argc, const Menu::arg *argv)
 {
@@ -830,6 +838,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 		}
 	}
 }
+#endif
 
 static int8_t
 test_mag(uint8_t argc, const Menu::arg *argv)