diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0b02cb04ef64..0ebdf419e554 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1040,6 +1040,54 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ break; + case vehicle_command_s::VEHICLE_CMD_DO_SET_SERVO: { + + struct actuator_controls_s actuator_controls = {}; + actuator_controls.timestamp = hrt_absolute_time(); + + for (size_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { + actuator_controls.control[i] = NAN; + } + + // scale 0 to 2000 to -1 to 1 + if ((int)cmd.param1 >= 0 && (int)cmd.param1 < actuator_controls_s::NUM_ACTUATOR_CONTROLS) { + actuator_controls.control[(int)cmd.param1] = 1.0f / 1000 * cmd.param2 - 1.0f; + _actuator_controls_2_pub.publish(actuator_controls); + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_ZOOM: { + + struct actuator_controls_s actuator_controls = {}; + actuator_controls.timestamp = hrt_absolute_time(); + + for (size_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { + actuator_controls.control[i] = NAN; + } + + switch ((int)(cmd.param1 + 0.5f)) { + case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_RANGE: + actuator_controls.control[actuator_controls_s::INDEX_CAMERA_ZOOM] = cmd.param2 / 50.0f - 1.0f; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + _actuator_controls_2_pub.publish(actuator_controls); + break; + + case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_STEP: + case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS: + case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH: + default: + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + } + + } + + 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: @@ -1058,7 +1106,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST: case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL: case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE: - case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_ZOOM: case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED: case vehicle_command_s::VEHICLE_CMD_DO_LAND_START: case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND: diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 3b2cf1658d48..cb1eb661baf4 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -417,6 +417,7 @@ class Commander : public ModuleBase, public ModuleParams // Publications uORB::Publication _armed_pub{ORB_ID(actuator_armed)}; + uORB::Publication _actuator_controls_2_pub{ORB_ID(actuator_controls_2)}; uORB::Publication _commander_state_pub{ORB_ID(commander_state)}; uORB::Publication _test_motor_pub{ORB_ID(test_motor)}; uORB::Publication _control_mode_pub{ORB_ID(vehicle_control_mode)}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 403c72ccb119..2aa64ca0de75 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -526,28 +526,6 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; } - } else if (cmd_mavlink.command == MAV_CMD_SET_CAMERA_ZOOM) { - struct actuator_controls_s actuator_controls = {}; - actuator_controls.timestamp = hrt_absolute_time(); - - for (size_t i = 0; i < 8; i++) { - actuator_controls.control[i] = NAN; - } - - switch ((int)(cmd_mavlink.param1 + 0.5f)) { - case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_RANGE: - actuator_controls.control[actuator_controls_s::INDEX_CAMERA_ZOOM] = cmd_mavlink.param2 / 50.0f - 1.0f; - break; - - case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_STEP: - case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS: - case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH: - default: - send_ack = false; - } - - _actuator_controls_pubs[actuator_controls_s::GROUP_INDEX_GIMBAL].publish(actuator_controls); - } else { send_ack = false; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4b8bc9ce01c6..aaaf4ecb6419 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include @@ -473,46 +472,31 @@ MissionBlock::issue_command(const mission_item_s &item) return; } - if (item.nav_cmd == NAV_CMD_DO_SET_SERVO) { - PX4_INFO("DO_SET_SERVO command"); + _action_start = hrt_absolute_time(); - // XXX: we should issue a vehicle command and handle this somewhere else - actuator_controls_s actuators = {}; - actuators.timestamp = hrt_absolute_time(); + // mission_item -> vehicle_command - // 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]; + // we're expecting a mission command item here so assign the "raw" inputs to the command + // (MAV_FRAME_MISSION mission item) + vehicle_command_s vcmd = {}; + vcmd.command = item.nav_cmd; + vcmd.param1 = item.params[0]; + vcmd.param2 = item.params[1]; + vcmd.param3 = item.params[2]; + vcmd.param4 = item.params[3]; - _actuator_pub.publish(actuators); + if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION && item.altitude_is_relative) { + vcmd.param5 = item.lat; + vcmd.param6 = item.lon; + vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; } else { - _action_start = hrt_absolute_time(); - - // mission_item -> vehicle_command - - // we're expecting a mission command item here so assign the "raw" inputs to the command - // (MAV_FRAME_MISSION mission item) - vehicle_command_s vcmd = {}; - vcmd.command = item.nav_cmd; - vcmd.param1 = item.params[0]; - vcmd.param2 = item.params[1]; - vcmd.param3 = item.params[2]; - vcmd.param4 = item.params[3]; - - if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION && item.altitude_is_relative) { - vcmd.param5 = item.lat; - vcmd.param6 = item.lon; - vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; - - } else { - vcmd.param5 = (double)item.params[4]; - vcmd.param6 = (double)item.params[5]; - vcmd.param7 = item.params[6]; - } - - _navigator->publish_vehicle_cmd(&vcmd); + vcmd.param5 = (double)item.params[4]; + vcmd.param6 = (double)item.params[5]; + vcmd.param7 = item.params[6]; } + + _navigator->publish_vehicle_cmd(&vcmd); } float diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 0e504230f56f..f79c7e76068f 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -46,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -154,5 +153,4 @@ class MissionBlock : public NavigatorMode hrt_abstime _action_start{0}; hrt_abstime _time_wp_reached{0}; - uORB::Publication _actuator_pub{ORB_ID(actuator_controls_2)}; };