offboard setpoint support

src/modules/commander/commander.cpp

@@ -119,6 +119,7 @@ extern struct system_load_s system_load;
 
 #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
 #define RC_TIMEOUT 100000
+#define OFFBOARD_TIMEOUT 200000
 #define DIFFPRESS_TIMEOUT 2000000
 
 #define PRINT_INTERVAL	5000000
@@ -699,9 +700,9 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&sp_man, 0, sizeof(sp_man));
 
 	/* Subscribe to offboard control data */
-	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+	int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	struct offboard_control_setpoint_s sp_offboard;
+	struct offboard_control_setpoint_s offboard_sp;
-	memset(&sp_offboard, 0, sizeof(sp_offboard));
+	memset(&offboard_sp, 0, sizeof(offboard_sp));
 
 	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -815,10 +816,10 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
 		}
 
-		orb_check(sp_offboard_sub, &updated);
+		orb_check(offboard_sp_sub, &updated);
 
 		if (updated) {
-			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+			orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp);
 		}
 
 		orb_check(sensor_sub, &updated);
@@ -1040,126 +1041,164 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* ignore RC signals if in offboard control mode */
+		/* start RC input check */
-		if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
+		if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
-			/* start RC input check */
+			/* handle the case where RC signal was regained */
-			if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+			if (!status.rc_signal_found_once) {
-				/* handle the case where RC signal was regained */
+				status.rc_signal_found_once = true;
-				if (!status.rc_signal_found_once) {
+				mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
-					status.rc_signal_found_once = true;
+				status_changed = true;
-					mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
+
+			} else {
+				if (status.rc_signal_lost) {
+					mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
 					status_changed = true;
-
-				} else {
-					if (status.rc_signal_lost) {
-						mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
-						status_changed = true;
-					}
 				}
+			}
 
-				status.rc_signal_lost = false;
+			status.rc_signal_lost = false;
 
-				transition_result_t res;	// store all transitions results here
+			transition_result_t res;	// store all transitions results here
 
-				/* arm/disarm by RC */
+			/* arm/disarm by RC */
-				res = TRANSITION_NOT_CHANGED;
+			res = TRANSITION_NOT_CHANGED;
 
-				/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
+			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
-				 * do it only for rotary wings */
+			 * do it only for rotary wings */
-				if (status.is_rotary_wing &&
+			if (status.is_rotary_wing &&
-				    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
+				(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-				    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+				(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
-				    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+				sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 
-					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
+					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
-						arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-						res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					res = arming_state_transition(&status, &safety, new_arming_state, &armed);
-						stick_off_counter = 0;
-
-					} else {
-						stick_off_counter++;
-					}
-
-				} else {
 					stick_off_counter = 0;
-				}
-
-				/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
-				if (status.arming_state == ARMING_STATE_STANDBY &&
-				    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						if (safety.safety_switch_available && !safety.safety_off) {
-							print_reject_arm("NOT ARMING: Press safety switch first.");
-
-						} else if (status.main_state != MAIN_STATE_MANUAL) {
-							print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
-
-						} else {
-							res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
-						}
-
-						stick_on_counter = 0;
-
-					} else {
-						stick_on_counter++;
-					}
 
 				} else {
-					stick_on_counter = 0;
+					stick_off_counter++;
-				}
-
-				if (res == TRANSITION_CHANGED) {
-					if (status.arming_state == ARMING_STATE_ARMED) {
-						mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
-
-					} else {
-						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
-					}
-
-				} else if (res == TRANSITION_DENIED) {
-					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-				}
-
-				/* fill current_status according to mode switches */
-				check_mode_switches(&sp_man, &status);
-
-				/* evaluate the main state machine */
-				res = check_main_state_machine(&status);
-
-				if (res == TRANSITION_CHANGED) {
-					//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
-					tune_positive();
-
-				} else if (res == TRANSITION_DENIED) {
-					/* DENIED here indicates bug in the commander */
-					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
 				}
 
 			} else {
-				if (!status.rc_signal_lost) {
+				stick_off_counter = 0;
-					mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
+			}
-					status.rc_signal_lost = true;
+
-					status_changed = true;
+			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
+			if (status.arming_state == ARMING_STATE_STANDBY &&
+				sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+					if (safety.safety_switch_available && !safety.safety_off) {
+						print_reject_arm("NOT ARMING: Press safety switch first.");
+
+					} else if (status.main_state != MAIN_STATE_MANUAL) {
+						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
+
+					} else {
+						res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+					}
+
+					stick_on_counter = 0;
+
+				} else {
+					stick_on_counter++;
 				}
-				if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
+
-					transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+			} else {
+				stick_on_counter = 0;
+			}
+
+			if (res == TRANSITION_CHANGED) {
+				if (status.arming_state == ARMING_STATE_ARMED) {
+					mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
+
+				} else {
+					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
+				}
+
+			} else if (res == TRANSITION_DENIED) {
+				warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+				mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+			}
+
+			/* fill current_status according to mode switches */
+			check_mode_switches(&sp_man, &status);
+
+			/* evaluate the main state machine */
+			res = check_main_state_machine(&status);
+
+			if (res == TRANSITION_CHANGED) {
+				//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
+				tune_positive();
+
+			} else if (res == TRANSITION_DENIED) {
+				/* DENIED here indicates bug in the commander */
+				warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+				mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+			}
+
+		} else {
+			if (!status.rc_signal_lost) {
+				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
+				status.rc_signal_lost = true;
+				status_changed = true;
+			}
+			if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
+				transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+				if (res == TRANSITION_CHANGED) {
+					mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
+					status.set_nav_state = NAV_STATE_RTL;
+					status.set_nav_state_timestamp = hrt_absolute_time();
+				} else if (status.main_state != MAIN_STATE_SEATBELT) {
+					res = main_state_transition(&status, MAIN_STATE_SEATBELT);
 					if (res == TRANSITION_CHANGED) {
-						mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
+						mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
-						status.set_nav_state = NAV_STATE_RTL;
-						status.set_nav_state_timestamp = hrt_absolute_time();
-					} else if (status.main_state != MAIN_STATE_SEATBELT) {
-						res = main_state_transition(&status, MAIN_STATE_SEATBELT);
-						if (res == TRANSITION_CHANGED) {
-							mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
-						}
 					}
 				}
 			}
 		}
 
+		/* check offboard signal */
+		if (hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) {
+			if (!status.offboard_control_signal_found_once) {
+				status.offboard_control_signal_found_once = true;
+				mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time");
+				status_changed = true;
+
+			} else {
+				if (status.offboard_control_signal_lost) {
+					mavlink_log_info(mavlink_fd, "[cmd] offboard signal regained");
+					status_changed = true;
+				}
+			}
+
+			status.offboard_control_signal_lost = false;
+
+			if (status.main_state == MAIN_STATE_OFFBOARD) {
+				if (offboard_sp.armed && !armed.armed) {
+					if (!safety.safety_switch_available || safety.safety_off) {
+						transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						if (res == TRANSITION_CHANGED) {
+							mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal");
+						}
+					}
+				} else if (!offboard_sp.armed && armed.armed) {
+					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+					transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					if (res == TRANSITION_CHANGED) {
+						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal");
+					}
+				}
+			}
+
+		} else {
+			if (!status.offboard_control_signal_lost) {
+				mavlink_log_critical(mavlink_fd, "[cmd[ CRITICAL: OFFBOARD SIGNAL LOST");
+				status.offboard_control_signal_lost = true;
+				status_changed = true;
+			}
+		}
+
 		/*  Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
 		if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
 			transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
@@ -1264,7 +1303,7 @@ int commander_thread_main(int argc, char *argv[])
 	led_deinit();
 	buzzer_deinit();
 	close(sp_man_sub);
-	close(sp_offboard_sub);
+	close(offboard_sp_sub);
 	close(local_position_sub);
 	close(global_position_sub);
 	close(gps_sub);

src/modules/mavlink/mavlink_receiver.cpp

@@ -97,6 +97,7 @@ static mavlink_status_t status;
 static struct vehicle_vicon_position_s vicon_position;
 static struct vehicle_command_s vcmd;
 static struct offboard_control_setpoint_s offboard_control_sp;
+static struct vehicle_attitude_setpoint_s att_sp;
 
 struct vehicle_global_position_s hil_global_pos;
 struct vehicle_local_position_s hil_local_pos;
@@ -125,6 +126,7 @@ static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
 
 static orb_advert_t offboard_control_sp_pub = -1;
+static orb_advert_t att_sp_pub = -1;
 static orb_advert_t vicon_position_pub = -1;
 static orb_advert_t telemetry_status_pub = -1;
 
@@ -266,8 +268,8 @@ handle_message(mavlink_message_t *msg)
 	/* Handle quadrotor motor setpoints */
 
 	if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
-		mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
+		mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_swarm_setpoint;
-		mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
+		mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_swarm_setpoint);
 
 		if (mavlink_system.sysid < 4) {
 
@@ -281,7 +283,7 @@ handle_message(mavlink_message_t *msg)
 			uint8_t ml_mode = 0;
 			bool ml_armed = false;
 
-			switch (quad_motors_setpoint.mode) {
+			switch (quad_swarm_setpoint.mode) {
 			case 0:
 				ml_armed = false;
 				break;
@@ -307,12 +309,12 @@ handle_message(mavlink_message_t *msg)
 				break;
 			}
 
-			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
+			offboard_control_sp.p1 = (float)quad_swarm_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
-			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
+			offboard_control_sp.p2 = (float)quad_swarm_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
-			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
+			offboard_control_sp.p3 = (float)quad_swarm_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
-			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
+			offboard_control_sp.p4 = (float)quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
 
-			if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
+			if (quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
 				ml_armed = false;
 			}
 
@@ -326,9 +328,37 @@ handle_message(mavlink_message_t *msg)
 				offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
 
 			} else {
-				/* Publish */
+				/* publish */
 				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
 			}
+
+			if (v_status.main_state == MAIN_STATE_OFFBOARD) {
+				/* in offboard mode also publish setpoint directly */
+				switch (offboard_control_sp.mode) {
+				case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
+					att_sp.timestamp = hrt_absolute_time();
+					att_sp.roll_body = offboard_control_sp.p1;
+					att_sp.pitch_body = offboard_control_sp.p2;
+					att_sp.yaw_body = offboard_control_sp.p3;
+					att_sp.thrust = offboard_control_sp.p4;
+					att_sp.R_valid = false;
+					att_sp.q_d_valid = false;
+					att_sp.q_e_valid = false;
+					att_sp.roll_reset_integral = false;
+
+					/* check if topic has to be advertised */
+					if (att_sp_pub <= 0) {
+						att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+
+					} else {
+						/* publish */
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+					}
+					break;
+				default:
+					break;
+				}
+			}
 		}
 	}
 

src/modules/navigator/navigator_main.cpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
 #include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/offboard_control_setpoint.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/state_table.h>
@@ -149,8 +150,9 @@ private:
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int		_params_sub;			/**< notification of parameter updates */
 	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
-	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
+	int		_onboard_mission_sub;		/**< notification of onboard mission updates */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_offboard_sub;			/**< offboard control setpoint subscription */
 
 	orb_advert_t	_triplet_pub;			/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
@@ -162,6 +164,7 @@ private:
 	struct home_position_s				_home_pos;		/**< home position for RTL */
 	struct mission_item_triplet_s			_mission_item_triplet;	/**< triplet of mission items */
 	struct mission_result_s				_mission_result;	/**< mission result for commander/mavlink */
+	struct offboard_control_setpoint_s	_offboard;		/**< offboard control setpoint, to get desired offboard control mode only */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 	
@@ -383,6 +386,7 @@ Navigator::Navigator() :
 	_offboard_mission_sub(-1),
 	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
+	_offboard_sub(-1),
 
 /* publications */
 	_triplet_pub(-1),
@@ -422,6 +426,7 @@ Navigator::Navigator() :
 	memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s));
 
 	memset(&_mission_result, 0, sizeof(struct mission_result_s));
+	memset(&_offboard, 0, sizeof(_offboard));
 
 	nav_states_str[0] = "NONE";
 	nav_states_str[1] = "READY";
@@ -591,6 +596,7 @@ Navigator::task_main()
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
+	_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
 	
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -1523,12 +1529,49 @@ Navigator::publish_control_mode()
 	case MAIN_STATE_OFFBOARD:
 		_control_mode.flag_control_offboard_enabled = true;
 		_control_mode.flag_control_manual_enabled = false;
-		_control_mode.flag_control_rates_enabled = false;
+
-		_control_mode.flag_control_attitude_enabled = false;
+		switch (_offboard.mode) {
-		_control_mode.flag_control_altitude_enabled = false;
+		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
-		_control_mode.flag_control_climb_rate_enabled = false;
+			_control_mode.flag_control_rates_enabled = true;
-		_control_mode.flag_control_position_enabled = false;
+			_control_mode.flag_control_attitude_enabled = false;
-		_control_mode.flag_control_velocity_enabled = false;
+			_control_mode.flag_control_altitude_enabled = false;
+			_control_mode.flag_control_climb_rate_enabled = false;
+			_control_mode.flag_control_position_enabled = false;
+			_control_mode.flag_control_velocity_enabled = false;
+			break;
+		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
+			_control_mode.flag_control_rates_enabled = true;
+			_control_mode.flag_control_attitude_enabled = true;
+			_control_mode.flag_control_altitude_enabled = false;
+			_control_mode.flag_control_climb_rate_enabled = false;
+			_control_mode.flag_control_position_enabled = false;
+			_control_mode.flag_control_velocity_enabled = false;
+			break;
+		case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
+			_control_mode.flag_control_rates_enabled = true;
+			_control_mode.flag_control_attitude_enabled = true;
+			_control_mode.flag_control_altitude_enabled = false;
+			_control_mode.flag_control_climb_rate_enabled = true;
+			_control_mode.flag_control_position_enabled = false;
+			_control_mode.flag_control_velocity_enabled = true;
+			break;
+		case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
+			_control_mode.flag_control_rates_enabled = true;
+			_control_mode.flag_control_attitude_enabled = true;
+			_control_mode.flag_control_altitude_enabled = true;
+			_control_mode.flag_control_climb_rate_enabled = true;
+			_control_mode.flag_control_position_enabled = true;
+			_control_mode.flag_control_velocity_enabled = true;
+			break;
+		default:
+			_control_mode.flag_control_rates_enabled = false;
+			_control_mode.flag_control_attitude_enabled = false;
+			_control_mode.flag_control_altitude_enabled = false;
+			_control_mode.flag_control_climb_rate_enabled = false;
+			_control_mode.flag_control_position_enabled = false;
+			_control_mode.flag_control_velocity_enabled = false;
+			break;
+		}
 		break;
 
 	default: