diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 4a504107a2..6ff52275ab 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -32,6 +32,7 @@ uint32 VEHICLE_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desir uint32 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| uint32 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| uint32 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ uint32 VEHICLE_CMD_DO_GO_AROUND = 191 # Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ uint32 VEHICLE_CMD_DO_REPOSITION = 192 uint32 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 63531451ec..71fc29ac89 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1175,6 +1175,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL: case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST: 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: case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: case vehicle_command_s::VEHICLE_CMD_LOGGING_START: diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 4b04d2205c..3d17bdc09d 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -982,6 +982,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_SET_SERVO: + case MAV_CMD_DO_LAND_START: case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_MOUNT_CONFIGURE: case MAV_CMD_DO_MOUNT_CONTROL: @@ -1045,6 +1046,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_DO_CHANGE_SPEED: case NAV_CMD_DO_SET_SERVO: + case NAV_CMD_DO_LAND_START: case NAV_CMD_DO_DIGICAM_CONTROL: case NAV_CMD_IMAGE_START_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE: diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d60dfd4a4d..c3b188a920 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -270,6 +270,33 @@ bool Mission::set_current_offboard_mission_index(unsigned index) return false; } +unsigned +Mission::find_offboard_land_start() +{ + /* find the first MAV_CMD_DO_LAND_START and return the index + * return -1 if not found + * + * TODO: implement full spec and find closest landing point geographically + */ + + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); + + for (size_t i = 0; i < _offboard_mission.count; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return -1; + } + + if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { + return i; + } + } + + return -1; +} + void Mission::update_onboard_mission() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4aa8ab8257..1917770e81 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -94,6 +94,8 @@ public: bool set_current_offboard_mission_index(unsigned index); + unsigned find_offboard_land_start(); + private: /** * Update onboard mission topic diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 9e0963f7fe..e12caaf996 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -103,6 +103,7 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_LOITER_UNLIMITED: return false; + case NAV_CMD_DO_LAND_START: case NAV_CMD_DO_DIGICAM_CONTROL: case NAV_CMD_IMAGE_START_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE: @@ -432,6 +433,11 @@ MissionBlock::issue_command(const struct mission_item_s *item) return; } + // NAV_CMD_DO_LAND_START is only a marker + if (item->nav_cmd == NAV_CMD_DO_LAND_START) { + 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 @@ -470,6 +476,7 @@ MissionBlock::item_contains_position(const struct mission_item_s *item) if (item->nav_cmd == NAV_CMD_DO_JUMP || item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED || item->nav_cmd == NAV_CMD_DO_SET_SERVO || + item->nav_cmd == NAV_CMD_DO_LAND_START || item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || item->nav_cmd == NAV_CMD_IMAGE_START_CAPTURE || item->nav_cmd == NAV_CMD_IMAGE_STOP_CAPTURE || diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 03c173dd7a..3ff1307d63 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -250,8 +250,9 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF && missionitem.nav_cmd != NAV_CMD_VTOL_LAND && missionitem.nav_cmd != NAV_CMD_DO_JUMP && - missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && + missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && + missionitem.nav_cmd != NAV_CMD_DO_LAND_START && missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 062b60ab1e..f021273917 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -69,6 +69,7 @@ enum NAV_CMD { NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_CHANGE_SPEED = 178, NAV_CMD_DO_SET_SERVO=183, + NAV_CMD_DO_LAND_START=189, NAV_CMD_DO_SET_ROI=201, NAV_CMD_DO_DIGICAM_CONTROL=203, NAV_CMD_DO_MOUNT_CONFIGURE=204, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 40e4530de8..361a8510af 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -492,6 +492,24 @@ Navigator::task_main() rep->current.valid = true; rep->next.valid = false; + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) { + + /* find NAV_CMD_DO_LAND_START in the mission and + * use MAV_CMD_MISSION_START to start the mission there + */ + unsigned land_start = _mission.find_offboard_land_start(); + if (land_start != -1) { + vehicle_command_s vcmd = {}; + vcmd.target_system = get_vstatus()->system_id; + vcmd.target_component = get_vstatus()->component_id; + vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; + vcmd.param1 = land_start; + vcmd.param2 = 0; + + orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + (void)orb_unadvertise(pub); + } + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (get_mission_result()->valid &&