commander, navigator: use updated manual_control_setpoint

src/modules/commander/commander.cpp

@@ -67,6 +67,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/subsystem_info.h>
@@ -112,8 +113,7 @@ extern struct system_load_s system_load;
 
 #define MAVLINK_OPEN_INTERVAL 50000
 
-#define STICK_ON_OFF_LIMIT 0.75f
+#define STICK_ON_OFF_LIMIT 0.9f
-#define STICK_THRUST_RANGE 1.0f
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
@@ -207,7 +207,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool
 
 void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
 
-transition_result_t set_main_state_rc(struct vehicle_status_s *status);
+transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
 
 void set_control_mode();
 
@@ -814,6 +814,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct subsystem_info_s info;
 	memset(&info, 0, sizeof(info));
 
+	/* Subscribe to position setpoint triplet */
+	int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	struct position_setpoint_triplet_s pos_sp_triplet;
+	memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -1005,6 +1010,13 @@ int commander_thread_main(int argc, char *argv[])
 			status_changed = true;
 		}
 
+		/* update subsystem */
+		orb_check(pos_sp_triplet_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
+		}
+
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 			/* compute system load */
 			uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
@@ -1135,7 +1147,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.is_rotary_wing &&
 			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
 			    (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 < 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
@@ -1153,7 +1165,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* 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) {
+			    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
 						print_reject_arm("NOT ARMING: Press safety switch first.");
@@ -1193,11 +1205,8 @@ int commander_thread_main(int argc, char *argv[])
 				transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 			}
 
-			/* fill status according to mode switches */
-			check_mode_switches(&sp_man, &status);
-
 			/* evaluate the main state machine according to mode switches */
-			res = set_main_state_rc(&status);
+			res = set_main_state_rc(&status, &sp_man);
 
 			/* play tune on mode change only if armed, blink LED always */
 			if (res == TRANSITION_CHANGED) {
@@ -1208,6 +1217,33 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
 
+			/* set navigation state */
+			/* RETURN switch, overrides MISSION switch */
+			if (sp_man.return_switch == SWITCH_POS_ON) {
+				/* switch to RTL if not already landed after RTL and home position set */
+				status.set_nav_state = NAV_STATE_RTL;
+				status.set_nav_state_timestamp = hrt_absolute_time();
+
+			} else {
+				/* MISSION switch */
+				if (sp_man.mission_switch == SWITCH_POS_ON) {
+					/* stick is in LOITER position */
+					status.set_nav_state = NAV_STATE_LOITER;
+					status.set_nav_state_timestamp = hrt_absolute_time();
+
+				} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
+					/* stick is in MISSION position */
+					status.set_nav_state = NAV_STATE_MISSION;
+					status.set_nav_state_timestamp = hrt_absolute_time();
+
+				} else if (sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE &&
+						pos_sp_triplet.nav_state == NAV_STATE_RTL) {
+					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
+					status.set_nav_state = NAV_STATE_MISSION;
+					status.set_nav_state_timestamp = hrt_absolute_time();
+				}
+			}
+
 		} else {
 			if (!status.rc_signal_lost) {
 				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
@@ -1255,7 +1291,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		// TODO remove this hack
 		/* flight termination in manual mode if assisted switch is on easy position */
-		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
 			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
 				tune_positive(armed.armed);
 			}
@@ -1470,76 +1506,26 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 	leds_counter++;
 }
 
-void
-check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
-{
-	/* main mode switch */
-	if (!isfinite(sp_man->mode_switch)) {
-		/* default to manual if signal is invalid */
-		status->mode_switch = MODE_SWITCH_MANUAL;
-
-	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
-		status->mode_switch = MODE_SWITCH_AUTO;
-
-	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
-		status->mode_switch = MODE_SWITCH_MANUAL;
-
-	} else {
-		status->mode_switch = MODE_SWITCH_ASSISTED;
-	}
-
-	/* return switch */
-	if (!isfinite(sp_man->return_switch)) {
-		status->return_switch = RETURN_SWITCH_NONE;
-
-	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
-		status->return_switch = RETURN_SWITCH_RETURN;
-
-	} else {
-		status->return_switch = RETURN_SWITCH_NORMAL;
-	}
-
-	/* assisted switch */
-	if (!isfinite(sp_man->assisted_switch)) {
-		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
-
-	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
-		status->assisted_switch = ASSISTED_SWITCH_EASY;
-
-	} else {
-		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
-	}
-
-	/* mission switch  */
-	if (!isfinite(sp_man->mission_switch)) {
-		status->mission_switch = MISSION_SWITCH_NONE;
-
-	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
-		status->mission_switch = MISSION_SWITCH_LOITER;
-
-	} else {
-		status->mission_switch = MISSION_SWITCH_MISSION;
-	}
-}
-
 transition_result_t
-set_main_state_rc(struct vehicle_status_s *status)
+set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man)
 {
 	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
 
-	switch (status->mode_switch) {
+	switch (sp_man->mode_switch) {
-	case MODE_SWITCH_MANUAL:
+	case SWITCH_POS_NONE:
+	case SWITCH_POS_OFF:		// MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
-	case MODE_SWITCH_ASSISTED:
+	case SWITCH_POS_MIDDLE:		// ASSISTED
-		if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
+		if (sp_man->assisted_switch == SWITCH_POS_ON) {
 			res = main_state_transition(status, MAIN_STATE_EASY);
 
-			if (res != TRANSITION_DENIED)
+			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
+			}
 
 			// else fallback to SEATBELT
 			print_reject_mode(status, "EASY");
@@ -1547,29 +1533,33 @@ set_main_state_rc(struct vehicle_status_s *status)
 
 		res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-		if (res != TRANSITION_DENIED)
+		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this mode
+		}
 
-		if (status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+		if (sp_man->assisted_switch != SWITCH_POS_ON) {
 			print_reject_mode(status, "SEATBELT");
+		}
 
 		// else fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
-	case MODE_SWITCH_AUTO:
+	case SWITCH_POS_ON:			// AUTO
 		res = main_state_transition(status, MAIN_STATE_AUTO);
 
-		if (res != TRANSITION_DENIED)
+		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
+		}
 
 		// else fallback to SEATBELT (EASY likely will not work too)
 		print_reject_mode(status, "AUTO");
 		res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-		if (res != TRANSITION_DENIED)
+		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
+		}
 
 		// else fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);

src/modules/navigator/navigator_main.cpp

@@ -690,84 +690,46 @@ Navigator::task_main()
 		if (fds[6].revents & POLLIN) {
 			vehicle_status_update();
 
-			/* evaluate state machine from commander and set the navigator mode accordingly */
+			/* evaluate state requested by commander */
 			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				bool stick_mode = false;
+				if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+					/* commander requested new navigation mode, try to set it */
+					switch (_vstatus.set_nav_state) {
+					case NAV_STATE_NONE:
+						/* nothing to do */
+						break;
 
-				if (!_vstatus.rc_signal_lost) {
+					case NAV_STATE_LOITER:
-					/* RC signal available, use control switches to set mode */
+						request_loiter_or_ready();
-					/* RETURN switch, overrides MISSION switch */
+						break;
-					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
+
-						/* switch to RTL if not already landed after RTL and home position set */
+					case NAV_STATE_MISSION:
+						request_mission_if_available();
+						break;
+
+					case NAV_STATE_RTL:
 						if (!(_rtl_state == RTL_STATE_DESCEND &&
-						      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+							  (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
-						    _vstatus.condition_home_position_valid) {
+							_vstatus.condition_home_position_valid) {
 							dispatch(EVENT_RTL_REQUESTED);
 						}
 
-						stick_mode = true;
+						break;
 
-					} else {
+					case NAV_STATE_LAND:
-						/* MISSION switch */
+						dispatch(EVENT_LAND_REQUESTED);
-						if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
-							request_loiter_or_ready();
-							stick_mode = true;
 
-						} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
+						break;
-							request_mission_if_available();
-							stick_mode = true;
-						}
 
-						if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
+					default:
-							/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
+						warnx("ERROR: Requested navigation state not supported");
-							request_mission_if_available();
+						break;
-							stick_mode = true;
-						}
 					}
-				}
 
-				if (!stick_mode) {
+				} else {
-					if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+					/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
-						/* commander requested new navigation mode, try to set it */
+					if (myState == NAV_STATE_NONE) {
-						_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
+						request_mission_if_available();
-
-						switch (_vstatus.set_nav_state) {
-						case NAV_STATE_NONE:
-							/* nothing to do */
-							break;
-
-						case NAV_STATE_LOITER:
-							request_loiter_or_ready();
-							break;
-
-						case NAV_STATE_MISSION:
-							request_mission_if_available();
-							break;
-
-						case NAV_STATE_RTL:
-							if (!(_rtl_state == RTL_STATE_DESCEND &&
-							      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
-							    _vstatus.condition_home_position_valid) {
-								dispatch(EVENT_RTL_REQUESTED);
-							}
-
-							break;
-
-						case NAV_STATE_LAND:
-							dispatch(EVENT_LAND_REQUESTED);
-
-							break;
-
-						default:
-							warnx("ERROR: Requested navigation state not supported");
-							break;
-						}
-
-					} else {
-						/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
-						if (myState == NAV_STATE_NONE) {
-							request_mission_if_available();
-						}
 					}
 				}
 
@@ -775,6 +737,8 @@ Navigator::task_main()
 				/* navigator shouldn't act */
 				dispatch(EVENT_NONE_REQUESTED);
 			}
+
+			_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
 		}
 
 		/* parameters updated */

src/modules/uORB/topics/vehicle_status.h

@@ -93,29 +93,6 @@ typedef enum {
 	FAILSAFE_STATE_MAX
 } failsafe_state_t;
 
-typedef enum {
-	MODE_SWITCH_MANUAL = 0,
-	MODE_SWITCH_ASSISTED,
-	MODE_SWITCH_AUTO
-} mode_switch_pos_t;
-
-typedef enum {
-	ASSISTED_SWITCH_SEATBELT = 0,
-	ASSISTED_SWITCH_EASY
-} assisted_switch_pos_t;
-
-typedef enum {
-	RETURN_SWITCH_NONE = 0,
-	RETURN_SWITCH_NORMAL,
-	RETURN_SWITCH_RETURN
-} return_switch_pos_t;
-
-typedef enum {
-	MISSION_SWITCH_NONE = 0,
-	MISSION_SWITCH_LOITER,
-	MISSION_SWITCH_MISSION
-} mission_switch_pos_t;
-
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
 	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -187,11 +164,6 @@ struct vehicle_status_s {
 
 	bool is_rotary_wing;
 
-	mode_switch_pos_t mode_switch;
-	return_switch_pos_t return_switch;
-	assisted_switch_pos_t assisted_switch;
-	mission_switch_pos_t mission_switch;
-
 	bool condition_battery_voltage_valid;
 	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
 	bool condition_system_sensors_initialized;