Flight task custom

msg/commander_state.msg

@@ -16,5 +16,6 @@ uint8 MAIN_STATE_AUTO_LAND = 11
 uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12
 uint8 MAIN_STATE_AUTO_PRECLAND = 13
 uint8 MAIN_STATE_MAX = 14
+uint8 MAIN_STATE_CUSTOM = 15
 
 uint8 main_state		    	# main state machine

msg/vehicle_status.msg

@@ -35,7 +35,8 @@ uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17	# Takeoff
 uint8 NAVIGATION_STATE_AUTO_LAND = 18		# Land
 uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19	# Auto Follow
 uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20	# Precision land with landing target
-uint8 NAVIGATION_STATE_MAX = 21
+uint8 NAVIGATION_STATE_CUSTOM = 21 # Custom flight task from vehicle command
+uint8 NAVIGATION_STATE_MAX = 22
 
 uint8 RC_IN_MODE_DEFAULT = 0
 uint8 RC_IN_MODE_OFF = 1

src/lib/FlightTasks/FlightTasks.cpp

@@ -17,8 +17,6 @@ FlightTasks::FlightTasks()
 
 bool FlightTasks::update()
 {
-	_updateCommand();
-
 	if (isAnyTaskActive()) {
 		_subscription_array.update();
 		return _current_task.task->updateInitialize() && _current_task.task->update();
@@ -112,54 +110,20 @@ int FlightTasks::switchTask(int new_task_index)
 	return -1;
 }
 
-void FlightTasks::handleParameterUpdate()
-{
-	if (isAnyTaskActive()) {
-		_current_task.task->handleParameterUpdate();
-	}
-}
-
-const char *FlightTasks::errorToString(const int error)
+int FlightTasks::switchTask(const vehicle_command_s &command, uint8_t &cmd_result)
 {
-	for (auto e : _taskError) {
-		if (e.error == error) {
-			return e.msg;
-		}
-	}
-
-	return "This error is not mapped to a string or is unknown.";
-}
-
-void FlightTasks::_updateCommand()
-{
-	// lazy subscription to command topic
-	if (_sub_vehicle_command < 0) {
-		_sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
-	}
-
-	// check if there's any new command
-	bool updated = false;
-	orb_check(_sub_vehicle_command, &updated);
-
-	if (!updated) {
-		return;
-	}
-
-	// get command
-	struct vehicle_command_s command;
-	orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
-
 	// check what command it is
 	FlightTaskIndex desired_task = switchVehicleCommand(command.command);
 
-	if (desired_task == FlightTaskIndex::None) {
+	cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+
+	if (desired_task == FlightTaskIndex::None)
 		// ignore all unkown commands
-		return;
+	{
+		return -1;
 	}
 
-	// switch to the commanded task
 	int switch_result = switchTask(desired_task);
-	uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
 
 	// if we are in/switched to the desired task
 	if (switch_result >= 0) {
@@ -168,29 +132,26 @@ void FlightTasks::_updateCommand()
 		// if the task is running apply parameters to it and see if it rejects
 		if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
 			cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
-
-			// if we just switched and parameters are not accepted, go to failsafe
-			if (switch_result == 1) {
-				switchTask(FlightTaskIndex::ManualPosition);
-				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
-			}
 		}
 	}
 
-	// send back acknowledgment
-	vehicle_command_ack_s command_ack = {};
-	command_ack.command = command.command;
-	command_ack.result = cmd_result;
-	command_ack.result_param1 = switch_result;
-	command_ack.target_system = command.source_system;
-	command_ack.target_component = command.source_component;
-
-	if (_pub_vehicle_command_ack == nullptr) {
-		_pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-					   vehicle_command_ack_s::ORB_QUEUE_LENGTH);
+	return switch_result;
+}
 
-	} else {
-		orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack);
+void FlightTasks::handleParameterUpdate()
+{
+	if (isAnyTaskActive()) {
+		_current_task.task->handleParameterUpdate();
+	}
+}
 
+const char *FlightTasks::errorToString(const int error)
+{
+	for (auto e : _taskError) {
+		if (e.error == error) {
+			return e.msg;
+		}
 	}
+
+	return "This error is not mapped to a string or is unknown.";
 }

src/lib/FlightTasks/FlightTasks.hpp

@@ -102,6 +102,7 @@ class FlightTasks
 	 */
 	int switchTask(FlightTaskIndex new_task_index);
 	int switchTask(int new_task_index);
+	int switchTask(const vehicle_command_s &command, uint8_t &cmd_result);
 
 	/**
 	 * Get the number of the active task
@@ -157,12 +158,9 @@ class FlightTasks
 		{-3, "Activation Failed"}
 	};
 	/**
-	 * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them
+	 * Map vehicle command to Flght Task
 	 */
-	void _updateCommand();
 	FlightTaskIndex switchVehicleCommand(const int command);
-	int _sub_vehicle_command = -1; /**< topic handle on which commands are received */
-	orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */
 
 	int _initTask(FlightTaskIndex task_index);
 };

src/modules/commander/Commander.cpp

@@ -991,6 +991,18 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 		}
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_DO_ORBIT: {
+			/* ok, home set, use it to take off */
+			if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_CUSTOM, status_flags, &internal_state)) {
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				mavlink_log_critical(&mavlink_log_pub, "Custom command rejected");
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+		}
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
@@ -1022,7 +1034,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
-	case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 
@@ -3280,6 +3291,7 @@ set_control_mode()
 
 	switch (status.nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
+	case vehicle_status_s::NAVIGATION_STATE_CUSTOM:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = stabilization_required();

src/modules/commander/state_machine_helper.cpp

@@ -350,6 +350,13 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
 
 		break;
 
+	case commander_state_s::MAIN_STATE_CUSTOM:
+
+		// checks will be done in flight tasks
+		ret = TRANSITION_CHANGED;
+
+		break;
+
 	case commander_state_s::MAIN_STATE_MAX:
 	default:
 		break;
@@ -531,6 +538,30 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
 		}
 		break;
 
+	case commander_state_s::MAIN_STATE_CUSTOM: {
+
+			if (rc_lost && is_armed) {
+					enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
+
+					set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act,
+								vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
+
+					/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
+					/* A local position estimate is enough for POSCTL for multirotors,
+					* this enables POSCTL using e.g. flow.
+					* For fixedwing, a global position is needed. */
+
+				} else if (is_armed
+					&& check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1),
+							!status->is_rotary_wing)) {
+					// nothing to do - everything done in check_invalid_pos_nav_state
+
+				} else {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_CUSTOM;
+				}
+			}
+			break;
+
 	case commander_state_s::MAIN_STATE_AUTO_MISSION:
 
 		/* go into failsafe