Mavlink Sync with APM

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

ArduCopterMega/ArduCopterMega.pde

@@ -43,9 +43,6 @@ version 2.1 of the License, or (at your option) any later version.
 #define MAVLINK_COMM_NUM_BUFFERS 2
 #include <GCS_MAVLink.h>    // MAVLink GCS definitions
 
-
-
-
 // Configuration
 #include "config.h"
 
@@ -75,10 +72,12 @@ FastSerialPort3(Serial3);       // Telemetry port
 //
 Parameters      g;
 
+
 ////////////////////////////////////////////////////////////////////////////////
 // prototypes
 void update_events(void);
 
+
 ////////////////////////////////////////////////////////////////////////////////
 // Sensors
 ////////////////////////////////////////////////////////////////////////////////
@@ -175,6 +174,7 @@ GPS         *g_gps;
 ////////////////////////////////////////////////////////////////////////////////
 //
 #if   GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
+	//GCS_MAVLINK	gcs(Parameters::k_param_streamrates_port3);
 	GCS_MAVLINK	gcs(Parameters::k_param_streamrates_port3);
 #else
 	// If we are not using a GCS, we need a stub that does nothing.
@@ -300,7 +300,6 @@ float 	battery_voltage2 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 +
 float 	battery_voltage3 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
 float 	battery_voltage4 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
 
-float 	current_voltage 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
 float	current_amps;
 float	current_total;
 
@@ -411,10 +410,6 @@ float G_Dt						= 0.02;		// Integration time for the gyros (DCM algorithm)
 long 	perf_mon_timer;
 float 	imu_health; 						// Metric based on accel gain deweighting
 int 	G_Dt_max;							// Max main loop cycle time in milliseconds
-byte 	gyro_sat_count;
-byte 	adc_constraints;
-byte 	renorm_sqrt_count;
-byte 	renorm_blowup_count;
 int 	gps_fix_count;
 byte	gcs_messages_sent;
 
@@ -1284,4 +1279,5 @@ void tuning(){
 		g.pitch_max.set(g.rc_6.control_in * 2);  // 0 to 2000
 
 	#endif
-}
+}
+

ArduCopterMega/GCS.h

@@ -66,6 +66,7 @@ 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.
@@ -196,6 +197,9 @@ private:
 	AP_Int16 streamRateExtra1;
 	AP_Int16 streamRateExtra2;
 	AP_Int16 streamRateExtra3;
+
+
+
 };
 #endif // GCS_PROTOCOL_MAVLINK
 

ArduCopterMega/GCS_Mavlink.pde

@@ -20,7 +20,9 @@ streamRatePosition			(&_group, 4, 0, PSTR("POSITION")),
 streamRateExtra1			(&_group, 5, 0, PSTR("EXTRA1")),
 streamRateExtra2			(&_group, 6, 0, PSTR("EXTRA2")),
 streamRateExtra3			(&_group, 7, 0, PSTR("EXTRA3"))
+
 {
+
 }
 
 void
@@ -49,6 +51,8 @@ GCS_MAVLINK::update(void)
     {
         uint8_t c = comm_receive_ch(chan);
 
+
+
         // Try to get a new message
         if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
     }
@@ -56,6 +60,11 @@ GCS_MAVLINK::update(void)
     // Update packet drops counter
     packet_drops += status.packet_rx_drop_count;
 
+
+
+
+
+
     // send out queued params/ waypoints
     _queued_send();
 
@@ -84,7 +93,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
         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);
+			send_message(MSG_NAV_CONTROLLER_OUTPUT);
     }
 
 		if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) {
@@ -215,10 +224,12 @@ 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"));
+            send_text_P(SEVERITY_LOW,PSTR("action received: "));
+//Serial.println(packet.action);
 			switch(packet.action){
 
 				case MAV_ACTION_LAUNCH:
@@ -317,11 +328,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 				default: break;
 				}
+
                 mavlink_msg_action_ack_send(
                     chan,
                     packet.action,
                     result
                     );
+
             break;
         }
 
@@ -332,7 +345,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // decode
             mavlink_waypoint_request_list_t packet;
             mavlink_msg_waypoint_request_list_decode(msg, &packet);
-
             if (mavlink_check_target(packet.target_system,packet.target_component))
             	break;
 
@@ -577,7 +589,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     tell_command.options = 1;
                     break;
                 }
-
             case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
                 {
                     tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
@@ -888,7 +899,6 @@ GCS_MAVLINK::_find_parameter(uint16_t index)
 void
 GCS_MAVLINK::_queued_send()
 {
-
     // Check to see if we are sending parameters
     if (NULL != _queued_parameter && (requested_interface == chan) && mavdelay > 1) {
         AP_Var      *vp;
@@ -915,9 +925,6 @@ GCS_MAVLINK::_queued_send()
             _queued_parameter_index++;
         }
 		mavdelay = 0;
-
-
-
     }
 
     // this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
@@ -941,4 +948,3 @@ GCS_MAVLINK::_queued_send()
 
 #endif
 
-

ArduCopterMega/Log.pde

@@ -340,6 +340,8 @@ void Log_Write_Cmd(byte num, struct Location *wp)
 
 void Log_Write_Nav_Tuning()
 {
+	Matrix3f tempmat = dcm.get_dcm_matrix();
+
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
@@ -359,35 +361,28 @@ void Log_Write_Nav_Tuning()
 	DataFlash.WriteInt((int)next_WP.alt);				// 11
 	DataFlash.WriteInt((int)altitude_error);			// 12
 
+	DataFlash.WriteInt((int)(wrap_360(ToDeg(compass.heading)*100)/100)); // Just a temp hack
+	DataFlash.WriteLong(compass.last_update); // Just a temp hack
+	DataFlash.WriteInt((int)(tempmat.b.x*1000)); // Just a temp hack
+	DataFlash.WriteInt((int)(compass.heading_x*1000)); // Just a temp hack
+	DataFlash.WriteInt((int)(tempmat.a.x*1000)); // Just a temp hack
+	DataFlash.WriteInt((int)(compass.heading_y*1000)); // Just a temp hack
+
 	DataFlash.WriteByte(END_BYTE);
 }
-
-/*
-Matrix3f tempmat = dcm.get_dcm_matrix();
-
-DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
-DataFlash.WriteInt((int)wp_distance);
-DataFlash.WriteInt((uint16_t)target_bearing);
-DataFlash.WriteInt((uint16_t)nav_bearing);
-DataFlash.WriteInt(altitude_error);
-DataFlash.WriteInt((int)airspeed);
-DataFlash.WriteInt((int)(nav_gain_scaler*1000));
-DataFlash.WriteInt((int)(wrap_360(ToDeg(compass.heading)*100)/100)); // Just a temp hack
-DataFlash.WriteLong(compass.last_update); // Just a temp hack
-DataFlash.WriteInt((int)(tempmat.b.x*1000)); // Just a temp hack
-DataFlash.WriteInt((int)(compass.heading_x*1000)); // Just a temp hack
-DataFlash.WriteInt((int)(tempmat.a.x*1000)); // Just a temp hack
-DataFlash.WriteInt((int)(compass.heading_y*1000)); // Just a temp hack
-*/
-
-
     //	1	2	3	4  5  6	 7	 8	   9	  10   11
 //NTUN, 236, 0, 132, 10, 0, 0, 29, 2963, 16545, 16682, 108
 
 void Log_Read_Nav_Tuning()
 {
 							 //	1	2	3	4	5	6	7	8	9	10  11
-	Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
+	Serial.printf_P(PSTR( "NTUN, %d, %d, %d, "
+								"%d, %d, "
+								"%d, %d, %d, %d, "
+								"%d, %d, %d "
+								"%d, %ld, %4.4f, %4.4f, %4.4f, %4.4f "
+
+								),
 				DataFlash.ReadInt(),	//yaw sensor
 				DataFlash.ReadInt(),	//distance
 				DataFlash.ReadByte(),	//bitmask
@@ -402,11 +397,19 @@ void Log_Read_Nav_Tuning()
 
 				DataFlash.ReadInt(),	//home.alt
 				DataFlash.ReadInt(),	//Next_alt
-				DataFlash.ReadInt());	//Alt Error
+				DataFlash.ReadInt(),	//Alt Error
+
+				DataFlash.ReadInt(),
+				DataFlash.ReadLong(),
+				(float)DataFlash.ReadInt()/1000,
+				(float)DataFlash.ReadInt()/1000,
+				(float)DataFlash.ReadInt()/1000,
+				(float)DataFlash.ReadInt()/1000);
 }
 
 
 
+
 // Write a mode packet. Total length : 5 bytes
 void Log_Write_Mode(byte mode)
 {
@@ -492,7 +495,7 @@ void Log_Write_Current()
 	DataFlash.WriteInt(g.rc_3.control_in);
 	DataFlash.WriteLong(throttle_integrator);
 
-	DataFlash.WriteInt((int)(current_voltage 	* 100.0));
+	DataFlash.WriteInt((int)(battery_voltage 	* 100.0));
 	DataFlash.WriteInt((int)(current_amps 		* 100.0));
 	DataFlash.WriteInt((int)current_total);
 
@@ -772,4 +775,5 @@ void Log_Read(int start_page, int end_page)
 	//Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count);
 }
 
-
+
+

ArduCopterMega/Mavlink_Common.h

@@ -7,9 +7,11 @@
 
 byte mavdelay = 0;
 
+
 // what does this do?
 static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
 {
+//Serial.print("target = "); Serial.print(sysid, DEC); Serial.print("\tcomp = "); Serial.println(compid, DEC);
     if (sysid != mavlink_system.sysid){
         return 1;
 
@@ -22,6 +24,7 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
     }
 }
 
+
 void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
 {
     uint64_t timeStamp = micros();
@@ -67,6 +70,7 @@ 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 = 10.0 * (float)(g.pack_capacity - current_total)/g.pack_capacity;	//Mavlink scaling 100% = 1000
 			uint8_t motor_block = false;
 
 			mavlink_msg_sys_status_send(
@@ -75,7 +79,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 					nav_mode,
 					status,
 					load * 1000,
-					battery_voltage1 * 1000,
+					battery_voltage * 1000,
 					motor_block,
 					packet_drops);
 			break;
@@ -110,6 +114,23 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 			break;
 		}
 
+		case MSG_NAV_CONTROLLER_OUTPUT:
+        {
+            //if (control_mode != MANUAL) {
+                mavlink_msg_nav_controller_output_send(
+					chan,
+                    nav_roll / 1.0e2,
+                    nav_pitch / 1.0e2,
+                    nav_bearing / 1.0e2,
+                    target_bearing / 1.0e2,
+                    wp_distance,
+                    altitude_error / 1.0e2,
+                    0,
+                    crosstrack_error);
+            //}
+			break;
+        }
+
 		case MSG_LOCAL_LOCATION:
 		{
 			Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
@@ -277,4 +298,3 @@ void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8
 #endif // mavlink in use
 
 #endif // inclusion guard
-

ArduCopterMega/defines.h

@@ -167,6 +167,7 @@
 #define MSG_VERSION 0x09
 #define MSG_EXTENDED_STATUS 0x0a
 #define MSG_CPU_LOAD 0x0b
+#define MSG_NAV_CONTROLLER_OUTPUT 0x0c
 
 #define MSG_COMMAND_REQUEST 0x20
 #define MSG_COMMAND_UPLOAD 0x21
@@ -297,5 +298,6 @@
 // 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/system.pde

@@ -155,7 +155,7 @@ void init_ardupilot()
 	#else
 		gcs.init(&Serial);
 	#endif
-	
+
 	// init the HIL
 #if HIL_MODE != HIL_MODE_DISABLED
 
@@ -469,10 +469,10 @@ void update_esc_light()
 void resetPerfData(void) {
 	mainLoop_count 		= 0;
 	G_Dt_max 			= 0;
-	gyro_sat_count 		= 0;
+	//gyro_sat_count 		= 0;
-	adc_constraints 	= 0;
+	//adc_constraints 	= 0;
-	renorm_sqrt_count 	= 0;
+	//renorm_sqrt_count 	= 0;
-	renorm_blowup_count = 0;
+	//renorm_blowup_count = 0;
 	gps_fix_count 		= 0;
 	perf_mon_timer 		= millis();
 }
@@ -525,4 +525,5 @@ init_throttle_cruise()
 			//Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get());
 		}
 	}
-}
+}
+

ArduCopterMega/test.pde

@@ -654,7 +654,7 @@ test_current(uint8_t argc, const Menu::arg *argv)
 		read_radio();
 		read_battery();
 		Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
-						current_voltage,
+						battery_voltage,
 						current_amps,
 						current_total);