PX4
PX4 Plane is not floowing offboard commands
Describe the bug
I am trying to simulate basic arm, takeoff and draw a circle with PX4 plane. It arms and takeoff properly. However, when takeoff is completed, it changes the mode to hold and ignores my offboard commands. My code is really basic. I just did some adjustment to existed one in https://github.com/PX4/px4_ros_com. I want the plane to follow given points. I publish my messages 30hz.
To Reproduce
/**
* @file offboard_plane.cpp
* @brief Simple offboard control example for PX4 autopilot plane
* @version 0.1
* @date 2023-11-01
*
* @copyright Copyright (c) 2023
*
*/
#include <cmath>
#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp>
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <px4_msgs/msg/vehicle_status.hpp>
#include <rclcpp/rclcpp.hpp>
#include <stdexcept>
#include <stdint.h>
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;
class OffboardPlane : public rclcpp::Node {
public:
OffboardPlane(std::vector<std::vector<float>> local_waypoints)
: Node("offboard_plane"), local_waypoints_(local_waypoints) {
rclcpp::QoS qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
qos_profile.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST);
qos_profile.keep_last(1);
offboard_control_mode_publisher_ =
this->create_publisher<OffboardControlMode>(
"/fmu/in/offboard_control_mode", 10);
trajectory_setpoint_publisher_ = this->create_publisher<TrajectorySetpoint>(
"/fmu/in/trajectory_setpoint", 10);
vehicle_command_publisher_ =
this->create_publisher<VehicleCommand>("/fmu/in/vehicle_command", 10);
vehicle_status_subcriber =
this->create_subscription<px4_msgs::msg::VehicleStatus>(
"/fmu/out/vehicle_status", qos_profile,
std::bind(&OffboardPlane::vehicle_status_callback, this,
std::placeholders::_1));
vehicle_odometry_subscriber =
this->create_subscription<px4_msgs::msg::VehicleOdometry>(
"/fmu/out/vehicle_odometry", qos_profile,
std::bind(&OffboardPlane::vehicle_odometry_callback, this,
std::placeholders::_1));
std::thread t1(&OffboardPlane::run, this);
t1.detach();
}
void arm();
void disarm();
void takeoff();
void land();
void run();
private:
std::vector<std::vector<float>> local_waypoints_;
int waypoint_index_ = 0;
px4_msgs::msg::VehicleStatus vehicle_status_;
px4_msgs::msg::VehicleOdometry vehicle_odometry_;
rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr
offboard_control_mode_publisher_;
rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr
trajectory_setpoint_publisher_;
rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr
vehicle_command_publisher_;
rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr
vehicle_status_subcriber;
rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr
vehicle_odometry_subscriber;
void publish_offboard_control_mode();
void publish_trajectory_setpoint();
void publish_vehicle_command(uint16_t command, float param1 = 0.0,
float param2 = 0.0);
void
vehicle_status_callback(const px4_msgs::msg::VehicleStatus::SharedPtr msg);
void vehicle_odometry_callback(
const px4_msgs::msg::VehicleOdometry::SharedPtr msg);
};
void OffboardPlane::run() {
// wait for FCU connection
// wait for vehicle to be armed
// send offboard control commands in a loop
int i = 0;
while (rclcpp::ok()) {
if (i == 10) {
this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1,
6);
arm();
takeoff();
}
if (i < 11) {
i++;
}
// if new setpoint is reached, then move to next setpoint
// calculate euclidean distance to next setpoint
float dx =
local_waypoints_[waypoint_index_][0] - vehicle_odometry_.position[0];
float dy =
local_waypoints_[waypoint_index_][1] - vehicle_odometry_.position[1];
float dz =
local_waypoints_[waypoint_index_][2] - vehicle_odometry_.position[2];
float dist_to_setpoint = sqrt(dx * dx + dy * dy + dz * dz);
RCLCPP_INFO(this->get_logger(), "Distance to setpoint: %f , %d",
dist_to_setpoint, waypoint_index_);
// move to next setpoint if current setpoint is reached
if (dist_to_setpoint < 1.0) {
if (waypoint_index_ + 1 == local_waypoints_.size()) {
waypoint_index_ = 0;
} else {
waypoint_index_++;
}
}
publish_offboard_control_mode();
publish_trajectory_setpoint();
std::this_thread::sleep_for(std::chrono::milliseconds(1000 / 30));
}
}
void OffboardPlane::land() {
publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_NAV_LAND,
0.0, 0.0);
RCLCPP_INFO(this->get_logger(), "Land command send");
}
void OffboardPlane::vehicle_status_callback(
const px4_msgs::msg::VehicleStatus::SharedPtr msg) {
vehicle_status_ = *msg;
}
void OffboardPlane::vehicle_odometry_callback(
const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
vehicle_odometry_ = *msg;
}
void OffboardPlane::arm() {
publish_vehicle_command(
px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
RCLCPP_INFO(this->get_logger(), "Arm command send");
}
void OffboardPlane::disarm() {
publish_vehicle_command(
px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);
RCLCPP_INFO(this->get_logger(), "Disarm command send");
}
void OffboardPlane::takeoff() {
publish_vehicle_command(
px4_msgs::msg::VehicleCommand::VEHICLE_CMD_NAV_TAKEOFF, 0.0, 0.0);
RCLCPP_INFO(this->get_logger(), "Takeoff command send");
}
void OffboardPlane::publish_offboard_control_mode() {
px4_msgs::msg::OffboardControlMode msg{};
msg.position = true;
msg.velocity = false;
msg.acceleration = false;
msg.attitude = false;
msg.body_rate = false;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
offboard_control_mode_publisher_->publish(msg);
}
void OffboardPlane::publish_trajectory_setpoint() {
px4_msgs::msg::TrajectorySetpoint msg{};
msg.position = {local_waypoints_[waypoint_index_][0],
local_waypoints_[waypoint_index_][1],
local_waypoints_[waypoint_index_][2]};
msg.yaw = 0; // [-PI:PI]
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
trajectory_setpoint_publisher_->publish(msg);
}
void OffboardPlane::publish_vehicle_command(uint16_t command, float param1,
float param2) {
px4_msgs::msg::VehicleCommand msg{};
msg.param1 = param1;
msg.param2 = param2;
msg.command = command;
msg.target_system = 1;
msg.target_component = 1;
msg.source_system = 1;
msg.source_component = 1;
msg.from_external = true;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
vehicle_command_publisher_->publish(msg);
}
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
// Local waypoints 4 points of a circle with radius 20m and center at (0,0)
// at 20m altitude
std::vector<std::vector<float>> local_waypoints = {
{20, 0, 20}, {0, 20, 20}, {-20, 0, 20}, {0, -20, 20}};
auto node = std::make_shared<OffboardPlane>(local_waypoints);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
Expected behavior
Fly and draw circle
Screenshot / Media
No response
Flight Log
sim
Software Version
sim
Flight controller
sim
Vehicle type
None
How are the different components wired up (including port information)
No response
Additional context
No response
However, when takeoff is completed, it changes the mode to hold and ignores my offboard commands.
Did you switch to offboard mode?
void OffboardPlane::publish_offboard_control_mode() {
px4_msgs::msg::OffboardControlMode msg{};
msg.position = true;
msg.velocity = false;
msg.acceleration = false;
msg.attitude = false;
msg.body_rate = false;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
offboard_control_mode_publisher_->publish(msg);
}
void OffboardPlane::publish_trajectory_setpoint() {
px4_msgs::msg::TrajectorySetpoint msg{};
msg.position = {local_waypoints_[waypoint_index_][0],
local_waypoints_[waypoint_index_][1],
local_waypoints_[waypoint_index_][2]};
msg.yaw = 0; // [-PI:PI]
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
trajectory_setpoint_publisher_->publish(msg);
}
Here is the methods and I am calling them here in void OffboardPlane::run() thread with 30hz. Is there another option to switch offboard mode?
@bilalkah That is just sending the offboard mode topics and does not necessarily switch the vehicle into offboard mode. You need to switch the vehicle flight mode into "OFFBOARD" mode
Hello @Jaeyoung-Lim , I applied these changes and now I can change flight mode to OFFBOARD.
// if takeoff is completed, then start mission
if (i > 10) {
if (this->get_clock()->now() - takeoff_time > 10s && !takeoff_completed) {
takeoff_completed = true;
this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE,
1, 6);
}
}
However, Plane does not follow my setpoints. Do you any suggestion?
@bilalkah Do you have a log of the vehicle not following the setpoints?
publish_offboard_control_mode();
publish_trajectory_setpoint();
I am periodically publishing the setpoint in running thread here this is the output. I use the gazebo simulation and run the plane as following command. "make px4_sitl gz_rc_cessna"
As you can see my first target is {20, 0, 20}. However, I converted to {20, 0, -20} because of NED transformation.
It will be very helpful if you can tell me what options I have for controlling a fixed-wing plane via ros2. The documentation is not enough for me. I want to give some target point to fw to fly.
@bilalkah You would need to check why the trajectory_setpoint is not getting properly consumed: https://github.com/PX4/PX4-Autopilot/blob/d6dbf38a1ba376cde9e65c10430e6feb52f6fdd5/src/modules/fw_pos_control/FixedwingPositionControl.cpp#L2304-L2360
Hello @Jaeyoung-Lim
Here is my final code
#include <cmath>
#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp>
#include <px4_msgs/msg/vehicle_local_position_setpoint.hpp>
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <px4_msgs/msg/vehicle_status.hpp>
#include <px4_msgs/msg/vehicle_trajectory_waypoint.hpp>
#include <rclcpp/rclcpp.hpp>
#include <stdexcept>
#include <stdint.h>
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;
class OffboardPlane : public rclcpp::Node {
public:
OffboardPlane(std::vector<std::vector<float>> local_waypoints)
: Node("offboard_plane"), local_waypoints_(local_waypoints) {
rclcpp::QoS qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
qos_profile.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST);
qos_profile.keep_last(1);
offboard_control_mode_publisher_ =
this->create_publisher<OffboardControlMode>(
"/fmu/in/offboard_control_mode", 10);
trajectory_setpoint_publisher_ = this->create_publisher<TrajectorySetpoint>(
"/fmu/in/trajectory_setpoint", 10);
vehicle_trajectory_waypoint_publisher_ =
this->create_publisher<VehicleTrajectoryWaypoint>(
"/fmu/in/vehicle_trajectory_waypoint", 10);
vehicle_command_publisher_ =
this->create_publisher<VehicleCommand>("/fmu/in/vehicle_command", 10);
vehicle_local_position_setpoint_publisher_ =
this->create_publisher<VehicleLocalPositionSetpoint>(
"/fmu/in/vehicle_local_position_setpoint", 10);
vehicle_status_subcriber =
this->create_subscription<px4_msgs::msg::VehicleStatus>(
"/fmu/out/vehicle_status", qos_profile,
std::bind(&OffboardPlane::vehicle_status_callback, this,
std::placeholders::_1));
vehicle_odometry_subscriber =
this->create_subscription<px4_msgs::msg::VehicleOdometry>(
"/fmu/out/vehicle_odometry", qos_profile,
std::bind(&OffboardPlane::vehicle_odometry_callback, this,
std::placeholders::_1));
std::thread t1(&OffboardPlane::run, this);
t1.detach();
}
void arm();
void disarm();
void takeoff();
void land();
void run();
private:
std::vector<std::vector<float>> local_waypoints_;
int waypoint_index_ = 0;
px4_msgs::msg::VehicleStatus vehicle_status_;
px4_msgs::msg::VehicleOdometry vehicle_odometry_;
rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr
offboard_control_mode_publisher_;
rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr
trajectory_setpoint_publisher_;
rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr
vehicle_command_publisher_;
rclcpp::Publisher<px4_msgs::msg::VehicleTrajectoryWaypoint>::SharedPtr
vehicle_trajectory_waypoint_publisher_;
rclcpp::Publisher<px4_msgs::msg::VehicleLocalPositionSetpoint>::SharedPtr
vehicle_local_position_setpoint_publisher_;
rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr
vehicle_status_subcriber;
rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr
vehicle_odometry_subscriber;
void publish_offboard_control_mode();
void publish_trajectory_setpoint();
void publish_vehicle_command(uint16_t command, float param1 = 0.0,
float param2 = 0.0);
void
vehicle_status_callback(const px4_msgs::msg::VehicleStatus::SharedPtr msg);
void vehicle_odometry_callback(
const px4_msgs::msg::VehicleOdometry::SharedPtr msg);
};
void OffboardPlane::run() {
// wait for FCU connection
// wait for vehicle to be armed
// send offboard control commands in a loop
int i = 0;
auto takeoff_time = this->get_clock()->now();
bool takeoff_completed = false;
// Set home position to current gps position
this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_HOME, 0.0,
0.0);
this->publish_vehicle_command(
VehicleCommand::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN, 0.0, 0.0);
while (rclcpp::ok()) {
if (i == 10) {
this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1,
6);
arm();
takeoff();
takeoff_time = this->get_clock()->now();
}
if (i < 11) {
i++;
}
// if takeoff is completed, then start mission
if (i > 10) {
if (this->get_clock()->now() - takeoff_time > 10s && !takeoff_completed) {
takeoff_completed = true;
this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE,
1, 6);
// publish_trajectory_setpoint();
}
}
// calculate distance to next waypoint and switch to next if close enough
float dx =
local_waypoints_[waypoint_index_][0] - vehicle_odometry_.position[0];
float dy =
local_waypoints_[waypoint_index_][1] - vehicle_odometry_.position[1];
float dz =
local_waypoints_[waypoint_index_][2] - vehicle_odometry_.position[2];
float distance_to_waypoint = sqrt(dx * dx + dy * dy + dz * dz);
if (distance_to_waypoint < 5.0) {
if (waypoint_index_ + 1 == local_waypoints_.size()) {
waypoint_index_ = 0;
} else {
waypoint_index_++;
}
}
RCLCPP_INFO(this->get_logger(), "Distance to waypoint: %f, setpoint: %d",
distance_to_waypoint, waypoint_index_);
// move to next setpoint if current setpoint is reached
publish_offboard_control_mode();
publish_trajectory_setpoint();
std::this_thread::sleep_for(std::chrono::milliseconds(1000 / 10));
}
}
void OffboardPlane::land() {
publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_NAV_LAND,
0.0, 0.0);
RCLCPP_INFO(this->get_logger(), "Land command send");
}
void OffboardPlane::vehicle_status_callback(
const px4_msgs::msg::VehicleStatus::SharedPtr msg) {
vehicle_status_ = *msg;
}
void OffboardPlane::vehicle_odometry_callback(
const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
vehicle_odometry_ = *msg;
}
void OffboardPlane::arm() {
publish_vehicle_command(
px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
RCLCPP_INFO(this->get_logger(), "Arm command send");
}
void OffboardPlane::disarm() {
publish_vehicle_command(
px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);
RCLCPP_INFO(this->get_logger(), "Disarm command send");
}
void OffboardPlane::takeoff() {
publish_vehicle_command(
px4_msgs::msg::VehicleCommand::VEHICLE_CMD_NAV_TAKEOFF, 0.0, 0.0);
RCLCPP_INFO(this->get_logger(), "Takeoff command send");
}
void OffboardPlane::publish_offboard_control_mode() {
px4_msgs::msg::OffboardControlMode msg{};
msg.position = true;
msg.velocity = false;
msg.acceleration = false;
msg.attitude = false;
msg.body_rate = false;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
offboard_control_mode_publisher_->publish(msg);
}
void OffboardPlane::publish_trajectory_setpoint() {
px4_msgs::msg::TrajectorySetpoint msg{};
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
msg.position[0] = local_waypoints_[waypoint_index_][0];
msg.position[1] = local_waypoints_[waypoint_index_][1];
msg.position[2] = local_waypoints_[waypoint_index_][2];
// pub
trajectory_setpoint_publisher_->publish(msg);
}
void OffboardPlane::publish_vehicle_command(uint16_t command, float param1,
float param2) {
px4_msgs::msg::VehicleCommand msg{};
msg.param1 = param1;
msg.param2 = param2;
msg.command = command;
msg.target_system = 1;
msg.target_component = 1;
msg.source_system = 1;
msg.source_component = 1;
msg.from_external = true;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
vehicle_command_publisher_->publish(msg);
}
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
// Local waypoints 4 points of a circle with radius 20m and center at (0,0)
// at 20m altitude
std::vector<std::vector<float>> local_waypoints = {{20.0, 0.0, -20.0},
{0.0, 20.0, -20.0},
{-20.0, 0.0, -20.0},
{0.0, -20.0, -20.0}};
auto node = std::make_shared<OffboardPlane>(local_waypoints);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
Additionally, I add some cout for debugging purposes here in below
_pos_sp_triplet.current.alt = NAN;
std::cout << "trajectory_setpoint received" << std::endl;
if (Vector3f(trajectory_setpoint.position).isAllFinite()) {
if (_global_local_proj_ref.isInitialized()) {
std::cout << "trajectory_setpoint position is finite" << std::endl;
double lat;
double lon;
and I am getting output as below from px4 sim
trajectory_setpoint position is finite
trajectory_setpoint received
trajectory_setpoint position is finite
trajectory_setpoint received
trajectory_setpoint position is finite
trajectory_setpoint received
trajectory_setpoint position is finite
trajectory_setpoint received
trajectory_setpoint position is finite
So, I believe we can say that vehicle is getting my setpoints. However, this is the behavior of fixed-wing
Here is the odometry position message output as well.
position:
- -137.63931274414062
- 164.79827880859375
- -0.07325202226638794
I did it once with ros1 and mavros. It was working properly. What is wrong here?
@bilalkah How did you solve the problem? I'm having exactly the same problem with you.
@yongyong3393 Please describe your problem.
The interface works, as it is designed for but you need to understand what different components are involved
@Jaeyoung-Lim Thank you for your comment! I'm trying to control a Fixed-wing vehicle(actually, VTOL) in offboard mode(ros2) with waypoint lists, using TrajectorySetpoint(uORB) message. My goal is to make vtol behave as it is in mission mode, but with offboard waypoint publishing(make waypoints 'flexible').
There were two problems, but I think I've already solved one of them.
The first problem was similar to the original issue above. In offboard mode(using ros2), I couldn't control the vehicle with TrajectorySetpoint(uORB message). But I found out that I need to send velocity setpoint as well as position setpoint when I publishing TrajectorySetpoint message! If I pass velocity setpoint as 'nan', then navigatePathTangent() will not work!
(in FixedwingPositionControl.cpp, in function control_auto_path(), in function navigatePathTangent())
(https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/fw_pos_control/FixedwingPositionControl.cpp#L3073)
So I tried sending both position and velocity setpoint together, and checked the vehicle travels well in xy plane. (
However, the second problem, which I haven't solved yet, is that the altitude goes constantly higher and higher at rate about 0.4 m/s, even if I send valid TrajectorySetpoint messages. Specifically, in my code(https://github.com/yongyong3393/vehicle_controller/blob/main/vehicle_controller/vehicle_controller.py), WP1, WP2, WP3 have same altitude(20 m), so if control_auto_path() and tecs_update_pitch_throttle() (https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/fw_pos_control/FixedwingPositionControl.cpp#L1398) works properly, it must fly level. I haven't found the reason and solution for this issue.
@Jaeyoung-Lim logfile Can I share it like this way? It is my first time to share log files to developers, so I don't know if it is enough to you. If you need more information or simulation data, please tell me!
I guess that the altitude increases in offboard mode because the tecs rate is set to -2.5, for unknown reasons.
@yongyong3393 Are you sure you are streaming trajectory_setpoints while in offboard mode? The setpoints seem pretty sparse. (There is only one setpoint message at 75.6 seconds during offfboard mode)
@Jaeyoung-Lim It is true that trajectory_setpoint is not streamed continuously, but that does not seem to be the cause.
First, when I analyzed FixedwingPositionControl.cpp, I thought that trajectory_setpoint does not need to be continuously sent. updating trajectory setpoint Even if the topic is not updated by ros2, pos_sp will automatically set to most recently received trajectory_setpoint data. I thought if I publish trajectory_setpoint once (at 75.6 s), the vehicle can fly toward the setpoint without additional publishing, and I also checked that the vehicle flies well in xy direction in above situation.
By the way, I also tried modifying the ros2 source code to stream trajectory_setpoint continuously, but the same problem (altitude rises constantly) still remains. A changed version's flight log is also attached below.
flight log
Thank you for your reply!
@yongyong3393 That may be true, but the behavior without continuous setpoint streaming is undefined.
@Jaeyoung-Lim Oh, then it would be better to avoid discontinuous trajectory_setpoint publishing. Thank you! But I'm still wondering why altitude goes high even if I changed the code to continuously stream trajectory_setpoint...
@yongyong3393 Could you maybe change the magnitude of velocity to 1 and populate the acceleration?
This might be a real bug
@yongyong3393 Actually, I think this might be a regression on the TECS implementation side. Might be worth checking
@Jaeyoung-Lim I'm sorry for the late reply.
I tried normalizing velocity, and setting acceleration to nonzero value when I was publishing trajectory_setpoint, as you suggested, but it didn't work either.
@Jaeyoung-Lim Then should I report this to TECS implementation team, or did you already tell them? If I need to, how can I do it?
@yongyong3393 You can post an issue or you can also try to fix it yourself.
@Jaeyoung-Lim Thank you so much. I'll first try to examine the code myself, and post the issue again soon.
@Jaeyoung-Lim Hi, it's been a while. I found the reason and fixed it by modifying px4 source code. Could you check the new issue #23040 and pull request #23041 ?