fix for Logging, motors_out buffer overrun, formatting.

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

ArduCopterMega/ArduCopterMega.pde

@@ -205,7 +205,7 @@ const char* flight_mode_strings[] = {
 
 // Radio
 // -----
-int motor_out[4];
+int motor_out[8];
 Vector3f omega;
 
 // Failsafe

ArduCopterMega/GCS_Mavlink.pde

@@ -52,19 +52,13 @@ 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 (global_data.waypoint_sending && (millis() - global_data.waypoint_timelast_send) > global_data.waypoint_send_timeout){
         send_text(SEVERITY_LOW,"waypoint send timeout");
         global_data.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 (global_data.waypoint_receiving && (millis() - global_data.waypoint_timelast_receive) > global_data.waypoint_receive_timeout){
         send_text(SEVERITY_LOW,"waypoint receive timeout");
         global_data.waypoint_receiving = false;
     }
@@ -102,8 +96,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
     if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax))    // Use Extra 3 for additional HIL info
     	send_message(MSG_VFR_HUD);
 
-    if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax))
-    {
+    if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)){
         // Available datastream
     }
 }
@@ -197,8 +190,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
             // do action
             send_text(SEVERITY_LOW,"action received");
-            switch(packet.action)
-            {
+			switch(packet.action){
 
 				case MAV_ACTION_LAUNCH:
 					//set_mode(TAKEOFF);
@@ -290,11 +282,16 @@ 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;
+
+            if (mavlink_check_target(packet.target_system,packet.target_component))
+            	break;
 
             // Start sending waypoints
-            mavlink_msg_waypoint_count_send(chan,msg->sysid,
-            msg->compid,g.waypoint_total + 1); // + home
+            mavlink_msg_waypoint_count_send(
+            	chan,msg->sysid,
+	            msg->compid,
+	            g.waypoint_total + 1); // + home
+
             global_data.waypoint_timelast_send 	= millis();
             global_data.waypoint_sending 		= true;
             global_data.waypoint_receiving 		= false;
@@ -520,7 +517,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             if (global_data.waypoint_request_i > g.waypoint_total)
             {
                 uint8_t type = 0; // ok (0), error(1)
-                mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
+
+                mavlink_msg_waypoint_ack_send(
+                	chan,
+                	msg->sysid,
+                	msg->compid,
+                	type);
+
                 send_text(SEVERITY_LOW,"flight plan received");
                 global_data.waypoint_receiving = false;
                 // XXX ignores waypoint radius for individual waypoints, can
@@ -574,7 +577,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             }
 
             // Report back new value
-            mavlink_msg_param_value_send(chan, (int8_t *)key, packet.param_value,
+            mavlink_msg_param_value_send(
+            	chan,
+            	(int8_t *)key,
+            	packet.param_value,
             	_count_parameters(),
             	-1); // XXX we don't actually know what its index is...
 
@@ -748,14 +754,10 @@ GCS_MAVLINK::_queued_send()
         AP_Var      *vp;
         float       value;
 
-
-
         // copy the current parameter and prepare to move to the next
         vp = _queued_parameter;
         _queued_parameter = _queued_parameter->next();
 
-
-
         // if the parameter can be cast to float, report it here and break out of the loop
         value = vp->cast_to_float();
         if (!isnan(value)) {
@@ -763,7 +765,8 @@ GCS_MAVLINK::_queued_send()
             char param_name[ONBOARD_PARAM_NAME_LENGTH];         /// XXX HACK
             vp->copy_name(param_name, sizeof(param_name));
 
-            mavlink_msg_param_value_send(chan,
+            mavlink_msg_param_value_send(
+            	chan,
 	            (int8_t*)param_name,
     	        value,
         	    _count_parameters(),
@@ -783,12 +786,16 @@ GCS_MAVLINK::_queued_send()
 
     // request waypoints one by one
     // XXX note that this is pan-interface
-    if (global_data.waypoint_receiving && global_data.requested_interface == chan &&
-            global_data.waypoint_request_i <= g.waypoint_total && mavdelay > 15) // limits to 3.33 hz
+    if (global_data.waypoint_receiving &&
+    		(global_data.requested_interface == chan) &&
+            global_data.waypoint_request_i <= (g.waypoint_total && mavdelay > 15)) // limits to 3.33 hz
     {
-        mavlink_msg_waypoint_request_send(chan,
+        mavlink_msg_waypoint_request_send(
+        	chan,
         	global_data.waypoint_dest_sysid,
-        global_data.waypoint_dest_compid ,global_data.waypoint_request_i);
+        	global_data.waypoint_dest_compid,
+        	global_data.waypoint_request_i);
+
         mavdelay = 0;
     }
 }

ArduCopterMega/Parameters.h

@@ -244,7 +244,7 @@ public:
 
         pitch_max         		(PITCH_MAX * 100,			k_param_pitch_max,		       			PSTR("PITCH_MAX")),
 
-        log_bitmask             (0,                         k_param_log_bitmask,           			PSTR("LOG_BITMASK")),
+        log_bitmask             (MASK_LOG_SET_DEFAULTS,		k_param_log_bitmask,            		PSTR("LOG_BITMASK")),
         ground_temperature      (0,                         k_param_ground_temperature,    			PSTR("GND_TEMP")),
         ground_pressure         (0,                         k_param_ground_pressure,       			PSTR("GND_ABS_PRESS")),
         RTL_altitude            (ALT_HOLD_HOME * 100,		k_param_RTL_altitude,          			PSTR("ALT_HOLD_RTL")),

ArduCopterMega/defines.h

@@ -204,6 +204,7 @@
 #define MASK_LOG_RAW			(1<<7)
 #define MASK_LOG_CMD			(1<<8)
 #define MASK_LOG_CUR			(1<<9)
+#define MASK_LOG_SET_DEFAULTS	(1<<15)
 
 // Waypoint Modes
 // ----------------

ArduCopterMega/motors.pde

@@ -279,20 +279,21 @@ set_servos_4()
 			motor_auto_safe = true;
 		}
 
-		// Send commands to motors
-		APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
-		APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
-		APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
-		APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
-
 		// fill the motor_out[] array for HIL use
-		for (unsigned char i=0; i<8; i++) {
-			motor_out[i] = 0;
+		for (unsigned char i = 0; i < 8; i++) {
+			motor_out[i] = g.rc_3.radio_min;
 		}
 
+		// Send commands to motors
+		APM_RC.OutputCh(CH_1, motor_out[CH_1]);
+		APM_RC.OutputCh(CH_2, motor_out[CH_2]);
+		APM_RC.OutputCh(CH_3, motor_out[CH_3]);
+		APM_RC.OutputCh(CH_4, motor_out[CH_4]);
+
+
 		if (g.frame_type == HEXA_FRAME) {
-			APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
-			APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
+			APM_RC.OutputCh(CH_7, motor_out[CH_7]);
+			APM_RC.OutputCh(CH_8, motor_out[CH_8]);
 		}
 
 		// reset I terms of PID controls

ArduCopterMega/setup.pde

@@ -99,6 +99,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
 	AP_Var::erase_all();
 	Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
 
+	delay(1000);
 	default_log_bitmask();
 	default_gains();
 

ArduCopterMega/system.pde

@@ -141,7 +141,8 @@ void init_ardupilot()
 	#endif
 
 
-
+	if (g.log_bitmask & MASK_LOG_MODE)
+		Log_Write_Mode(control_mode);
 
 	if(g.compass_enabled)
 		init_compass();
@@ -199,6 +200,10 @@ void init_ardupilot()
 	if (g.log_bitmask & MASK_LOG_CMD)
 		Log_Write_Startup(TYPE_GROUNDSTART_MSG);
 
+	if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) {
+		default_log_bitmask();
+	}
+
 	// set the correct flight mode
 	// ---------------------------
 	reset_control_switch();
@@ -212,7 +217,7 @@ void startup_ground(void)
 	gcs.send_text(SEVERITY_LOW,"<startup_ground> GROUND START");
 
 	#if(GROUND_START_DELAY > 0)
-		gcs.send_text(SEVERITY_LOW,"<startup_ground> With Delay");
+		//gcs.send_text(SEVERITY_LOW," With Delay");
 		delay(GROUND_START_DELAY * 1000);
 	#endif
 
@@ -302,9 +307,6 @@ void set_mode(byte mode)
 	// output control mode to the ground station
 	gcs.send_message(MSG_MODE_CHANGE);
 
-	if (g.log_bitmask & MASK_LOG_MODE)
-		Log_Write_Mode(control_mode);
-
 }
 
 void set_failsafe(boolean mode)

ArduCopterMega/test.pde

@@ -835,8 +835,9 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 			update_alt();
 			output_auto_throttle();
 
-			Serial.printf_P(PSTR("Alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"),
+			Serial.printf_P(PSTR("B_alt: %ld, S_alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"),
 						baro_alt,
+						sonar_alt,
 						next_WP.alt,
 						altitude_error,
 						(int)g.throttle_cruise,