From 17cd65a239aeb1fdfb8ef2605339fc6e6ec09f2b Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 7 Mar 2023 10:45:48 +0100 Subject: [PATCH] Navigator: remove support for DO_SET_SERVO Signed-off-by: Silvan Fuhrer --- msg/VehicleCommand.msg | 1 - src/modules/mavlink/mavlink_mission.cpp | 2 -- .../MissionFeasibility/FeasibilityChecker.cpp | 24 ------------------- src/modules/navigator/mission_block.cpp | 18 ++------------ src/modules/navigator/mission_block.h | 3 --- src/modules/navigator/navigation.h | 1 - 6 files changed, 2 insertions(+), 47 deletions(-) diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 0135873e38..ec74e916fc 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -36,7 +36,6 @@ uint16 VEHICLE_CMD_DO_SET_HOME = 179 # Changes the home location either to the uint16 VEHICLE_CMD_DO_SET_PARAMETER = 180 # Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_SET_RELAY = 181 # Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_SET_SERVO = 183 # Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_CHANGE_ALTITUDE = 186 # Set the vehicle to Loiter mode and change the altitude to specified value |Altitude| Frame of new altitude | Empty| Empty| Empty| Empty| Empty| diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 72daf437d6..e89f0f1f42 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1527,7 +1527,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_SET_HOME: - case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_LAND_START: case MAV_CMD_DO_TRIGGER_CONTROL: case MAV_CMD_DO_DIGICAM_CONTROL: @@ -1624,7 +1623,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_DO_CHANGE_SPEED: case NAV_CMD_DO_SET_HOME: - case NAV_CMD_DO_SET_SERVO: case NAV_CMD_DO_LAND_START: case NAV_CMD_DO_TRIGGER_CONTROL: case NAV_CMD_DO_DIGICAM_CONTROL: diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index bb8b3b9f61..3965fa6ad5 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -267,7 +267,6 @@ bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item, mission_item.nav_cmd != NAV_CMD_DO_JUMP && mission_item.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && mission_item.nav_cmd != NAV_CMD_DO_SET_HOME && - mission_item.nav_cmd != NAV_CMD_DO_SET_SERVO && mission_item.nav_cmd != NAV_CMD_DO_LAND_START && mission_item.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL && mission_item.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && @@ -300,28 +299,6 @@ bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item, return false; } - /* Check non navigation item */ - if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) { - - /* check actuator number */ - if (mission_item.params[0] < 0 || mission_item.params[0] > 5) { - mavlink_log_critical(_mavlink_log_pub, "Actuator number %d is out of bounds 0..5\t", - (int)mission_item.params[0]); - events::send(events::ID("navigator_mis_act_index"), {events::Log::Error, events::LogInternal::Warning}, - "Actuator number {1} is out of bounds 0..5", (int)mission_item.params[0]); - return false; - } - - /* check actuator value */ - if (mission_item.params[1] < -PWM_DEFAULT_MAX || mission_item.params[1] > PWM_DEFAULT_MAX) { - mavlink_log_critical(_mavlink_log_pub, - "Actuator value %d is out of bounds -PWM_DEFAULT_MAX..PWM_DEFAULT_MAX\t", (int)mission_item.params[1]); - events::send(events::ID("navigator_mis_act_range"), {events::Log::Error, events::LogInternal::Warning}, - "Actuator value {1} is out of bounds -{2}..{2}", (int)mission_item.params[1], PWM_DEFAULT_MAX); - return false; - } - } - // check if the mission starts with a land command while the vehicle is landed if ((current_index == 0) && mission_item.nav_cmd == NAV_CMD_LAND && _is_landed) { @@ -375,7 +352,6 @@ bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item) mission_item.nav_cmd != NAV_CMD_DO_JUMP && mission_item.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && mission_item.nav_cmd != NAV_CMD_DO_SET_HOME && - mission_item.nav_cmd != NAV_CMD_DO_SET_SERVO && mission_item.nav_cmd != NAV_CMD_DO_LAND_START && mission_item.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL && mission_item.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index f368b357f4..565b2f2150 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -70,7 +70,6 @@ MissionBlock::is_mission_item_reached_or_completed() switch (_mission_item.nav_cmd) { // Action Commands that doesn't have timeout completes instantaneously - case NAV_CMD_DO_SET_SERVO: case NAV_CMD_DO_SET_ACTUATOR: case NAV_CMD_DO_LAND_START: case NAV_CMD_DO_TRIGGER_CONTROL: @@ -556,21 +555,8 @@ MissionBlock::issue_command(const mission_item_s &item) return; } - if (item.nav_cmd == NAV_CMD_DO_SET_SERVO) { - PX4_INFO("DO_SET_SERVO command"); - - // XXX: we should issue a vehicle command and handle this somewhere else - actuator_controls_s actuators = {}; - actuators.timestamp = hrt_absolute_time(); - - // params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6) - // params[1] new value for selected actuator in ms 900...2000 - actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1]; - - _actuator_pub.publish(actuators); - - } else if (item.nav_cmd == NAV_CMD_DO_WINCH || - item.nav_cmd == NAV_CMD_DO_GRIPPER) { + if (item.nav_cmd == NAV_CMD_DO_WINCH || + item.nav_cmd == NAV_CMD_DO_GRIPPER) { // Initiate Payload Deployment vehicle_command_s vcmd = {}; vcmd.command = item.nav_cmd; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 705d07c246..450b57ba7b 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -212,7 +212,4 @@ protected: bool _payload_deploy_ack_successful{false}; // Flag to keep track of whether we received an acknowledgement for a successful payload deployment hrt_abstime _payload_deployed_time{0}; // Last payload deployment start time to handle timeouts float _payload_deploy_timeout_s{0.0f}; // Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received - - // Used for MAV_CMD_DO_SET_SERVO command of a Mission item - uORB::Publication _actuator_pub{ORB_ID(actuator_controls_2)}; }; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index b6a3361dac..c194b80532 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -74,7 +74,6 @@ enum NAV_CMD { NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_CHANGE_SPEED = 178, NAV_CMD_DO_SET_HOME = 179, - NAV_CMD_DO_SET_SERVO = 183, NAV_CMD_DO_SET_ACTUATOR = 187, NAV_CMD_DO_LAND_START = 189, NAV_CMD_DO_SET_ROI_LOCATION = 195,