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

Revert "commander: publish full home attitude, not only yaw (#19717)"

Browse Source 
This reverts commit 6855aa57c4879bb5c67298f3ce1f738118fbafa0.
This commit is contained in:
Silvan Fuhrer 2025-07-14 16:07:27 +02:00
parent a3f1fb2e01
commit 35a3f519f2
5 changed files with 14 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/versioned/HomePosition.msg
View File

@ -12,8 +12,6 @@ float32 x # X coordinate in meters
float32 y # Y coordinate in meters
float32 z # Z coordinate in meters
float32 roll # Pitch angle in radians
float32 pitch # Roll angle in radians
float32 yaw # Yaw angle in radians
bool valid_alt # true when the altitude has been set

6
src/modules/commander/Commander.cpp
View File

@ -999,10 +999,6 @@ 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;
@ -1011,7 +1007,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, roll, pitch, yaw)) {
if (_home_position.setManually(lat, lon, alt, yaw)) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;

27
src/modules/commander/HomePosition.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022-2025 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -99,9 +99,7 @@ bool HomePosition::setHomePosition(bool force)
const vehicle_local_position_s &lpos = _local_position_sub.get();
_heading_reset_counter = lpos.heading_reset_counter; // TODO: should not be here
const vehicle_attitude_s &attitude = _attitude_sub.get();
fillLocalHomePos(home, lpos, attitude);
fillLocalHomePos(home, lpos);
updated = true;
}
@ -137,25 +135,19 @@ bool HomePosition::setHomePosition(bool force)
return updated;
}
void HomePosition::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos,
const vehicle_attitude_s &attitude)
void HomePosition::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos)
{
matrix::Quatf q(attitude.q);
matrix::Eulerf euler(q);
fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, euler(0), euler(1), euler(2));
fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, lpos.heading);
}
void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, float z, float roll, float pitch,
float yaw)
void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading)
{
home.x = x;
home.y = y;
home.z = z;
home.valid_lpos = true;
home.roll = roll;
home.pitch = pitch;
home.yaw = yaw;
home.yaw = heading;
}
void HomePosition::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos)
@ -237,7 +229,7 @@ void HomePosition::setInAirHomePosition()
ref_pos.project(home.lat, home.lon, home_x, home_y);
const float home_z = -(home.alt - lpos.ref_alt);
fillLocalHomePos(home, home_x, home_y, home_z, NAN, NAN, NAN);
fillLocalHomePos(home, home_x, home_y, home_z, NAN);
home.timestamp = hrt_absolute_time();
home.update_count++;
@ -253,7 +245,7 @@ void HomePosition::setInAirHomePosition()
}
}
bool HomePosition::setManually(double lat, double lon, float alt, float roll, float pitch, float yaw)
bool HomePosition::setManually(double lat, double lon, float alt, float yaw)
{
const vehicle_local_position_s &vehicle_local_position = _local_position_sub.get();
@ -276,8 +268,6 @@ bool HomePosition::setManually(double lat, double lon, float alt, float roll, fl
home.z = -(alt - vehicle_local_position.ref_alt);
home.valid_lpos = vehicle_local_position.xy_valid && vehicle_local_position.z_valid;
home.roll = roll;
home.pitch = pitch;
home.yaw = yaw;
home.timestamp = hrt_absolute_time();
@ -313,7 +303,6 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
{
_local_position_sub.update();
_global_position_sub.update();
_attitude_sub.update();
if (_vehicle_air_data_sub.updated()) {
vehicle_air_data_s baro_data;

11
src/modules/commander/HomePosition.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022-2025 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -39,7 +39,6 @@
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/failsafe_flags.h>
#include <uORB/topics/vehicle_air_data.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
@ -65,7 +64,7 @@ public:
bool setHomePosition(bool force = false);
void setInAirHomePosition();
bool setManually(double lat, double lon, float alt, float roll, float pitch, float yaw);
bool setManually(double lat, double lon, float alt, float yaw);
void setTakeoffTime(uint64_t takeoff_time) { _takeoff_time = takeoff_time; }
void update(bool set_automatically, bool check_if_changed);
@ -77,9 +76,8 @@ private:
void setHomePosValid();
void updateHomePositionYaw(float yaw);
static void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos,
const vehicle_attitude_s &attitude);
static void fillLocalHomePos(home_position_s &home, float x, float y, float z, float roll, float pitch, float yaw);
static void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos);
static void fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading);
static void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos);
static void fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt);
@ -87,7 +85,6 @@ private:
uORB::SubscriptionData<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<vehicle_attitude_s> _attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uint64_t _last_gps_timestamp{0};

2
src/modules/mavlink/streams/HOME_POSITION.hpp
View File

@ -75,7 +75,7 @@ private:
msg.y = home.y;
msg.z = home.z;
matrix::Quatf q(matrix::Eulerf(home.roll, home.pitch, home.yaw));
matrix::Quatf q(matrix::Eulerf(0.f, 0.f, home.yaw));
q.copyTo(msg.q);
msg.approach_x = 0.f;