fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 19:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander/mavlink: use home attitude, not only yaw

Browse Source 
According to the mavlink spec we should be publishing the home attitude
as a quaternion rather than just the yaw/heading.

Additionally, this allows setting the landing roll and pitch angle using
DO_SET_HOME (this yet needs to go into the MAVLink spec though).

This time including the message versioning and translation.
This commit is contained in:
Julian Oes
2022-05-25 20:07:45 +12:00
parent 2065f577d6
commit 2c97a875bf
8 changed files with 124 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+5 -1
View File
@@ -999,6 +999,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
} else {
float roll = matrix::wrap_2pi(math::radians(cmd.param2));
roll = PX4_ISFINITE(roll) ? roll : 0.0f;
float pitch = matrix::wrap_2pi(math::radians(cmd.param3));
pitch = PX4_ISFINITE(pitch) ? pitch : 0.0f;
float yaw = matrix::wrap_2pi(math::radians(cmd.param4));
yaw = PX4_ISFINITE(yaw) ? yaw : (float)NAN;
const double lat = cmd.param5;
@@ -1007,7 +1011,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
if (_home_position.setManually(lat, lon, alt, yaw)) {
if (_home_position.setManually(lat, lon, alt, roll, pitch, yaw)) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;