State machine cleanup, introduced variable rates for MAVLink depending on the baud rate

apps/commander/commander.c

@@ -95,10 +95,7 @@ static int leds;
 static int buzzer;
 static int mavlink_fd;
 static bool commander_initialized = false;
-static struct vehicle_status_s current_status = {
-	.state_machine = SYSTEM_STATE_PREFLIGHT,
-	.mode = 0
-}; /**< Main state machine */
+static struct vehicle_status_s current_status; /**< Main state machine */
 static int stat_pub;
 
 static uint16_t nofix_counter = 0;
@@ -798,6 +795,7 @@ int commander_main(int argc, char *argv[])
 	/* make sure we are in preflight state */
 	memset(&current_status, 0, sizeof(current_status));
 	current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
+	current_status.flag_system_armed = false;
 
 	/* advertise to ORB */
 	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@@ -905,8 +903,8 @@ int commander_main(int argc, char *argv[])
 			}
 
 			/* toggle error led at 5 Hz in HIL mode */
-			if ((current_status.mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-				/* armed */
+			if (current_status.flag_hil_enabled) {
+				/* hil enabled */
 				led_toggle(LED_AMBER);
 
 			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {

apps/commander/state_machine_helper.c

@@ -83,14 +83,14 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 			} else {
 				ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
 			}
-
-			return;
 		}
 		break;
 
 	case SYSTEM_STATE_EMCY_LANDING:
 		/* Tell the controller to land */
-		//TODO: add emcy landing code here
+
+		/* set system flags according to state */
+		current_status->flag_system_armed = true;
 
 		fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
 		mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
@@ -98,11 +98,21 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 
 	case SYSTEM_STATE_EMCY_CUTOFF:
 		/* Tell the controller to cutoff the motors (thrust = 0) */
+
+		/* set system flags according to state */
+		current_status->flag_system_armed = false;
+
 		fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
 		mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
 		break;
 
 	case SYSTEM_STATE_GROUND_ERROR:
+
+		/* set system flags according to state */
+
+		/* prevent actuators from arming */
+		current_status->flag_system_armed = false;
+
 		fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
 		mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
 		break;
@@ -110,7 +120,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 	case SYSTEM_STATE_PREFLIGHT:
 		if (current_status->state_machine == SYSTEM_STATE_STANDBY
 		 || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			invalid_state = false;
+			/* set system flags according to state */
+			current_status->flag_system_armed = true;
 			mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
 		} else {
 			invalid_state = true;
@@ -122,6 +133,8 @@ 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;
+			/* set system flags according to state */
+			current_status->flag_system_armed = false;
 			mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
 			usleep(500000);
 			reboot();
@@ -133,22 +146,47 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		break;
 
 	case SYSTEM_STATE_STANDBY:
+		/* set system flags according to state */
+
+		/* standby enforces disarmed */
+		current_status->flag_system_armed = false;
+
 		mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
 		break;
 
 	case SYSTEM_STATE_GROUND_READY:
+
+		/* set system flags according to state */
+
+		/* ground ready has motors / actuators armed */
+		current_status->flag_system_armed = true;
+
 		mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
 		break;
 
 	case SYSTEM_STATE_AUTO:
+
+		/* set system flags according to state */
+
+		/* auto is airborne and in auto mode, motors armed */
+		current_status->flag_system_armed = true;
+
 		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
 		break;
 
 	case SYSTEM_STATE_STABILIZED:
+
+		/* set system flags according to state */
+		current_status->flag_system_armed = true;
+
 		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
 		break;
 
 	case SYSTEM_STATE_MANUAL:
+
+		/* set system flags according to state */
+		current_status->flag_system_armed = true;
+
 		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
 		break;
 
@@ -424,15 +462,10 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_
 
 void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	// XXX CHANGE BACK
 	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
 		printf("[commander] arming\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-	} /*else if (current_status->state_machine == SYSTEM_STATE_AUTO) {
-
-		printf("[commander] landing\n");
-		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-	}*/
+	}
 }
 
 void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
@@ -451,7 +484,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 {
 	int old_mode = current_status->flight_mode;
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-	current_status->control_manual_enabled = true;
+	current_status->flag_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_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -464,7 +497,7 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_
 {
 	int old_mode = current_status->flight_mode;
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
-	current_status->control_manual_enabled = true;
+	current_status->flag_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_AUTO) {
@@ -477,7 +510,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
 {
 	int old_mode = current_status->flight_mode;
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
-	current_status->control_manual_enabled = true;
+	current_status->flag_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) {
@@ -492,20 +525,11 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 	printf("in update state request\n");
 	uint8_t ret = 1;
 
-				current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
-			/* Set manual input enabled flag */
-			current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-
 	/* vehicle is disarmed, mode requests arming */
-	if (!(current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+	if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
 		/* only arm in standby state */
 		// XXX REMOVE
 		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			/* Set armed flag */
-			current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
-			/* Set manual input enabled flag */
-			current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
 			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 			ret = OK;
 			printf("[commander] arming due to command request\n");
@@ -513,28 +537,26 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 	}
 
 	/* vehicle is armed, mode requests disarming */
-	//if ((current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+	if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
 		/* only disarm in ground ready */
-		//if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-			/* Clear armed flag, leave manual input enabled */
-		//	current_status->mode &= ~VEHICLE_MODE_FLAG_SAFETY_ARMED;
-		//	do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-		//	ret = OK;
-		//	printf("[commander] disarming due to command request\n");
-		//}
-	//}
+		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+			ret = OK;
+			printf("[commander] disarming due to command request\n");
+		}
+	}
 
 	/* Switch on HIL if in standby */
 	if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
 		/* Enable HIL on request */
-		current_status->mode |= VEHICLE_MODE_FLAG_HIL_ENABLED;
+		current_status->flag_hil_enabled = true;
 		ret = OK;
 		state_machine_publish(status_pub, current_status, mavlink_fd);
 		printf("[commander] Enabling HIL\n");
 	}
 
 	/* NEVER actually switch off HIL without reboot */
-	if ((current_status->mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
+	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
 		fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
 		ret = ERROR;
 	}

apps/mavlink/mavlink.c

@@ -92,7 +92,6 @@ static mavlink_status_t status;
 static pthread_t receive_thread;
 static pthread_t uorb_receive_thread;
 
-static uint16_t mavlink_message_intervals[256];  /**< intervals at which to send MAVLink packets */
 /* Allocate storage space for waypoints */
 mavlink_wpm_storage wpm_s;
 
@@ -125,6 +124,7 @@ static struct vehicle_command_s vcmd;
 static int pub_hil_global_pos = -1;
 static int ardrone_motors_pub = -1;
 static int cmd_pub = -1;
+static int sensor_sub = -1;
 static int global_pos_sub = -1;
 static int local_pos_sub = -1;
 static int flow_pub = -1;
@@ -154,7 +154,7 @@ void handleMessage(mavlink_message_t *msg);
 /**
  * Enable / disable Hardware in the Loop simulation mode.
  */
-int set_hil_on_off(uint8_t vehicle_mode);
+int set_hil_on_off(bool hil_enabled);
 
 /**
  * Translate the custom state into standard mavlink modes and state.
@@ -281,12 +281,12 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa
 
 
 
-int set_hil_on_off(uint8_t vehicle_mode)
+int set_hil_on_off(bool hil_enabled)
 {
 	int ret = OK;
 
 	/* Enable HIL */
-	if ((vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && !mavlink_hil_enabled) {
+	if (hil_enabled && !mavlink_hil_enabled) {
 
 		//printf("\n HIL ON \n");
 
@@ -308,7 +308,7 @@ int set_hil_on_off(uint8_t vehicle_mode)
 		}
 	}
 
-	if (!(vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && mavlink_hil_enabled) {
+	if (!hil_enabled && mavlink_hil_enabled) {
 		mavlink_hil_enabled = false;
 		(void)close(pub_hil_attitude);
 		(void)close(pub_hil_global_pos);
@@ -326,7 +326,7 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t
 	*mavlink_mode = 0;
 
 	/* set mode flags independent of system state */
-	if (c_status->control_manual_enabled) {
+	if (c_status->flag_control_manual_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 	}
 
@@ -432,6 +432,28 @@ static void *receiveloop(void *arg)
 	return NULL;
 }
 
+static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
+{
+	int ret = OK;
+
+	switch (mavlink_msg_id) {
+		case MAVLINK_MSG_ID_SCALED_IMU:
+			/* senser sub triggers scaled IMU */
+			orb_set_interval(sensor_sub, min_interval);
+			break;
+		case MAVLINK_MSG_ID_RAW_IMU:
+			/* senser sub triggers RAW IMU */
+			orb_set_interval(sensor_sub, min_interval);
+			break;
+		default:
+			/* not found */
+			ret = ERROR;
+			break;
+	}
+
+	return ret;
+}
+
 /**
  * Listen for uORB topics and send via MAVLink.
  *
@@ -466,7 +488,7 @@ static void *uorb_receiveloop(void *arg)
 
 	/* --- SENSORS RAW VALUE --- */
 	/* subscribe to ORB for sensors raw */
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	orb_set_interval(sensor_sub, 100);	/* 10Hz updates */
 	fds[fdsc_count].fd = sensor_sub;
 	fds[fdsc_count].events = POLLIN;
@@ -662,11 +684,11 @@ static void *uorb_receiveloop(void *arg)
 				/* immediately communicate state changes back to user */
 				orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
 				/* enable or disable HIL */
-				set_hil_on_off(v_status.mode);
+				set_hil_on_off(v_status.flag_hil_enabled);
 
 				/* translate the current syste state to mavlink state and mode */
 				uint8_t mavlink_state = 0;
-				uint8_t mavlink_mode = v_status.mode;
+				uint8_t mavlink_mode = 0;
 				get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
 
 				/* send heartbeat */
@@ -691,7 +713,7 @@ static void *uorb_receiveloop(void *arg)
 					fw_control.attitude_control_output[3]);
 
 				/* Only send in HIL mode */
-				if (v_status.mode & MAV_MODE_FLAG_HIL_ENABLED) {
+				if (v_status.flag_hil_enabled) {
 					/* Send the desired attitude from RC or from the autonomous controller */
 					// XXX it should not depend on a RC setting, but on a system_state value
 
@@ -898,10 +920,11 @@ void handleMessage(mavlink_message_t *msg)
 
 		/* check if topic is advertised */
 		if (flow_pub <= 0) {
-			flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
+			flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+		} else {
+			/* publish */
+			orb_publish(ORB_ID(optical_flow), flow_pub, &f);
 		}
-		/* publish */
-		orb_publish(ORB_ID(optical_flow), flow_pub, &flow);
 	}
 
 	if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
@@ -1192,11 +1215,6 @@ int mavlink_main(int argc, char *argv[])
 	/* reate the device node that's used for sending text log messages, etc. */
 	register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
 
-	/* Send attitude at 10 Hz / every 100 ms */
-	mavlink_message_intervals[MAVLINK_MSG_ID_ATTITUDE] = 100;
-	/* Send raw sensor values at 10 Hz / every 100 ms */
-	mavlink_message_intervals[MAVLINK_MSG_ID_RAW_IMU] = 100;
-
 	/* default values for arguments */
 	char *uart_name = "/dev/ttyS0";
 	int baudrate = 57600;
@@ -1283,9 +1301,23 @@ int mavlink_main(int argc, char *argv[])
 	uint16_t counter = 0;
 	int lowspeed_counter = 0;
 
-	/**< Subscribe to system state and RC channels */
-	// int status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	// int rc_sub = orb_subscribe(ORB_ID(rc_channels));
+	/* all subscriptions are now active, set up initial guess about rate limits */
+	if (baudrate >= 921600) {
+		/* 1000 Hz / 1 ms */
+		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1);
+	} else if (baudrate >= 460800) {
+		/* 250 Hz / 4 ms */
+		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 4);
+	} else if (baudrate >= 115200) {
+		/* 50 Hz / 20 ms */
+		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 20);
+	} else if (baudrate >= 57600) {
+		/* 10 Hz / 100 ms */
+		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100);
+	} else {
+		/* very low baud rate, limit to 1 Hz / 1000 ms */
+		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000);
+	}
 
 	while (1) {
 
@@ -1305,7 +1337,7 @@ int mavlink_main(int argc, char *argv[])
 		if (lowspeed_counter == 10) {
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = v_status.mode;
+			uint8_t mavlink_mode = 0;
 			get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
 
 			/* send heartbeat */

apps/px4/attitude_estimator_bm/attitude_estimator_bm.c

@@ -245,7 +245,7 @@ int attitude_estimator_bm_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
 			/* switching from non-HIL to HIL mode */
 			//printf("[attitude_estimator_bm] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
-			if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
+			if (vstatus.flag_hil_enabled && !hil_enabled) {
 				hil_enabled = true;
 				publishing = false;
 				int ret = close(pub_att);

apps/sensors/sensors.c

@@ -452,7 +452,7 @@ int sensors_main(int argc, char *argv[])
 
 				/* switching from non-HIL to HIL mode */
 				//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
-				if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
+				if (vstatus.flag_hil_enabled && !hil_enabled) {
 					hil_enabled = true;
 					publishing = false;
 					int ret = close(sensor_pub);

apps/systemcmds/top/top.c

@@ -135,9 +135,9 @@ int top_main(int argc, char *argv[])
 					memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
 					header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
 #if CONFIG_RR_INTERVAL > 0
-					printf("\033[KPID\tCOMMAND%s    CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
+					printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
 #else
-					printf("\033[KPID\tCOMMAND%s    CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\n", header_spaces);
+					printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\n", header_spaces);
 #endif
 
 				} else {
@@ -190,7 +190,7 @@ int top_main(int argc, char *argv[])
 						runtime_spaces = "";
 					}
 
-					printf("\033[K % 2d\t%s%s\t% 8lld ms%s  \t % 2d.%03d \t % 6d B", (int)system_load.tasks[i].tcb->pid, system_load.tasks[i].tcb->name, spaces, (system_load.tasks[i].total_runtime / 1000), runtime_spaces, (int)(curr_loads[i] * 100), (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), (uint32_t)system_load.tasks[i].tcb->adj_stack_ptr - (uint32_t)system_load.tasks[i].tcb->xcp.regs[REG_R13]);
+					printf("\033[K % 2d\t%s%s % 8lld ms%s  \t % 2d.%03d \t % 6d B", (int)system_load.tasks[i].tcb->pid, system_load.tasks[i].tcb->name, spaces, (system_load.tasks[i].total_runtime / 1000), runtime_spaces, (int)(curr_loads[i] * 100), (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), (uint32_t)system_load.tasks[i].tcb->adj_stack_ptr - (uint32_t)system_load.tasks[i].tcb->xcp.regs[REG_R13]);
 					/* Print scheduling info with RR time slice */
 #if CONFIG_RR_INTERVAL > 0
 					printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice);

apps/uORB/topics/vehicle_status.h

@@ -87,9 +87,16 @@ enum VEHICLE_MODE_FLAG {
 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
 
 enum VEHICLE_FLIGHT_MODE {
-	VEHICLE_FLIGHT_MODE_MANUAL,
-	VEHICLE_FLIGHT_MODE_STABILIZED,
-	VEHICLE_FLIGHT_MODE_AUTO
+	VEHICLE_FLIGHT_MODE_MANUAL = 0,	/**< direct manual control, same as VEHICLE_FLIGHT_MODE_ATTITUDE for multirotors */
+	VEHICLE_FLIGHT_MODE_ATTITUDE,	/**< attitude or rate stabilization, as defined by VEHICLE_ATTITUDE_MODE */
+	VEHICLE_FLIGHT_MODE_STABILIZED,	/**< attitude or rate stabilization plus velocity or position stabilization */
+	VEHICLE_FLIGHT_MODE_AUTO	/**< attitude or rate stabilization plus absolute position control and waypoints */
+};
+
+enum VEHICLE_ATTITUDE_MODE {
+	VEHICLE_ATTITUDE_MODE_DIRECT,	/**< no attitude control, direct stick input mixing (only fixed wing) */
+	VEHICLE_ATTITUDE_MODE_RATES,	/**< body rates control mode */
+	VEHICLE_ATTITUDE_MODE_ATTITUDE	/**< tait-bryan attitude control mode */
 };
 
 /**
@@ -106,14 +113,21 @@ struct vehicle_status_s
 
 	commander_state_machine_t state_machine;	/**< Current flight state, main state machine */
 	enum VEHICLE_FLIGHT_MODE flight_mode;		/**< Current flight mode, as defined by mode switch */
+	enum VEHICLE_ATTITUDE_MODE attitute_mode;	/**< Current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
+
+	// uint8_t mode;
+
+
+	/* system flags - these represent the state predicates */
 
-	uint8_t mode;
+	bool flag_system_armed;				/**< True is motors / actuators are armed */
+	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
+	bool flag_hil_enabled;				/**< True if hardware in the loop simulation is enabled */
 
-	bool control_manual_enabled;			/**< true if manual input is mixed in */
-	bool control_rates_enabled;			/**< true if rates are stabilized */
-	bool control_attitude_enabled;			/**< true if attitude stabilization is mixed in */
-	bool control_speed_enabled;			/**< true if speed (implies direction) is controlled */
-	bool control_position_enabled;			/**< true if position is controlled */
+	// bool flag_control_rates_enabled;			/**< true if rates are stabilized */
+	// bool flag_control_attitude_enabled;			*< true if attitude stabilization is mixed in 
+	// bool control_speed_enabled;			/**< true if speed (implies direction) is controlled */
+	// bool control_position_enabled;			/**< true if position is controlled */
 
 	bool preflight_gyro_calibration;		/**< true if gyro calibration is requested */
 	bool preflight_mag_calibration;			/**< true if mag calibration is requested */