Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

feat: use MAV_CMD_GUIDED_CHANGE_HEADING

Browse Source
This commit is contained in:
mahima-yoga
2026-05-18 17:26:32 +02:00
parent bc459a8814
commit 99a85865d6
4 changed files with 12 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/versioned/VehicleCommand.msg
+1 -1
View File
@@ -128,7 +128,7 @@ uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # Start of PX4 internal only vehic
uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Unused|Unused|Unused|Unused|Latitude (WGS-84)|Longitude (WGS-84)|[m] Altitude (AMSL from GNSS, positive above ground)|
uint32 VEHICLE_CMD_SET_NAV_STATE = 100001 # Change mode by specifying nav_state directly. |nav_state|Unused|Unused|Unused|Unused|Unused|Unused|
uint16 VEHICLE_CMD_DO_CHANGE_COURSE = 43007 # Change the course setpoint in Course mode. |[deg] Course (bearing) [0..360, NaN = no change]|Unused|Unused|Unused|Unused|Unused|Unused|
uint16 VEHICLE_CMD_GUIDED_CHANGE_HEADING = 43002 # Change heading/course. param1: heading type (0=course-over-ground, 1=heading). param2: target [deg]. param3: max rate [deg/s]. |Heading type (HEADING_TYPE enum)|[deg] Target bearing [0..360]|[deg/s] Max rate of change|Unused|Unused|Unused|Unused|
uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization.
uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory.
src/modules/commander/Commander.cpp
+1 -1
View File
@@ -910,7 +910,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
break;
case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_COURSE: {
case vehicle_command_s::VEHICLE_CMD_GUIDED_CHANGE_HEADING: {
// Navigator handles this command: it acks ACCEPTED when
// the vehicle is in course mode with a valid position, DENIED otherwise.
}
src/modules/navigator/course.h
+2 -2
View File
@@ -35,8 +35,8 @@
*
* Course mode: maintain constant course, altitude, and airspeed.
*
* Accepts MAV_CMD_DO_CHANGE_ALTITUDE, VEHICLE_CMD_DO_REPOSITION, DO_CHANGE_COURSE and DO_CHANGE_SPEED commands.
* - DO_CHANGE_COURSE: sets course (ground track direction, requires valid position).
* Accepts MAV_CMD_DO_CHANGE_ALTITUDE, MAV_CMD_GUIDED_CHANGE_HEADING and MAV_CMD_DO_CHANGE_SPEED commands.
* - MAV_CMD_GUIDED_CHANGE_HEADING (param1=HEADING_TYPE_COURSE_OVER_GROUND): sets course (ground track direction, requires valid position).
*/
#pragma once
src/modules/navigator/navigator_main.cpp
+8 -5
View File
@@ -525,21 +525,24 @@ void Navigator::run()
// DO_CHANGE_ALTITUDE is acknowledged by commander
} // else (not course hold)
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_COURSE
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_GUIDED_CHANGE_HEADING
&& _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED};
// param1: heading type (0 = HEADING_TYPE_COURSE_OVER_GROUND)
// param2: target bearing [deg, 0=north]
const bool control_course = (lroundf(cmd.param1) == 0);
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
&& _navigation_mode == &_course && PX4_ISFINITE(cmd.param1)) {
// param1: course angle [deg], 0 = north, converted to radians
float course_rad = cmd.param1 * M_DEG_TO_RAD_F;
&& _navigation_mode == &_course && control_course && PX4_ISFINITE(cmd.param2)) {
float course_rad = cmd.param2 * M_DEG_TO_RAD_F;
if (_course.set_course(course_rad)) {
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
// DENIED if no positioning available for course mode, or if we are not in fixed wing mode
// DENIED if not FW, not in course mode, not correct heading type, or no positioning available
}
publish_vehicle_command_ack(cmd, result);