zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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.
mission-4.1.18
Randy Mackay 10 years ago
parent
e5c3c306bd
commit
be1621877f
2 changed files with 29 additions and 53 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 50
      libraries/AP_Mission/AP_Mission.cpp
  2. 32
      libraries/AP_Mission/AP_Mission.h

50
libraries/AP_Mission/AP_Mission.cpp
Unescape View File

@ -517,23 +517,9 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP @@ -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 @@ -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, @@ -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, @@ -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;

32
libraries/AP_Mission/AP_Mission.h
Unescape View File

@ -43,21 +43,6 @@ @@ -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: @@ -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: @@ -167,6 +154,9 @@ public:
// do-gripper
Gripper_Command gripper;
// do-guided-limits
Guided_Limits_Command guided_limits;
// location
Location location; // Waypoint location

Write Preview
Loading…
Cancel
Save