fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 03:00:34 +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;
src/modules/commander/HomePosition.cpp
+19 -8
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2025 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,7 +99,9 @@ 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
fillLocalHomePos(home, lpos);
const vehicle_attitude_s &attitude = _attitude_sub.get();
fillLocalHomePos(home, lpos, attitude);
updated = true;
}
@@ -135,19 +137,25 @@ bool HomePosition::setHomePosition(bool force)
return updated;
}
void HomePosition::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos)
void HomePosition::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos,
const vehicle_attitude_s &attitude)
{
fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, lpos.heading);
matrix::Quatf q(attitude.q);
matrix::Eulerf euler(q);
fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, euler(0), euler(1), euler(2));
}
void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading)
void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, float z, float roll, float pitch,
float yaw)
{
home.x = x;
home.y = y;
home.z = z;
home.valid_lpos = true;
home.yaw = heading;
home.roll = roll;
home.pitch = pitch;
home.yaw = yaw;
}
void HomePosition::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos)
@@ -229,7 +237,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);
fillLocalHomePos(home, home_x, home_y, home_z, NAN, NAN, NAN);
home.timestamp = hrt_absolute_time();
home.update_count++;
@@ -245,7 +253,7 @@ void HomePosition::setInAirHomePosition()
}
}
bool HomePosition::setManually(double lat, double lon, float alt, float yaw)
bool HomePosition::setManually(double lat, double lon, float alt, float roll, float pitch, float yaw)
{
const vehicle_local_position_s &vehicle_local_position = _local_position_sub.get();
@@ -268,6 +276,8 @@ bool HomePosition::setManually(double lat, double lon, float alt, float yaw)
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();
@@ -303,6 +313,7 @@ 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;
src/modules/commander/HomePosition.hpp
+7 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2025 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,6 +39,7 @@
#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>
@@ -64,7 +65,7 @@ public:
bool setHomePosition(bool force = false);
void setInAirHomePosition();
bool setManually(double lat, double lon, float alt, float yaw);
bool setManually(double lat, double lon, float alt, float roll, float pitch, float yaw);
void setTakeoffTime(uint64_t takeoff_time) { _takeoff_time = takeoff_time; }
void update(bool set_automatically, bool check_if_changed);
@@ -76,8 +77,9 @@ private:
void setHomePosValid();
void updateHomePositionYaw(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 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 fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos);
static void fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt);
@@ -85,6 +87,7 @@ 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};
src/modules/mavlink/streams/HOME_POSITION.hpp
+1 -1
View File
@@ -75,7 +75,7 @@ private:
msg.y = home.y;
msg.z = home.z;
matrix::Quatf q(matrix::Eulerf(0.f, 0.f, home.yaw));
matrix::Quatf q(matrix::Eulerf(home.roll, home.pitch, home.yaw));
q.copyTo(msg.q);
msg.approach_x = 0.f;