Updated start script, checking commander mishaps

ROMFS/scripts/rc.PX4IOAR

@@ -19,6 +19,7 @@ sh /etc/init.d/rc.sensors
 # Start MAVLink
 #
 mavlink -d /dev/ttyS0 -b 57600 &
+usleep 5000
 
 #
 # Start the commander.
@@ -40,7 +41,7 @@ attitude_estimator_bm &
 #
 # XXX arguments?
 #
-px4fmu start
+#fmu mode_
 
 #
 # Fire up the AR.Drone controller.
@@ -54,12 +55,12 @@ ardrone_control -d /dev/ttyS1 -m attitude &
 #
 # XXX this should not need to be backgrounded
 #
-gps -d /dev/ttyS3 -m all &
+#gps -d /dev/ttyS3 -m all &
 
 #
 # Start logging to microSD if we can
 #
-sh /etc/init.d/rc.logging
+#sh /etc/init.d/rc.logging
 
 #
 # startup is done; we don't want the shell because we

apps/commander/state_machine_helper.c

@@ -111,7 +111,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		if (current_status->state_machine == SYSTEM_STATE_STANDBY
 		 || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
 			invalid_state = false;
-			mavlink_log_info(mavlink_fd, "[commander] Switched to PREFLIGHT state");
+			mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
 		} else {
 			invalid_state = true;
 			mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
@@ -161,6 +161,9 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		current_status->state_machine = new_state;
 		state_machine_publish(status_pub, current_status, mavlink_fd);
 	}
+	if (invalid_state) {
+		mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
+	}
 }
 
 void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
@@ -476,7 +479,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
 	current_status->control_manual_enabled = true;
 	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-	
+
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
 		printf("[commander] auto mode\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);

apps/mavlink/mavlink.c

@@ -925,6 +925,10 @@ void handleMessage(mavlink_message_t *msg)
 		vcmd.source_component = msg->compid;
 		vcmd.confirmation = 1;
 
+		/* check if topic is advertised */
+		if (cmd_pub <= 0) {
+			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+		}
 		/* create command */
 		orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
 	}