Merge branch 'navigator_new' into navigator_new_vector

src/modules/commander/commander.cpp

@@ -193,7 +193,7 @@ void usage(const char *reason);
 /**
  * React to commands that are sent e.g. from the mavlink module.
  */
-void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
 
 /**
  * Mainloop of commander.
@@ -345,10 +345,11 @@ void print_status()
 
 static orb_advert_t status_pub;
 
-void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
 	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+	bool ret = false;
 
 	/* only handle high-priority commands here */
 
@@ -375,6 +376,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
 				}
 			}
+			if (hil_ret == OK)
+				ret = true;
 
 			// TODO remove debug code
 			//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
@@ -407,6 +410,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					arming_res = TRANSITION_NOT_CHANGED;
 				}
 			}
+			if (arming_res == TRANSITION_CHANGED)
+				ret = true;
 
 			/* set main state */
 			transition_result_t main_res = TRANSITION_DENIED;
@@ -447,6 +452,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					}
 				}
 			}
+			if (main_res == TRANSITION_CHANGED)
+				ret = true;
 
 			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -473,6 +480,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				if (arming_res == TRANSITION_CHANGED) {
 					mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					ret = true;
 
 				} else {
 					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
@@ -482,12 +490,36 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
+	case VEHICLE_CMD_OVERRIDE_GOTO: {
+		// TODO listen vehicle_command topic directly from navigator (?)
+			unsigned int mav_goto = cmd->param1;
+			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
+				status->set_nav_state = NAV_STATE_LOITER;
+				status->set_nav_state_timestamp = hrt_absolute_time();
+				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				ret = true;
+
+			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
+				status->set_nav_state = NAV_STATE_MISSION;
+				status->set_nav_state_timestamp = hrt_absolute_time();
+				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				ret = true;
+
+			} else {
+				mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
+			}
+		}
+		break;
+
 	/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
 			if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
 				transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				ret = true;
 
 			} else {
 				/* reject parachute depoyment not armed */
@@ -1151,7 +1183,8 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			/* handle it */
-			handle_command(&status, &safety, &cmd, &armed);
+			if (handle_command(&status, &safety, &cmd, &armed))
+				status_changed = true;
 		}
 
 		/* check which state machines for changes, clear "changed" flag */
@@ -1359,7 +1392,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 		current_status->mode_switch = MODE_SWITCH_ASSISTED;
 	}
 
-	/* land switch */
+	/* return switch */
 	if (!isfinite(sp_man->return_switch)) {
 		current_status->return_switch = RETURN_SWITCH_NONE;
 
@@ -1367,7 +1400,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 		current_status->return_switch = RETURN_SWITCH_RETURN;
 
 	} else {
-		current_status->return_switch = RETURN_SWITCH_NONE;
+		current_status->return_switch = RETURN_SWITCH_NORMAL;
 	}
 
 	/* assisted switch */
@@ -1383,10 +1416,10 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 
 	/* mission switch  */
 	if (!isfinite(sp_man->mission_switch)) {
-		current_status->mission_switch = MISSION_SWITCH_MISSION;
+		current_status->mission_switch = MISSION_SWITCH_NONE;
 
 	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
-		current_status->mission_switch = MISSION_SWITCH_NONE;
+		current_status->mission_switch = MISSION_SWITCH_LOITER;
 
 	} else {
 		current_status->mission_switch = MISSION_SWITCH_MISSION;

src/modules/mavlink/mavlink.c

@@ -220,22 +220,18 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 	} else if (v_status.main_state == MAIN_STATE_AUTO) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-		// TODO use control_mode topic
+		// TODO review
-		/*
+		if (control_mode.nav_state == NAV_STATE_NONE) {
-		if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+		} else if (control_mode.nav_state == NAV_STATE_LOITER) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
+		} else if (control_mode.nav_state == NAV_STATE_MISSION_LOITER) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		} else if (control_mode.nav_state == NAV_STATE_RTL_LOITER) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		} else if (control_mode.nav_state == NAV_STATE_MISSION) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 		}
-		*/
 	}
 	*mavlink_custom_mode = custom_mode.data;
 

src/modules/mavlink/orb_listener.c

@@ -69,6 +69,7 @@ struct vehicle_global_position_s global_pos;
 struct vehicle_local_position_s local_pos;
 struct navigation_capabilities_s nav_cap;
 struct vehicle_status_s v_status;
+struct vehicle_control_mode_s control_mode;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
@@ -125,6 +126,7 @@ static void	l_vehicle_rates_setpoint(const struct listener *l);
 static void	l_home(const struct listener *l);
 static void	l_airspeed(const struct listener *l);
 static void	l_nav_cap(const struct listener *l);
+static void	l_control_mode(const struct listener *l);
 
 static const struct listener listeners[] = {
 	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
@@ -151,6 +153,7 @@ static const struct listener listeners[] = {
 	{l_home,			&mavlink_subs.home_sub,		0},
 	{l_airspeed,			&mavlink_subs.airspeed_sub,		0},
 	{l_nav_cap,			&mavlink_subs.navigation_capabilities_sub,		0},
+	{l_control_mode,			&mavlink_subs.control_mode_sub,		0},
 };
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -678,6 +681,26 @@ l_nav_cap(const struct listener *l)
 
 }
 
+void
+l_control_mode(const struct listener *l)
+{
+	orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode);
+
+	/* translate the current syste state to mavlink state and mode */
+	uint8_t mavlink_state = 0;
+	uint8_t mavlink_base_mode = 0;
+	uint32_t mavlink_custom_mode = 0;
+	get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+	/* send heartbeat */
+	mavlink_msg_heartbeat_send(chan,
+				   mavlink_system.type,
+				   MAV_AUTOPILOT_PX4,
+				   mavlink_base_mode,
+				   mavlink_custom_mode,
+				   mavlink_state);
+}
+
 static void *
 uorb_receive_thread(void *arg)
 {
@@ -753,6 +776,10 @@ uorb_receive_start(void)
 	status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	orb_set_interval(status_sub, 300);		/* max 3.33 Hz updates */
 
+	/* --- CONTROL MODE --- */
+	mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	orb_set_interval(mavlink_subs.control_mode_sub, 300);		/* max 3.33 Hz updates */
+
 	/* --- RC CHANNELS VALUE --- */
 	rc_sub = orb_subscribe(ORB_ID(rc_channels));
 	orb_set_interval(rc_sub, 100);			/* 10Hz updates */

src/modules/mavlink/orb_topics.h

@@ -95,6 +95,7 @@ struct mavlink_subscriptions {
 	int home_sub;
 	int airspeed_sub;
 	int navigation_capabilities_sub;
+	int control_mode_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;
@@ -111,6 +112,9 @@ extern struct navigation_capabilities_s nav_cap;
 /** Vehicle status */
 extern struct vehicle_status_s v_status;
 
+/** Vehicle control mode */
+extern struct vehicle_control_mode_s control_mode;
+
 /** RC channels */
 extern struct rc_channels_s rc;
 

src/modules/navigator/navigator_main.cpp

@@ -526,6 +526,8 @@ Navigator::task_main()
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
+	unsigned prevState = 0;
+
 	/* wakeup source(s) */
 	struct pollfd fds[7];
 
@@ -545,7 +547,6 @@ Navigator::task_main()
 	fds[6].fd = _vstatus_sub;
 	fds[6].events = POLLIN;
 
-
 	while (!_task_should_exit) {
 
 		/* wait for up to 100ms for data */
@@ -571,41 +572,75 @@ Navigator::task_main()
 
 			/* Evaluate state machine from commander and set the navigator mode accordingly */
 			if (_vstatus.main_state == MAIN_STATE_AUTO) {
-				if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+				bool stick_mode = false;
-					/* commander requested new navigation mode, try to set it */
+				if (!_vstatus.rc_signal_lost) {
-					_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
+					/* RC signal available, use control switches to set mode */
-
+					/* RETURN switch, overrides MISSION switch */
-					switch (_vstatus.set_nav_state) {
+					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
-					case NAV_STATE_INIT:
-					case NAV_STATE_NONE:
-						/* nothing to do */
-						break;
-
-					case NAV_STATE_LOITER:
-						dispatch(EVENT_LOITER_REQUESTED);
-						break;
-
-					case NAV_STATE_MISSION:
-						dispatch(EVENT_MISSION_REQUESTED);
-						break;
-
-					case NAV_STATE_RTL:
 						dispatch(EVENT_RTL_REQUESTED);
-						break;
+						stick_mode = true;
-
-					default:
-						warnx("ERROR: Requested navigation state not supported");
-						break;
-					}
-
-				} else {
-					/* try mission, if none is available fallback to loiter instead */
-					if (_mission.current_mission_available()) {
-						dispatch(EVENT_MISSION_REQUESTED);
 					} else {
-						dispatch(EVENT_LOITER_REQUESTED);
+						/* MISSION switch */
+						if (!stick_mode) {
+							if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
+								dispatch(EVENT_LOITER_REQUESTED);
+								stick_mode = true;
+							} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
+								/* switch to mission only if available */
+								if (_mission.current_mission_available()) {
+									dispatch(EVENT_MISSION_REQUESTED);
+								} else {
+									dispatch(EVENT_LOITER_REQUESTED);
+								}
+								stick_mode = true;
+							}
+						}
+						if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
+							/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
+							dispatch(EVENT_LOITER_REQUESTED);
+							stick_mode = true;
+						}
+					}
+				}
+
+				if (!stick_mode) {
+					if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+						/* commander requested new navigation mode, try to set it */
+						_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
+
+						switch (_vstatus.set_nav_state) {
+						case NAV_STATE_INIT:
+						case NAV_STATE_NONE:
+							/* nothing to do */
+							break;
+
+						case NAV_STATE_LOITER:
+							dispatch(EVENT_LOITER_REQUESTED);
+							break;
+
+						case NAV_STATE_MISSION:
+							dispatch(EVENT_MISSION_REQUESTED);
+							break;
+
+						case NAV_STATE_RTL:
+							dispatch(EVENT_RTL_REQUESTED);
+							break;
+
+						default:
+							warnx("ERROR: Requested navigation state not supported");
+							break;
+						}
+
+					} else {
+						/* on first switch to AUTO try mission, if none is available fallback to loiter instead */
+						if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) {
+							if (_mission.current_mission_available()) {
+								dispatch(EVENT_MISSION_REQUESTED);
+							} else {
+								dispatch(EVENT_LOITER_REQUESTED);
+							}
+						}
 					}
-					break;
 				}
 
 			} else {
@@ -673,6 +708,11 @@ Navigator::task_main()
 			}
 		}
 
+		if (myState != prevState) {
+			mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState);
+			prevState = myState;
+		}
+
 		publish_control_mode();
 
 		perf_end(_loop_perf);

src/modules/sensors/sensors.cpp

@@ -1370,18 +1370,6 @@ Sensors::rc_poll()
 			manual_control.yaw *= _parameters.rc_scale_yaw;
 		}
 
-		/* mode switch input */
-		manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
-
-		/* land switch input */
-		manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
-
-		/* assisted switch input */
-		manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
-
-		/* mission switch input */
-		manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
-
 		/* flaps */
 		if (_rc.function[FLAPS] >= 0) {
 
@@ -1392,14 +1380,26 @@ Sensors::rc_poll()
 			}
 		}
 
+		/* mode switch input */
 		if (_rc.function[MODE] >= 0) {
 			manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
 		}
 
+		/* assisted switch input */
+		if (_rc.function[ASSISTED] >= 0) {
+			manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+		}
+
+		/* mission switch input */
 		if (_rc.function[MISSION] >= 0) {
 			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 		}
 
+		/* return switch input */
+		if (_rc.function[RETURN] >= 0) {
+			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
+		}
+
 //		if (_rc.function[OFFBOARD_MODE] >= 0) {
 //			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
 //		}

src/modules/uORB/topics/vehicle_status.h

@@ -99,11 +99,13 @@ typedef enum {
 
 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;