ACM mavlink update/log fix

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

ArduCopterMega/ArduCopterMega.pde

@@ -52,7 +52,6 @@ version 2.1 of the License, or (at your option) any later version.
 // Local modules
 #include "defines.h"
 #include "Parameters.h"
-#include "global_data.h"
 #include "GCS.h"
 #include "HIL.h"
 

ArduCopterMega/GCS.h

@@ -65,12 +65,13 @@ public:
 	///
 	void		send_text(uint8_t severity, const char *str) {}
 
+#define send_text_P(severity, msg) send_text(severity, msg)
 	/// Send a text message with a PSTR()
 	///
 	/// @param	severity	A value describing the importance of the message.
 	/// @param	str			The text to be sent.
 	///
-	void		send_text_P(uint8_t severity, const prog_char_t *str) {}
+	void		send_text(uint8_t severity, const prog_char_t *str) {}
 
 	/// Send acknowledgement for a message.
 	///
@@ -140,7 +141,7 @@ public:
 	void	init(BetterStream *port);
 	void	send_message(uint8_t id, uint32_t param = 0);
 	void	send_text(uint8_t severity, const char *str);
-	void	send_text_P(uint8_t severity, const prog_char_t *str);
+	void	send_text(uint8_t severity, const prog_char_t *str);
 	void	acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
     void    data_stream_send(uint16_t freqMin, uint16_t freqMax);
 private:
@@ -152,6 +153,7 @@ private:
 
 	AP_Var      *_queued_parameter;                 ///< next parameter to be sent in queue
 	uint16_t    _queued_parameter_index;            ///< next queued parameter's index
+    uint16_t    _queued_parameter_count;            ///< saved count of parameters for queued send
 
 	/// Count the number of reportable parameters.
 	///
@@ -175,6 +177,30 @@ private:
     uint16_t rawControllerStreamRate;
     uint16_t rcStreamRate;
     uint16_t extraStreamRate[3];
+
+	// waypoints
+    uint16_t requested_interface; // request port to use
+	uint16_t waypoint_request_i; // request index
+	uint16_t waypoint_dest_sysid; // where to send requests
+	uint16_t waypoint_dest_compid; // "
+	bool waypoint_sending; // currently in send process
+	bool waypoint_receiving; // currently receiving
+	uint16_t waypoint_count;
+	uint32_t waypoint_timelast_send; // milliseconds
+	uint32_t waypoint_timelast_receive; // milliseconds
+	uint16_t waypoint_send_timeout; // milliseconds
+	uint16_t waypoint_receive_timeout; // milliseconds
+	float junk; //used to return a junk value for interface
+
+	// data stream rates
+	uint16_t streamRateRawSensors;
+	uint16_t streamRateExtendedStatus;
+	uint16_t streamRateRCChannels;
+	uint16_t streamRateRawController;
+	uint16_t streamRatePosition;
+	uint16_t streamRateExtra1;
+	uint16_t streamRateExtra2;
+	uint16_t streamRateExtra3;
 };
 #endif // GCS_PROTOCOL_MAVLINK
 

ArduCopterMega/GCS_Mavlink.pde

@@ -5,7 +5,21 @@
 #include "Mavlink_Common.h"
 
 GCS_MAVLINK::GCS_MAVLINK() :
-packet_drops(0)
+packet_drops(0),
+// parameters
+// note, all values not explicitly initialised here are zeroed
+waypoint_send_timeout(1000), // 1 second
+waypoint_receive_timeout(1000), // 1 second
+// stream rates
+streamRateRawSensors(0),
+streamRateExtendedStatus(0),
+streamRateRCChannels(0),
+streamRateRawController(0),
+//streamRateRawSensorFusion(0),
+streamRatePosition(0),
+streamRateExtra1(0),
+streamRateExtra2(0),
+streamRateExtra3(0)
 {
 }
 
@@ -20,6 +34,7 @@ GCS_MAVLINK::init(BetterStream * port)
         mavlink_comm_1_port = port;
         chan = MAVLINK_COMM_1;
     }
+	_queued_parameter = NULL;
 }
 
 void
@@ -45,57 +60,60 @@ GCS_MAVLINK::update(void)
     _queued_send();
 
     // stop waypoint sending if timeout
-    if (global_data.waypoint_sending && (millis() - global_data.waypoint_timelast_send) > global_data.waypoint_send_timeout){
+    if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){
         send_text_P(SEVERITY_LOW,PSTR("waypoint send timeout"));
-        global_data.waypoint_sending = false;
+        waypoint_sending = false;
     }
 
     // stop waypoint receiving if timeout
-    if (global_data.waypoint_receiving && (millis() - global_data.waypoint_timelast_receive) > global_data.waypoint_receive_timeout){
+    if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){
         send_text_P(SEVERITY_LOW,PSTR("waypoint receive timeout"));
-        global_data.waypoint_receiving = false;
+        waypoint_receiving = false;
     }
 }
 
 void
 GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
 {
-    if (freqLoopMatch(global_data.streamRateRawSensors,freqMin,freqMax))
+	if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
+		if (freqLoopMatch(streamRateRawSensors,freqMin,freqMax))
     	send_message(MSG_RAW_IMU);
 
-    if (freqLoopMatch(global_data.streamRateExtendedStatus,freqMin,freqMax)) {
+		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(global_data.streamRatePosition,freqMin,freqMax)) {
+		if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) {
 		send_message(MSG_LOCATION);
 	}
 
-    if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax)) {
+		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(global_data.streamRateRCChannels,freqMin,freqMax)) {
+		if (freqLoopMatch(streamRateRCChannels,freqMin,freqMax)) {
         send_message(MSG_RADIO_OUT);
         send_message(MSG_RADIO_IN);
     }
 
-    if (freqLoopMatch(global_data.streamRateExtra1,freqMin,freqMax)){   // Use Extra 1 for AHRS info
+		if (freqLoopMatch(streamRateExtra1,freqMin,freqMax)){   // Use Extra 1 for AHRS info
 	    send_message(MSG_ATTITUDE);
     }
 
-    if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)){    // Use Extra 2 for additional HIL info
+		if (freqLoopMatch(streamRateExtra2,freqMin,freqMax)){    // Use Extra 2 for additional HIL info
     	send_message(MSG_VFR_HUD);
     }
 
-    if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)){
+		if (freqLoopMatch(streamRateExtra3,freqMin,freqMax)){
         // Available datastream
 		}
     }
+}
 
 void
 GCS_MAVLINK::send_message(uint8_t id, uint32_t param)
@@ -110,12 +128,12 @@ GCS_MAVLINK::send_text(uint8_t severity, const char *str)
 }
 
 void
-GCS_MAVLINK::send_text_P(uint8_t severity, const prog_char_t *str)
+GCS_MAVLINK::send_text(uint8_t severity, const prog_char_t *str)
 {
 	mavlink_statustext_t m;
 	uint8_t i;
 	for (i=0; i<sizeof(m.text); i++) {
-		m.text[i] = pgm_read_byte((const prog_char *)str++);
+        m.text[i] = pgm_read_byte((const prog_char *)(str++));
 	}
 	if (i < sizeof(m.text)) m.text[i] = 0;
     mavlink_send_text(chan, severity, (const char *)m.text);
@@ -148,41 +166,41 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			switch(packet.req_stream_id){
 
 				case MAV_DATA_STREAM_ALL:
-					global_data.streamRateRawSensors 		= freq;
+                    streamRateRawSensors         = freq;
-					global_data.streamRateExtendedStatus 	= freq;
+                    streamRateExtendedStatus     = freq;
-					global_data.streamRateRCChannels 		= freq;
+                    streamRateRCChannels         = freq;
-					global_data.streamRateRawController 	= freq;
+                    streamRateRawController      = freq;
-					global_data.streamRatePosition 			= freq;
+                    streamRatePosition           = freq;
-					global_data.streamRateExtra1 			= freq;
+                    streamRateExtra1             = freq;
-					global_data.streamRateExtra2 			= freq;
+                    streamRateExtra2             = freq;
-					global_data.streamRateExtra3 			= freq;
+                    streamRateExtra3             = freq;
 					break;
 				case MAV_DATA_STREAM_RAW_SENSORS:
-					global_data.streamRateRawSensors = freq;
+                    streamRateRawSensors = freq;
 					break;
 				case MAV_DATA_STREAM_EXTENDED_STATUS:
-					global_data.streamRateExtendedStatus = freq;
+                    streamRateExtendedStatus = freq;
 					break;
 				case MAV_DATA_STREAM_RC_CHANNELS:
-					global_data.streamRateRCChannels = freq;
+                    streamRateRCChannels = freq;
 					break;
 				case MAV_DATA_STREAM_RAW_CONTROLLER:
-					global_data.streamRateRawController = freq;
+                    streamRateRawController = freq;
 					break;
 					//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
-					//    global_data.streamRateRawSensorFusion = freq;
+                    //    streamRateRawSensorFusion = freq;
 					//    break;
 				case MAV_DATA_STREAM_POSITION:
-					global_data.streamRatePosition = freq;
+                    streamRatePosition = freq;
 					break;
 				case MAV_DATA_STREAM_EXTRA1:
-					global_data.streamRateExtra1 = freq;
+                    streamRateExtra1 = freq;
 					break;
 				case MAV_DATA_STREAM_EXTRA2:
-					global_data.streamRateExtra2 = freq;
+                    streamRateExtra2 = freq;
 					break;
 				case MAV_DATA_STREAM_EXTRA3:
-					global_data.streamRateExtra3 = freq;
+                    streamRateExtra3 = freq;
 					break;
 				default:
 					break;
@@ -196,6 +214,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_action_t packet;
             mavlink_msg_action_decode(msg, &packet);
             if (mavlink_check_target(packet.target,packet.target_component)) break;
+            uint8_t result = 0;
 
             // do action
             send_text_P(SEVERITY_LOW,PSTR("action received"));
@@ -207,6 +226,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 				case MAV_ACTION_RETURN:
 					set_mode(RTL);
+                    result=1;
 					break;
 
 				case MAV_ACTION_EMCY_LAND:
@@ -215,6 +235,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 				case MAV_ACTION_HALT:
 					do_loiter_at_location();
+                    result=1;
 					break;
 
 					/* No mappable implementation in APM 2.0
@@ -228,26 +249,32 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 				case MAV_ACTION_CONTINUE:
 					process_next_command();
+                    result=1;
 					break;
 
 				case MAV_ACTION_SET_MANUAL:
                     set_mode(STABILIZE);
+                    result=1;
 					break;
 
 				case MAV_ACTION_SET_AUTO:
 					set_mode(AUTO);
+                    result=1;
 					break;
 
 				case MAV_ACTION_STORAGE_READ:
 					AP_Var::load_all();
+                    result=1;
 					break;
 
 				case MAV_ACTION_STORAGE_WRITE:
 					AP_Var::save_all();
+                    result=1;
 					break;
 
 				case MAV_ACTION_CALIBRATE_RC: break;
 					trim_radio();
+                    result=1;
 					break;
 
 				case MAV_ACTION_CALIBRATE_GYRO:
@@ -256,6 +283,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 				case MAV_ACTION_CALIBRATE_PRESSURE:
 				case MAV_ACTION_REBOOT:  // this is a rough interpretation
                     //startup_IMU_ground();
+                    //result=1;
 					break;
 
 				/*    For future implemtation
@@ -272,6 +300,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 				case MAV_ACTION_NAVIGATE:
 					set_mode(AUTO);
+                    result=1;
 					break;
 
 				/* Land is not an implemented flight mode in APM 2.0
@@ -282,10 +311,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 				case MAV_ACTION_LOITER:
 					set_mode(LOITER);
+                    result=1;
 					break;
 
 				default: break;
 				}
+                mavlink_msg_action_ack_send(
+                    chan,
+                    packet.action,
+                    result
+                    );
             break;
         }
 
@@ -306,12 +341,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 	            msg->compid,
 	            g.waypoint_total + 1); // + home
 
-            global_data.waypoint_timelast_send 	= millis();
+            waypoint_timelast_send   = millis();
-            global_data.waypoint_sending 		= true;
+            waypoint_sending         = true;
-            global_data.waypoint_receiving 		= false;
+            waypoint_receiving       = false;
-            global_data.waypoint_dest_sysid 	= msg->sysid;
+            waypoint_dest_sysid      = msg->sysid;
-            global_data.waypoint_dest_compid 	= msg->compid;
+            waypoint_dest_compid     = msg->compid;
-            global_data.requested_interface 	= chan;
+            requested_interface      = chan;
             break;
         }
 
@@ -320,7 +355,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             //send_text_P(SEVERITY_LOW,PSTR("waypoint request"));
 
             // Check if sending waypiont
-            if (!global_data.waypoint_sending) break;
+            if (!waypoint_sending) break;
 
             // decode
             mavlink_waypoint_request_t packet;
@@ -331,7 +366,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             tell_command = get_wp_with_index(packet.seq);
 
             // set frame of waypoint
-            uint8_t frame = MAV_FRAME_GLOBAL; // reference frame
+            uint8_t frame;
+            if (tell_command.options & WP_OPTION_ALT_RELATIVE) {
+                frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame
+            } else {
+                frame = MAV_FRAME_GLOBAL; // reference frame
+            }
             float param1 = 0, param2 = 0 , param3 = 0, param4 = 0;
 
             // time that the mav should loiter in milliseconds
@@ -344,7 +384,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 // command needs scaling
                 x = tell_command.lat/1.0e7; // local (x), global (latitude)
                 y = tell_command.lng/1.0e7; // local (y), global (longitude)
-                z = tell_command.alt/1.0e2; // local (z), global (altitude)
+                if (tell_command.options & WP_OPTION_ALT_RELATIVE) {
+                    z = (tell_command.alt - home.alt) / 1.0e2; // because tell_command.alt already includes a += home.alt
+                } else {
+                    z = tell_command.alt/1.0e2; // local (z), global/relative (altitude)
+                }
             }
 
 			switch (tell_command.id) {					// Switch to map APM command fields inot MAVLink command fields
@@ -391,7 +435,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             param1,param2,param3,param4,x,y,z);
 
             // update last waypoint comm stamp
-            global_data.waypoint_timelast_send = millis();
+            waypoint_timelast_send = millis();
             break;
         }
 
@@ -408,7 +452,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             uint8_t type = packet.type; // ok (0), error(1)
 
             // turn off waypoint send
-            global_data.waypoint_sending = false;
+            waypoint_sending = false;
             break;
         }
 
@@ -425,6 +469,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
             _queued_parameter = AP_Var::first();
             _queued_parameter_index = 0;
+            _queued_parameter_count = _count_parameters();
+			requested_interface = chan;
             break;
         }
 
@@ -483,17 +529,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 packet.count = MAX_WAYPOINTS;
             }
             g.waypoint_total.set_and_save(packet.count - 1);
-            global_data.waypoint_timelast_receive = millis();
+            waypoint_timelast_receive = millis();
-            global_data.waypoint_receiving 	= true;
+            waypoint_receiving   = true;
-            global_data.waypoint_sending 	= false;
+            waypoint_sending     = false;
-            global_data.waypoint_request_i 	= 0;
+            waypoint_request_i   = 0;
             break;
         }
 
     case MAVLINK_MSG_ID_WAYPOINT:
         {
             // Check if receiving waypiont
-            if (!global_data.waypoint_receiving) break;
+            if (!waypoint_receiving) break;
 
             // decode
             mavlink_waypoint_t packet;
@@ -501,7 +547,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
             // check if this is the requested waypoint
-            if (packet.seq != global_data.waypoint_request_i) break;
+            if (packet.seq != waypoint_request_i) break;
 
             // store waypoint
             uint8_t loadAction = 0; // 0 insert in list, 1 exec now
@@ -511,12 +557,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
             switch (packet.frame)
             {
-            case MAV_FRAME_GLOBAL:
             case MAV_FRAME_MISSION:
+            case MAV_FRAME_GLOBAL:
                 {
                     tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
                     tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
                     tell_command.alt = packet.z*1.0e2; // in as m converted to cm
+                    tell_command.options = 0;
                     break;
                 }
 
@@ -525,19 +572,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     tell_command.lat = 1.0e7*ToDeg(packet.x/
                     (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat;
                     tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
-                    tell_command.alt = -packet.z*1.0e2 + home.alt;
+                    tell_command.alt = packet.z*1.0e2;
+                    tell_command.options = 1;
                     break;
                 }
 
             case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
                 {
-                    if (!home_is_set) {
-                        send_text_P(SEVERITY_MEDIUM, PSTR("Refusing relative alt wp: home not set"));
-                        return;
-                    }
                     tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
                     tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
-                    tell_command.alt = packet.z*1.0e2 + home.alt;
+                    tell_command.alt = packet.z*1.0e2;
+                    tell_command.options = 1;
                     break;
                 }
             }
@@ -586,15 +631,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 
 
-            // save waypoint - and prevent re-setting home
-            if (packet.seq != 0)
             set_wp_with_index(tell_command, packet.seq);
 
             // update waypoint receiving state machine
-            global_data.waypoint_timelast_receive = millis();
+            waypoint_timelast_receive = millis();
-            global_data.waypoint_request_i++;
+            waypoint_request_i++;
 
-            if (global_data.waypoint_request_i > g.waypoint_total)
+            if (waypoint_request_i > g.waypoint_total)
             {
                 uint8_t type = 0; // ok (0), error(1)
 
@@ -605,7 +648,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 	type);
 
                 send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
-                global_data.waypoint_receiving = false;
+                waypoint_receiving = false;
                 // XXX ignores waypoint radius for individual waypoints, can
                 // only set WP_RADIUS parameter
             }
@@ -657,7 +700,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
 
                 } else if (var_type == AP_Var::k_typeid_int8) {
-                    ((AP_Int8 *)vp)->set(packet.param_value+rounding_addition);
+                    ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
                 } else {
                     // we don't support mavlink set on this parameter
                     break;
@@ -846,7 +889,7 @@ GCS_MAVLINK::_queued_send()
 {
 
     // Check to see if we are sending parameters
-    while (NULL != _queued_parameter) {
+    if (NULL != _queued_parameter && (requested_interface == chan) && mavdelay > 1) {
         AP_Var      *vp;
         float       value;
 
@@ -865,13 +908,12 @@ GCS_MAVLINK::_queued_send()
             	chan,
 	            (int8_t*)param_name,
     	        value,
-        	    _count_parameters(),
+                _queued_parameter_count,
             	_queued_parameter_index);
 
             _queued_parameter_index++;
-            break;
         }
-
+		mavdelay = 0;
 
 
 
@@ -882,15 +924,15 @@ GCS_MAVLINK::_queued_send()
 
     // request waypoints one by one
     // XXX note that this is pan-interface
-	if (global_data.waypoint_receiving &&
+    if (waypoint_receiving &&
-		(global_data.requested_interface == chan) &&
+        (requested_interface == chan) &&
-		global_data.waypoint_request_i <= g.waypoint_total &&
+        waypoint_request_i <= g.waypoint_total &&
 		mavdelay > 15) { // limits to 3.33 hz
 		mavlink_msg_waypoint_request_send(
 			chan,
-			global_data.waypoint_dest_sysid,
+            waypoint_dest_sysid,
-			global_data.waypoint_dest_compid,
+            waypoint_dest_compid,
-			global_data.waypoint_request_i);
+            waypoint_request_i);
 
 		mavdelay = 0;
 	}

ArduCopterMega/Log.pde

@@ -449,7 +449,6 @@ void Log_Write_GPS(	long log_Time, long log_Lattitude, long log_Longitude, long
 	DataFlash.WriteLong(log_Ground_Speed);
 	DataFlash.WriteLong(log_Ground_Course);
 	DataFlash.WriteByte(END_BYTE);
-	DataFlash.WriteByte(END_BYTE);
 }
 
 // Write an raw accel/gyro data packet. Total length : 28 bytes
@@ -532,7 +531,7 @@ void Log_Read_Performance()
 	long pm_time;
 	int logvar;
 
-	Serial.print("PM:");
+	Serial.printf_P(PSTR("PM:"));
 	pm_time = DataFlash.ReadLong();
 	Serial.print(pm_time);
 	Serial.print(comma);
@@ -554,7 +553,7 @@ void Log_Read_Cmd()
 	byte logvarb;
 	long logvarl;
 
-	Serial.print("CMD:");
+	Serial.printf_P(PSTR("CMD:"));
 	for(int i = 1; i < 4; i++) {
 		logvarb = DataFlash.ReadByte();
 		Serial.print(logvarb, DEC);
@@ -585,7 +584,7 @@ void Log_Read_Startup()
 // Read an attitude packet
 void Log_Read_Attitude()
 {
-	Serial.printf_P(PSTR("ATT: %d, %d, %d\n"),
+	Serial.printf_P(PSTR("ATT: %d, %d, %u\n"),
 			DataFlash.ReadInt(),
 			DataFlash.ReadInt(),
 			(uint16_t)DataFlash.ReadInt());
@@ -594,7 +593,7 @@ void Log_Read_Attitude()
 // Read a mode packet
 void Log_Read_Mode()
 {
-	Serial.print("MOD:");
+	Serial.printf_P(PSTR("MOD:"));
 	Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
 }
 
@@ -618,7 +617,7 @@ void Log_Read_GPS()
 void Log_Read_Raw()
 {
 	float logvar;
-	Serial.print("RAW:");
+	Serial.printf_P(PSTR("RAW:"));
 	for (int y = 0; y < 6; y++) {
 		logvar = (float)DataFlash.ReadLong() / t7;
 		Serial.print(logvar);
@@ -631,8 +630,8 @@ void Log_Read_Raw()
 void Log_Read(int start_page, int end_page)
 {
 	byte data;
-	byte log_step;
+	byte log_step = 0;
-	int packet_count;
+	int packet_count = 0;
 	int page = start_page;
 
 

ArduCopterMega/Mavlink_Common.h

@@ -5,7 +5,6 @@
 
 #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
 
-uint16_t system_type = MAV_FIXED_WING;
 byte mavdelay = 0;
 
 // what does this do?
@@ -31,7 +30,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 		case MSG_HEARTBEAT:
 			mavlink_msg_heartbeat_send(
 					chan,
-					system_type,
+					mavlink_system.type,
 					MAV_AUTOPILOT_ARDUPILOTMEGA);
 			break;
 
@@ -149,10 +148,10 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 			// This is used for HIL.  Do not change without discussing with HIL maintainers
 			mavlink_msg_rc_channels_scaled_send(
 					chan,
-					g.rc_1.norm_output(),
+					10000 * g.rc_1.norm_output(),
-					g.rc_2.norm_output(),
+					10000 * g.rc_2.norm_output(),
-					g.rc_3.norm_output(),
+					10000 * g.rc_3.norm_output(),
-					g.rc_4.norm_output(),
+					10000 * g.rc_4.norm_output(),
 					0,
 					0,
 					0,
@@ -199,10 +198,10 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 					chan,
 					(float)airspeed / 100.0,
 					(float)g_gps->ground_speed / 100.0,
-					dcm.yaw_sensor,
+					(dcm.yaw_sensor / 100) % 360,
+					nav_throttle,
 					current_loc.alt / 100.0,
-					climb_rate,
+					climb_rate);
-					nav_throttle);
 			break;
 		}
 
@@ -258,7 +257,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 			break;
 		}
 
-		defualt:
+		default:
 			break;
 	}
 }

ArduCopterMega/defines.h

@@ -271,3 +271,5 @@
 // parameters get the first 1KiB of EEPROM, remainder is for waypoints
 #define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
 #define WP_SIZE 15
+#define ONBOARD_PARAM_NAME_LENGTH 15
+#define MAX_WAYPOINTS  ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe

ArduCopterMega/global_data.h

@@ -1,52 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-#ifndef __GLOBAL_DATA_H
-#define __GLOBAL_DATA_H
-
-#define ONBOARD_PARAM_NAME_LENGTH 15
-#define MAX_WAYPOINTS  ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe
-
-#define FIRMWARE_VERSION 18 // <-- change on param data struct modifications
-
-#ifdef __cplusplus
-
-///
-// global data structure
-// This structure holds all the vehicle parameters.
-// TODO: bring in varibles floating around in ArduPilotMega.pde
-//
-struct global_struct
-{
-	// waypoints
-    uint16_t requested_interface; // request port to use
-	uint16_t waypoint_request_i; // request index
-	uint16_t waypoint_dest_sysid; // where to send requests
-	uint16_t waypoint_dest_compid; // "
-	bool waypoint_sending; // currently in send process
-	bool waypoint_receiving; // currently receiving
-	uint16_t waypoint_count;
-	uint32_t waypoint_timelast_send; // milliseconds
-	uint32_t waypoint_timelast_receive; // milliseconds
-	uint16_t waypoint_send_timeout; // milliseconds
-	uint16_t waypoint_receive_timeout; // milliseconds
-	float junk; //used to return a junk value for interface
-
-	// data stream rates
-	uint16_t streamRateRawSensors;
-	uint16_t streamRateExtendedStatus;
-	uint16_t streamRateRCChannels;
-	uint16_t streamRateRawController;
-	uint16_t streamRatePosition;
-	uint16_t streamRateExtra1;
-	uint16_t streamRateExtra2;
-	uint16_t streamRateExtra3;
-
-	// struct constructor
-	global_struct();
-} global_data;
-
-extern "C" const char *param_nametab[];
-
-#endif // __cplusplus
-
-#endif // __GLOBAL_DATA_H

ArduCopterMega/global_data.pde

@@ -1,20 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-global_struct::global_struct() :
-	// parameters
-	// note, all values not explicitly initialised here are zeroed
-	waypoint_send_timeout(1000), // 1 second
-	waypoint_receive_timeout(1000), // 1 second
-
-	// stream rates
-	streamRateRawSensors(0),
-	streamRateExtendedStatus(0),
-	streamRateRCChannels(0),
-	streamRateRawController(0),
-	//streamRateRawSensorFusion(0),
-	streamRatePosition(0),
-	streamRateExtra1(0),
-	streamRateExtra2(0),
-	streamRateExtra3(0)
-{
-}