fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 21:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_command: add initial frame support (GLOBAL & GLOBAL_RELATIVE_ALT)

Browse Source
This commit is contained in:
Daniel Agar
2024-05-01 18:13:46 -04:00
committed by Matthias Grob
parent c13e3bae12
commit ed35155b50
5 changed files with 45 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/VehicleCommand.msg
+6 -1
View File
@@ -165,7 +165,10 @@ int8 ARMING_ACTION_ARM = 1
uint8 GRIPPER_ACTION_RELEASE = 0
uint8 GRIPPER_ACTION_GRAB = 1
uint8 ORB_QUEUE_LENGTH = 8
uint8 frame # The coordinate system of the COMMAND.
uint8 FRAME_UNKNOWN = 0 # Not specified.
uint8 FRAME_GLOBAL = 1 # Global (WGS84) coordinate frame + MSL altitude.
uint8 FRAME_GLOBAL_RELATIVE_ALT = 2 # Global (WGS84) coordinate frame + altitude relative to the home position.
float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.
float32 param2 # Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum.
@@ -184,4 +187,6 @@ bool from_external
uint16 COMPONENT_MODE_EXECUTOR_START = 1000
uint8 ORB_QUEUE_LENGTH = 8
# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor
src/modules/mavlink/mavlink_receiver.cpp
+18
View File
@@ -469,6 +469,24 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
switch (cmd_mavlink.frame) {
case MAV_FRAME_GLOBAL:
// FALLTHROUGH
case MAV_FRAME_GLOBAL_INT:
vcmd.frame = vehicle_command_s::FRAME_GLOBAL;
break;
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
vcmd.frame = vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALT;
break;
default:
vcmd.frame = vehicle_command_s::FRAME_UNKNOWN;
PX4_WARN("invalid frame %" PRIu8 " for command %" PRIu16, cmd_mavlink.frame, cmd_mavlink.command);
break;
}
if (cmd_mavlink.x == INT32_MAX && cmd_mavlink.y == INT32_MAX) {
// INT32_MAX for x and y means to ignore it.
vcmd.param5 = (double)NAN;
src/modules/navigator/land.cpp
+1
View File
@@ -108,6 +108,7 @@ Land::on_active()
vehicle_command_s vcmd = {};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
vcmd.frame = vehicle_command_s::FRAME_GLOBAL;
vcmd.param1 = -1;
vcmd.param2 = 1;
vcmd.param5 = _navigator->get_global_position()->lat;
src/modules/navigator/mission_base.cpp
+1
View File
@@ -855,6 +855,7 @@ MissionBase::do_abort_landing()
vehicle_command_s vcmd = {};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
vcmd.frame = vehicle_command_s::FRAME_GLOBAL;
vcmd.param1 = -1;
vcmd.param2 = 1;
vcmd.param5 = _mission_item.lat;
src/modules/navigator/navigator_main.cpp
+19 -1
View File
@@ -267,6 +267,7 @@ void Navigator::run()
vehicle_global_position_s position_setpoint{};
// latitude/longitude
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
position_setpoint.lat = cmd.param5;
position_setpoint.lon = cmd.param6;
@@ -276,7 +277,24 @@ void Navigator::run()
position_setpoint.lon = get_global_position()->lon;
}
position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
// altitude
if (PX4_ISFINITE(cmd.param7)) {
switch (cmd.frame) {
default:
// FALLTHROUGH
case vehicle_command_s::FRAME_GLOBAL:
position_setpoint.alt = cmd.param7;
break;
case vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALT:
position_setpoint.alt = cmd.param7 + get_home_position()->alt;
break;
}
} else {
position_setpoint.alt = get_global_position()->alt;
}
if (geofence_allows_position(position_setpoint)) {
position_setpoint_triplet_s *rep = get_reposition_triplet();