2.0.8 - Mavlink fixes. Works much better now with M.Oborne's planner

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

ArduCopterMega/GCS_Mavlink.pde

@@ -365,6 +365,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+	// XXX read a WP from EEPROM and send it to the GCS
     case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
         {
             //send_text_P(SEVERITY_LOW,PSTR("waypoint request"));
@@ -375,6 +376,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // decode
             mavlink_waypoint_request_t packet;
             mavlink_msg_waypoint_request_decode(msg, &packet);
+
  			if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
             // send waypoint
@@ -382,6 +384,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
             // set frame of waypoint
             uint8_t frame;
+
             if (tell_command.options & WP_OPTION_ALT_RELATIVE) {
                 frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame
             } else {
@@ -392,8 +395,12 @@ 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) current = 1;
+
+            if (packet.seq == g.waypoint_index)
+            	current = 1;
+
             uint8_t autocontinue = 1; // 1 (true), 0 (false)
+
             float x = 0, y = 0, z = 0;
 
             if (tell_command.id < MAV_CMD_NAV_LAST) {
@@ -406,15 +413,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 
 			switch (tell_command.id) {					// Switch to map APM command fields inot MAVLink command fields
+
 				case MAV_CMD_NAV_LOITER_TURNS:
-			case MAV_CMD_NAV_TAKEOFF:
 				case MAV_CMD_CONDITION_CHANGE_ALT:
 				case MAV_CMD_DO_SET_HOME:
 					param1 = tell_command.p1;
 					break;
 
+				case MAV_CMD_NAV_TAKEOFF:
+					param1 = 0;
+					break;
+
 				case MAV_CMD_NAV_LOITER_TIME:
-				param1 = tell_command.p1*10;	// APM loiter time is in ten second increments
+					param1 = tell_command.p1;	// APM loiter time is in ten second increments
 					break;
 
 				case MAV_CMD_CONDITION_DELAY:
@@ -436,6 +447,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					param1 = tell_command.p1;
 					break;
 
+				case MAV_CMD_NAV_WAYPOINT:
+					param1 = tell_command.p1;
+					break;
+
 				case MAV_CMD_DO_SET_PARAMETER:
 				case MAV_CMD_DO_SET_RELAY:
 				case MAV_CMD_DO_SET_SERVO:
@@ -445,8 +460,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			}
 
             mavlink_msg_waypoint_send(chan,msg->sysid,
-            msg->compid,packet.seq,frame,tell_command.id,current,autocontinue,
+							            msg->compid,
-            param1,param2,param3,param4,x,y,z);
+							            packet.seq,
+							            frame,
+							            tell_command.id,
+							            current,
+							            autocontinue,
+							            param1,
+							            param2,
+							            param3,
+							            param4,
+							            x,
+							            y,
+							            z);
 
             // update last waypoint comm stamp
             waypoint_timelast_send = millis();
@@ -546,6 +572,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+	// XXX receive a WP from GCS and store in EEPROM
     case MAVLINK_MSG_ID_WAYPOINT:
         {
             // Check if receiving waypiont
@@ -565,8 +592,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // defaults
             tell_command.id = packet.command;
 
-            switch (packet.frame)
+            /*
-            {
+            switch (packet.frame){
+
 				case MAV_FRAME_MISSION:
 				case MAV_FRAME_GLOBAL:
 					{
@@ -586,29 +614,40 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 						tell_command.options = 1;
 						break;
 					}
-            case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
+				//case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
+				default:
 					{
-                    tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
+						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.lng = 1.0e7 * packet.y; // in as DD converted to * t7
-                    tell_command.alt = packet.z*1.0e2;
+						tell_command.alt = packet.z * 1.0e2;
-                    tell_command.options = 1;
+						tell_command.options = 1; // store altitude relative!! Always!!
 						break;
 					}
 			}
+			*/
+
+			// we only are supporting Abs position, relative Alt
+			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;
+			tell_command.options = 1; // store altitude relative!! Always!!
 
 			switch (tell_command.id) {					// Switch to map APM command fields inot MAVLink command fields
 				case MAV_CMD_NAV_LOITER_TURNS:
-			case MAV_CMD_NAV_TAKEOFF:
 				case MAV_CMD_DO_SET_HOME:
 					tell_command.p1 = packet.param1;
 					break;
 
+				case MAV_CMD_NAV_TAKEOFF:
+					tell_command.p1 = 0;
+					break;
+
 				case MAV_CMD_CONDITION_CHANGE_ALT:
 					tell_command.p1 = packet.param1 * 100;
 					break;
 
 				case MAV_CMD_NAV_LOITER_TIME:
-				tell_command.p1 = packet.param1 / 10;	// APM loiter time is in ten second increments
+					tell_command.p1 = packet.param1;	// APM loiter time is in ten second increments
 					break;
 
 				case MAV_CMD_CONDITION_DELAY:
@@ -630,6 +669,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					tell_command.p1 = packet.param1;
 					break;
 
+				case MAV_CMD_NAV_WAYPOINT:
+					tell_command.p1 = packet.param1;
+					break;
+
 				case MAV_CMD_DO_SET_PARAMETER:
 				case MAV_CMD_DO_SET_RELAY:
 				case MAV_CMD_DO_SET_SERVO:
@@ -638,16 +681,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					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 > g.waypoint_total){
-            {
                 uint8_t type = 0; // ok (0), error(1)
 
                 mavlink_msg_waypoint_ack_send(

ArduCopterMega/Log.pde

@@ -78,10 +78,11 @@ print_log_menu(void)
 		Serial.printf_P(PSTR("\nNo logs\n"));
 	}else{
 		Serial.printf_P(PSTR("\n%d logs\n"), last_log_num);
+
 		for(int i = 1; i < last_log_num + 1; i++) {
 			get_log_boundaries(last_log_num, i, log_start, log_end);
-			Serial.printf_P(PSTR("Log # %d,    start %d,   end %d\n"),
+			//Serial.printf_P(PSTR("last_num %d "), last_log_num);
-							i, log_start, log_end);
+			Serial.printf_P(PSTR("Log # %d,    start %d,   end %d\n"), i, log_start, log_end);
 		}
 		Serial.println();
 	}
@@ -98,9 +99,11 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 
 	// check that the requested log number can be read
 	dump_log = argv[1].i;
+
 	last_log_num = get_num_logs();
-	if ((argc != 2) || (dump_log < 1) || (dump_log > last_log_num)) {
+
-		Serial.printf_P(PSTR("bad log number\n"));
+	if (/*(argc != 2) || */(dump_log < 1) || (dump_log > last_log_num)) {
+		Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
 		return(-1);
 	}
 
@@ -186,11 +189,12 @@ process_logs(uint8_t argc, const Menu::arg *argv)
 }
 
 
+// finds out how many logs are available
 byte get_num_logs(void)
 {
 	int page = 1;
 	byte data;
-	byte log_step = 0;
+	byte log_step;
 
 	DataFlash.StartRead(1);
 
@@ -211,11 +215,14 @@ byte get_num_logs(void)
 				break;
 
 			case 2:
-				if(data==LOG_INDEX_MSG){
+				if(data == LOG_INDEX_MSG){
 					byte num_logs = DataFlash.ReadByte();
+					//Serial.printf("num_logs, %d\n", num_logs);
+
 					return num_logs;
 				}else{
-						log_step=0;	 // Restart, we have a problem...
+					//Serial.printf("* %d\n", data);
+					log_step = 0;	 // Restart, we have a problem...
 				}
 				break;
 			}
@@ -225,10 +232,12 @@ byte get_num_logs(void)
 }
 
 // send the number of the last log?
-void start_new_log(byte num_existing_logs)
+void start_new_log()
 {
-	int start_pages[50] = {0, 0, 0};
+	byte num_existing_logs = get_num_logs();
-	int end_pages[50]	= {0, 0, 0};
+
+	int start_pages[50];
+	int end_pages[50];
 
 	if(num_existing_logs > 0){
 		for(int i = 0; i < num_existing_logs; i++) {
@@ -237,7 +246,8 @@ void start_new_log(byte num_existing_logs)
 		end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
 	}
 
-	if(end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
+	if((end_pages[num_existing_logs - 1] < 4095) && (num_existing_logs < MAX_NUM_LOGS /*50*/)) {
+
 		if(num_existing_logs > 0)
 			start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1;
 		else
@@ -265,38 +275,46 @@ void start_new_log(byte num_existing_logs)
 	}
 }
 
-void get_log_boundaries(byte num_logs, byte log_num, int & start_page, int & end_page)
+// All log data is stored in page 1?
+void get_log_boundaries(byte num_logs, byte log_num, int &start_page, int &end_page)
 {
 	int page = 1;
 	byte data;
-	byte log_step = 0;
+	byte log_step;
+
+	// start reading at page 1,
+	// XXX not 0?
+	DataFlash.StartRead(page);
 
-	DataFlash.StartRead(1);
 	while (page == 1) {
 		data = DataFlash.ReadByte();
-		switch(log_step)		 //This is a state machine to read the packets
+		switch(log_step){		 //This is a state machine to read the packets
-			{
 			case 0:
 				if(data==HEAD_BYTE1)	// Head byte 1
 					log_step++;
 				break;
+
 			case 1:
 				if(data==HEAD_BYTE2)	// Head byte 2
 					log_step++;
 				else
 					log_step = 0;
 				break;
+
 			case 2:
-				if(data==LOG_INDEX_MSG){
+				if(data == LOG_INDEX_MSG){
 
 					byte num_logs = DataFlash.ReadByte();
 
-					for(int i=0;i<log_num;i++) {
+					for(int i = 0; i < log_num; i++) {
 						start_page 	= DataFlash.ReadInt();
 						end_page 	= DataFlash.ReadInt();
+						//Serial.printf("log %d, start page:%d, end page:%d\n",i, start_page, end_page);
 					}
 
-					if(log_num==num_logs)
+					// what is this?
+					//
+					if(log_num == num_logs)
 						end_page = find_last_log_page(start_page);
 
 					return;		// This is the normal exit point
@@ -307,9 +325,14 @@ void get_log_boundaries(byte num_logs, byte log_num, int & start_page, int & end
 			}
 		page = DataFlash.GetPage();
 	}
+
+	if(page > 1){
+		Serial.printf("page er:%d\n", page);
+	}
 	//  Error condition if we reach here with page = 2   TO DO - report condition
 }
 
+//
 int find_last_log_page(int bottom_page)
 {
 	int top_page = 4096;
@@ -320,6 +343,9 @@ int find_last_log_page(int bottom_page)
 		look_page = (top_page + bottom_page) / 2;
 		DataFlash.StartRead(look_page);
 		check = DataFlash.ReadLong();
+
+		//Serial.printf("look page:%d, check:%d\n", look_page, check);
+
 		if(check == 0xFFFFFFFF)
 			top_page = look_page;
 		else
@@ -614,15 +640,15 @@ void Log_Write_Cmd(byte num, struct Location *wp)
 
 	DataFlash.WriteByte(num);
 	DataFlash.WriteByte(wp->id);
-	DataFlash.WriteByte(wp->p1);
 	DataFlash.WriteByte(wp->options);
-
+	DataFlash.WriteByte(wp->p1);
 	DataFlash.WriteLong(wp->alt);
 	DataFlash.WriteLong(wp->lat);
 	DataFlash.WriteLong(wp->lng);
 
 	DataFlash.WriteByte(END_BYTE);
 }
+//CMD, 3, 0, 16, 8, 1, 800, 340440192, -1180692736
 
 
 // Read a command processing packet

ArduCopterMega/sensors.pde

@@ -56,7 +56,7 @@ void init_barometer(void)
 	ground_pressure = abs_pressure;
 
 	//Serial.printf("abs_pressure %ld\n", abs_pressure);
-	SendDebugln("barometer calibration complete.");
+	//SendDebugln("barometer calibration complete.");
 }
 
 long read_barometer(void)

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.7 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.8 Beta", main_menu_commands);
 
 void init_ardupilot()
 {
@@ -239,8 +239,7 @@ void init_ardupilot()
 	if(g.log_bitmask != 0){
 		//	TODO - Here we will check  on the length of the last log
 		//  We don't want to create a bunch of little logs due to powering on and off
-		byte last_log_num = get_num_logs();
+		start_new_log();
-		start_new_log(last_log_num);
 	}
 
 	if (g.log_bitmask & MASK_LOG_MODE)
@@ -354,7 +353,7 @@ void set_mode(byte mode)
 			break;
 
 		case RTL:
-			init_throttle_cruise();
+			//init_throttle_cruise();
 			do_RTL();
 			break;