From be1621877f6708a0d9439824fdab1542369a4039 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 13 Oct 2014 16:03:56 +0900 Subject: [PATCH] Mission: support GUIDED_ENABLE and GUIDED_LIMITS This replaces the ardupilot only NAV_GUIDED command. Also remove support for NAV_VELOCITY mission command which will be replaced by SET_POSITION_TARGET non-mission command. --- libraries/AP_Mission/AP_Mission.cpp | 50 +++++++++++------------------ libraries/AP_Mission/AP_Mission.h | 32 +++++++----------- 2 files changed, 29 insertions(+), 53 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 613c273173..49458bbe63 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -517,23 +517,9 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP cmd.p1 = packet.param1; // delay at waypoint in seconds break; -#ifdef MAV_CMD_NAV_GUIDED - case MAV_CMD_NAV_GUIDED: // MAV ID: 90 - cmd.p1 = packet.param1; // max time in seconds the external controller will be allowed to control the vehicle - cmd.content.nav_guided.alt_min = packet.param2; // min alt below which the command will be aborted. 0 for no lower alt limit - cmd.content.nav_guided.alt_max = packet.param3; // max alt above which the command will be aborted. 0 for no upper alt limit - cmd.content.nav_guided.horiz_max = packet.param4; // max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit - break; -#endif - -#ifdef MAV_CMD_NAV_VELOCITY - case MAV_CMD_NAV_VELOCITY: // MAV ID: 91 - cmd.p1 = packet.param1; // frame - unused - cmd.content.nav_velocity.x = packet.x; // lat (i.e. north) velocity in m/s - cmd.content.nav_velocity.y = packet.y; // lon (i.e. east) velocity in m/s - cmd.content.nav_velocity.z = packet.z; // vertical (i.e. up) velocity in m/s + case MAV_CMD_NAV_GUIDED_ENABLE: // MAV ID: 92 + cmd.p1 = packet.param1; // on/off. >0.5 means "on", hand-over control to external controller break; -#endif case MAV_CMD_CONDITION_DELAY: // MAV ID: 112 cmd.content.delay.seconds = packet.param1; // delay in seconds @@ -630,6 +616,13 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP cmd.content.gripper.action = packet.param2; // action 0=release, 1=grab. See GRIPPER_ACTION enum break; + case MAV_CMD_DO_GUIDED_LIMITS: // MAV ID: 222 + cmd.p1 = packet.param1; // max time in seconds the external controller will be allowed to control the vehicle + cmd.content.guided_limits.alt_min = packet.param2; // min alt below which the command will be aborted. 0 for no lower alt limit + cmd.content.guided_limits.alt_max = packet.param3; // max alt above which the command will be aborted. 0 for no upper alt limit + cmd.content.guided_limits.horiz_max = packet.param4;// max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit + break; + default: // unrecognised command return false; @@ -785,23 +778,9 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd, packet.param1 = cmd.p1; // delay at waypoint in seconds break; -#ifdef MAV_CMD_NAV_GUIDED - case MAV_CMD_NAV_GUIDED: // MAV ID: 90 - packet.param1 = cmd.p1; // max time in seconds the external controller will be allowed to control the vehicle - packet.param2 = cmd.content.nav_guided.alt_min; // min alt below which the command will be aborted. 0 for no lower alt limit - packet.param3 = cmd.content.nav_guided.alt_max; // max alt above which the command will be aborted. 0 for no upper alt limit - packet.param4 = cmd.content.nav_guided.horiz_max; // max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit - break; -#endif - -#ifdef MAV_CMD_NAV_VELOCITY - case MAV_CMD_NAV_VELOCITY: // MAV ID: 91 - packet.param1 = cmd.p1; // frame - unused - packet.x = cmd.content.nav_velocity.x; // lat (i.e. north) velocity in m/s - packet.y = cmd.content.nav_velocity.y; // lon (i.e. east) velocity in m/s - packet.z = cmd.content.nav_velocity.z; // vertical (i.e. up) velocity in m/s + case MAV_CMD_NAV_GUIDED_ENABLE: // MAV ID: 92 + packet.param1 = cmd.p1; // on/off. >0.5 means "on", hand-over control to external controller break; -#endif case MAV_CMD_CONDITION_DELAY: // MAV ID: 112 packet.param1 = cmd.content.delay.seconds; // delay in seconds @@ -898,6 +877,13 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd, packet.param2 = cmd.content.gripper.action; // action 0=release, 1=grab. See GRIPPER_ACTION enum break; + case MAV_CMD_DO_GUIDED_LIMITS: // MAV ID: 222 + packet.param1 = cmd.p1; // max time in seconds the external controller will be allowed to control the vehicle + packet.param2 = cmd.content.guided_limits.alt_min; // min alt below which the command will be aborted. 0 for no lower alt limit + packet.param3 = cmd.content.guided_limits.alt_max; // max alt above which the command will be aborted. 0 for no upper alt limit + packet.param4 = cmd.content.guided_limits.horiz_max;// max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit + break; + default: // unrecognised command return false; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index b0e0374b6d..ec13605dc7 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -43,21 +43,6 @@ class AP_Mission { public: - - // nav guided command - struct PACKED Nav_Guided_Command { - float alt_min; // min alt below which the command will be aborted. 0 for no lower alt limit - float alt_max; // max alt above which the command will be aborted. 0 for no upper alt limit - float horiz_max; // max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit - }; - - // nav velocity command - struct PACKED Nav_Velocity_Command { - float x; // lat (i.e. north) velocity in m/s - float y; // lon (i.e. east) velocity in m/s - float z; // vertical velocity in m/s - }; - // jump command structure struct PACKED Jump_Command { uint16_t target; // target command id @@ -127,13 +112,15 @@ public: uint8_t action; // action (0 = release, 1 = grab) }; - union PACKED Content { - // Nav_Guided_Command - Nav_Guided_Command nav_guided; - - // Nav_Velocity_Command - Nav_Velocity_Command nav_velocity; + // nav guided command + struct PACKED Guided_Limits_Command { + // max time is held in p1 field + float alt_min; // min alt below which the command will be aborted. 0 for no lower alt limit + float alt_max; // max alt above which the command will be aborted. 0 for no upper alt limit + float horiz_max; // max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit + }; + union PACKED Content { // jump structure Jump_Command jump; @@ -167,6 +154,9 @@ public: // do-gripper Gripper_Command gripper; + // do-guided-limits + Guided_Limits_Command guided_limits; + // location Location location; // Waypoint location