I am using PX4 v1.16 , using it with a visual odometry setup and have a tfmini/distance sensor to estimate height and have configured EXT_HGT_REF to range finder but it gives wildly wrong values like 9m when it’s on the ground , if I do ekf2 stop and ekf2 start it resets to zero but drifts when taking off making the drone shoot straight up , I have set EKF2_EV_CTRL to 9 (no vertical position aid) , the distance sensor gives correct values when viewed in Mavlink inspector , so maybe the imu is making the EKF drift so much ?
Hi @Himanshu069,
Quick clarification: you mention EXT_HGT_REF set to rangefinder. Did you mean EKF2_HGT_REF? That’s the parameter that controls the height reference source (0=baro, 1=GPS, 2=range sensor, 3=vision). If you’re relying on the rangefinder for height, it should be set to 2.
Also, with EKF2_EV_CTRL set to 9 (horizontal position + yaw, no vertical position), make sure EKF2_RNG_CTRL is configured as well if the rangefinder is meant to be your height source.
Beyond that, it’s hard to say more without seeing the data. Could you share a flight log? Upload at logs.px4.io and post the link here, and we can take a look at what’s going on with the height estimation.
1 Like
I changed some params which solved the drone shooting straight up issue, my odom from visual odometry is being published at around 12hz , EKF2_EV_CTRL is 9 and EKF2_HGT_REF is set to range sensor(2). Now, the z setpoint seems to be increasing randomly which I have given to be 0.8m from offboard.
Hi @Himanshu069, thanks for the log.
The core issue is your range sensor data. The TFmini driver expects data at 100 Hz (docs), but looking at the EKF internals, range height was only fused 22 times in 16.5 seconds, with many observations reporting 0.000m (invalid). The signal quality is also frequently 0 or -1. Your SENS_TFMINI_CFG is set to TELEM2 (102) which looks correct, so the issue is with the sensor data itself.
This is causing EKF2 to constantly toggle the range height source on and off. In your 16-second flight I can see it switching at least 6 times:
t= 0.0s range: ON
t= 2.1s range: OFF
t= 3.1s range: OFF, fake_hgt: ON (fallback)
t= 5.0s range: ON
t= 7.2s range: OFF
t=11.8s range: OFF
t=12.0s range: OFF (z drifts from 0 to 3.2m here)
t=16.3s range: ON (z snaps back)
Every time the range drops out, EKF2 has no height reference (you have baro and GPS height disabled) and z drifts on pure IMU integration. That’s your “z setpoint increasing randomly”. It’s actually the z estimate drifting upward during range dropout periods, while the controller chases the 0.8m setpoint relative to a moving reference.
Things to check:
- Wiring first: Check your TX/RX/GND connections to TELEM2. A loose connection or bad solder joint at 115200 baud would cause dropped frames, which would explain the low data rate and frequent invalid readings. Make sure the cable is secure and not too long.
- Sensor health: If wiring looks good, check that the sensor lens is clean and has a clear view of the ground. You can monitor live data with listener distance_sensor in the MAVLink console to see what rate and values you’re actually getting.
- VIO is also intermittent: EV position fusion drops out at t=8s and t=14s. At 12 Hz it’s on the low side. Consider enabling EV vertical position as well (EKF2_EV_CTRL = 11 instead of 9) to give EKF2 a secondary height source during range dropouts.
- Fallback height source: You’ve disabled both baro (EKF2_BARO_CTRL=0) and GPS height (EKF2_GPS_CTRL=0). That means when range drops out, there’s nothing to fall back to. Consider keeping baro enabled as a safety net even if range is your primary reference.
Fix the range sensor data quality first. Everything else follows from not having a stable height measurement.
Thanks for the review @rroche .
I added `
mavlink stream -d /dev/ttyACM0 -s DISTANCE_SENSOR -r 100
to view my tfmini publishing at 100 Hz in my QGC mavlink inspector but internally it should work at 100 Hz regardless i think. Also, I am working on two drones with tfmini in both , and the signal quality is consistently 0 in both, the distance value I get seems correct and reliable , only thing that seems plausible now is hardcoding the quality somewhere in TFmini.cpp or tfmini_parser.cpp if that’s possible?
The signal_quality showing 0 in the MAVLink Inspector is actually expected behavior for TFmini and is not the cause of your EKF2 issues.
Internally, the TFmini driver reports quality as -1 (unknown). PX4’s MAVLink layer converts -1 to 0 for display, since the MAVLink protocol uses 0 to mean “unknown.” EKF2 handles -1 correctly and does not reject readings because of it. So hardcoding quality to 100 won’t solve the problem.
About the MAVLink Inspector vs the flight log:
Your screenshot shows clean, stable distance readings in QGC, which makes sense. The MAVLink Inspector only shows the latest value at whatever rate QGC polls it. It’s essentially a snapshot. If the sensor produces 90 good readings and 10 bad ones per second, the Inspector will almost always show a good one.
The flight log tells a different story because it records every distance_sensor publication. In your log, there are many 0.000m readings mixed in with the valid ones, and only 22 fusions happened in 16.5 seconds. EKF2 correctly rejects those 0.000m readings (they’re below min_distance, so PX4 sets quality to 0 for those specific samples), which causes the height source to drop in and out. That toggling is what makes your z estimate drift.
So the sensor works most of the time, but the intermittent invalid frames are enough to destabilize EKF2’s height fusion. This still points to a hardware/wiring issue:
- Check TX/RX/GND connections for loose or cold solder joints (115200 baud is sensitive to signal integrity)
- Make sure the sensor lens is clean and unobstructed
- Try a different UART port if available
- Run
listener distance_sensoron the MAVLink console for 10-20 seconds and share the output. That will show every reading including the bad ones, unlike the Inspector which hides them.
Both drones showing signal_quality = 0 is normal (all TFmini units report it that way). The question is whether both also show the 0.000m dropouts in the logs.
hmm, it worked previously with the same setup on a single drone , I will try resoldering the wires to test again and update, in the meantime is it possible to maybe change the EKF2 frequency or something to make my distance sensor values acceptable?
Hi @rroche , How did you see the the range ON, OFF data from my log , I can’t access that range related data under the additional data in the same flight log link I provided?
I downloaded the log and parsed with pyulog, you should try it out.
HI, The EKF seems to be consistently rejecting the range finder values . I have already set so many parameters to make it not reject the range values.
I don’t know what can I do at this point?
This is my latest log values.
from pyulog import ULog
import numpy as np
ulog = ULog(‘log_17_2026-4-6-02-06-44.ulg’)
for idx, dataset in enumerate([d for d in ulog.data_list if d.name == ‘estimator_aid_src_rng_hgt’]):
data = dataset.data
ts = data[‘timestamp’] / 1e6 # µs → seconds
obs = data[‘observation’] # raw range finder height (m)
innov = data[‘innovation’] # residual: obs - EKF estimate
ekf_hgt = obs - innov # EKF2 height estimate (m)
test_ratio = data[‘test_ratio’]
rejected = data[‘innovation_rejected’].astype(bool)
fused = data[‘fused’].astype(bool)
print(f"\n=== Instance {idx} ===")
print(f"{'Time(s)':>10} {'RngFinder(m)':>14} {'EKF2 Hgt(m)':>12} {'Innovation(m)':>14} {'TestRatio':>10} {'Rejected':>9} {'Fused':>6}")
print("-" * 80)
for i in range(len(ts)):
print(f"{ts[i]:10.2f} {obs[i]:14.3f} {ekf_hgt[i]:12.3f} {innov[i]:14.3f} "
f"{test_ratio[i]:10.3f} {'YES' if rejected[i] else 'no':>9} {'Y' if fused[i] else 'n':>6}")
Instance 0
Time(s) RngFinder(m) EKF2 Hgt(m) Innovation(m) TestRatio Rejected Fused
195.27 0.010 0.009 0.001 0.000 no Y
195.79 0.010 0.009 0.001 0.000 no Y
196.28 0.010 0.010 -0.000 0.000 no Y
196.78 0.010 0.010 -0.000 0.000 no Y
197.28 0.050 0.089 -0.039 0.000 no n
197.79 0.050 0.087 -0.037 0.000 no n
198.27 0.050 0.079 -0.029 0.000 no n
198.78 0.070 0.068 0.002 0.000 no n
199.27 0.140 0.120 0.020 0.000 no n
199.78 0.170 0.141 0.029 0.000 no n
200.28 0.130 0.060 0.070 0.000 no n
200.77 0.080 -0.030 0.110 0.000 no n
201.29 0.050 -0.099 0.149 0.000 no n
201.78 0.060 -0.103 0.163 0.000 no n
202.27 0.050 -0.151 0.201 0.000 no n
202.78 0.050 -0.162 0.212 0.000 no n
203.28 0.050 -0.172 0.222 0.000 no n
203.79 0.050 -0.181 0.231 0.000 no n
204.28 0.050 -0.166 0.216 0.000 no n
204.79 0.050 -0.165 0.215 0.000 no n
205.28 0.050 -0.163 0.213 0.000 no n
205.79 0.050 -0.152 0.202 0.000 no n
206.27 0.050 -0.153 0.203 0.000 no n
206.78 0.050 -0.145 0.195 0.000 no n
207.29 0.050 -0.147 0.197 0.000 no n
207.78 0.050 -0.148 0.198 0.000 no n
208.28 0.060 -0.125 0.185 0.000 no n
208.78 0.060 -0.130 0.190 0.000 no n
209.29 0.060 -0.129 0.189 0.000 no n
209.79 0.050 -0.157 0.207 0.000 no n
210.27 0.060 -0.142 0.202 0.000 no n
210.78 0.050 -0.157 0.207 0.000 no n
211.28 0.050 -0.165 0.215 0.000 no n
211.78 0.050 -0.167 0.217 0.000 no n
212.27 0.050 -0.174 0.224 0.000 no n
212.77 0.050 -0.180 0.230 0.000 no n
213.28 0.050 -0.181 0.231 0.000 no n
213.78 0.050 -0.187 0.237 0.000 no n
214.28 0.050 -0.186 0.236 0.000 no n
214.79 0.050 -0.192 0.242 0.000 no n
215.28 0.050 -0.198 0.248 0.000 no n
215.77 0.050 -0.197 0.247 0.000 no n
Instance 1
Time(s) RngFinder(m) EKF2 Hgt(m) Innovation(m) TestRatio Rejected Fused
195.27 0.010 0.010 -0.000 0.000 no Y
195.78 0.010 0.010 -0.000 0.000 no Y
196.28 0.010 0.010 0.000 0.000 no Y
196.78 0.010 0.010 -0.000 0.000 no Y
197.28 0.050 0.091 -0.041 0.000 no n
197.78 0.050 0.094 -0.044 0.000 no n
198.27 0.050 0.085 -0.035 0.000 no n
198.78 0.070 0.059 0.011 0.000 no n
199.27 0.140 0.082 0.058 0.000 no n
199.78 0.170 0.057 0.113 0.000 no n
200.28 0.130 -0.090 0.220 0.000 no n
200.77 0.080 -0.262 0.342 0.000 no n
201.28 0.060 -0.372 0.432 0.000 no n
201.78 0.060 -0.399 0.459 0.000 no n
202.28 0.050 -0.454 0.504 0.000 no n
202.76 0.050 -0.486 0.536 0.000 no n
203.28 0.050 -0.491 0.541 0.000 no n
203.79 0.050 -0.511 0.561 0.000 no n
204.28 0.050 -0.499 0.549 0.000 no n
204.77 0.050 -0.509 0.559 0.000 no n
205.28 0.050 -0.521 0.571 0.000 no n
205.78 0.050 -0.513 0.563 0.000 no n
206.27 0.050 -0.528 0.578 0.000 no n
206.78 0.050 -0.526 0.576 0.000 no n
207.28 0.060 -0.525 0.585 0.000 no n
207.78 0.050 -0.566 0.616 0.000 no n
208.27 0.050 -0.568 0.618 0.000 no n
208.78 0.060 -0.577 0.637 0.000 no n
209.28 0.060 -0.607 0.667 0.000 no n
209.78 0.050 -0.639 0.689 0.000 no n
210.28 0.060 -0.653 0.713 0.000 no n
210.78 0.050 -0.678 0.728 0.000 no n
211.27 0.050 -0.706 0.756 0.000 no n
211.78 0.050 -0.731 0.781 0.000 no n
212.27 0.050 -0.722 0.772 0.000 no n
212.77 0.050 -0.737 0.787 0.000 no n
213.28 0.050 -0.725 0.775 0.000 no n
213.77 0.050 -0.732 0.782 0.000 no n
214.27 0.050 -0.738 0.788 0.000 no n
214.77 0.050 -0.721 0.771 0.000 no n
215.28 0.050 -0.725 0.775 0.000 no n
215.78 0.050 -0.725 0.775 0.000 no n
Can you try raising the sensor above 0.4m? Maybe go further if you can, and try with as much clearance to the ground as possible
I get that you wanted me to start the drone from minimum detectable distance of tfmini, I was using the tfmini plus model whose minimum distance is 0.1m and I changed the minimum distance for it according to this thread, and took off the drone from around 0.2 m.
Today I tried it using vl53l0x sensor which also showed the signal quality of -1 ,and hovering stably couldn’t be done in default, also the distance seemed to be dropping to 0 after a few seconds
the vl53l0x sensor was showing a distance of 8.19m which seems to be a known error when it cannot return a successful scan, I added a simple check in vl53l0x.cpp to reject when the sensor couldn’t detect proper scans and also hard coded signal strength to 100
if (distance_m >2.0f){
return PX4_OK;
}
In this case , it kind of showed better performance but still can’t hold altitude
I’m currently hitting a roadblock with this setup and would appreciate some guidance on the best path forward . What is the recommended best practice here? I feel there are too many parameters to change , I am changing everything and it’s hard to keep track of what is what,
With my vl53l0x, I am commanding the drone to go to 0.8m height , and turned on EKF2_EV_CTRL for vertical position too + baro, however it is stuck at around 0.4m with both normal firmware vs hardcoding signal quality to 100 + filtering,
this is one is on normal firmware(release/1.16).
HI, @rroche
I have mostly resolved the ekf2 and distance sensor not converging issue by rebooting the fc from my companion computer with my current sensor.
But the drone is not going to it’s required position
This is the offboard code I am using, this is either a params issue or in offboard code, i think?
/****************************************************************************
*
Copyright 2020 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 are met:
Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS”
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
****************************************************************************/
/**
@brief Offboard control example
@file offboard_control.cpp
@addtogroup examples
@author Mickey Cowden info@cowden.tech
@author Nuno Marques nuno.marques@dronesolutions.io
*/
#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_command_ack.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp>
#include <px4_msgs/msg/vehicle_local_position.hpp>
#include <px4_msgs/msg/vehicle_status.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include <rclcpp/rclcpp.hpp>
#include <stdint.h>
#include
#include
using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;
class OffboardControl : public rclcpp::Node
{
public:
enum ControlState {
kPositionControl,
kVelocityControl
};
public:
OffboardControl() :
Node(“offboard_control”),
use_sim_time_(false)
{
offboard_control_mode_publisher_ = this->create_publisher(“/fmu/in/offboard_control_mode”, 10);
trajectory_setpoint_publisher_ = this->create_publisher(“/fmu/in/trajectory_setpoint”, 10);
vehicle_command_publisher_ = this->create_publisher(“/fmu/in/vehicle_command”, 10);
// Subscription to listen for Twist commands
twist_subscriber_ = this->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", 10, std::bind(&OffboardControl::twist_callback, this, std::placeholders::_1));
// Subscription to get the current vehicle position
local_pos_subscriber_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>(
"/fmu/out/vehicle_local_position", rclcpp::QoS(10).best_effort(), std::bind(&OffboardControl::local_pos_callback, this, std::placeholders::_1));
status_subscriber_ = this->create_subscription<px4_msgs::msg::VehicleStatus>(
"/fmu/out/vehicle_status_v1", rclcpp::QoS(10).best_effort(), std::bind(&OffboardControl::vehicle_status_callback, this, std::placeholders::_1));
ack_subscriber_ = this->create_subscription<px4_msgs::msg::VehicleCommandAck>(
"/fmu/out/vehicle_command_ack", rclcpp::QoS(10).best_effort(), std::bind(&OffboardControl::vehicle_cmd_ack_callback, this, std::placeholders::_1));
joy_subscriber_ = this->create_subscription<sensor_msgs::msg::Joy>(
"/joy", 10, std::bind(&OffboardControl::joy_callback, this, std::placeholders::_1));
this->get_parameter("use_sim_time", use_sim_time_);
offboard_setpoint_counter_ = 0;
current_goal_.x = 0; // North position in NED earth-fixed frame, (metres)
current_goal_.y = 0; // East position in NED earth-fixed frame, (metres)
current_goal_.z = -0.8; // Down position (negative altitude) in NED earth-fixed frame, (metres)
current_goal_.heading = 0; // Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians)
control_State_ = kPositionControl;
velocity2d_ = true;
last_request_ = this->get_clock()->now();
arming_stamp_ = this->get_clock()->now();
auto timer_callback = [this]() -> void {
if(offboard_setpoint_counter_ == 10) {
RCLCPP_INFO(get_logger(), "Setting offboard mode...");
this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
}
else if(offboard_setpoint_counter_ == 20) {
if(!vehicle_status_.pre_flight_checks_pass ||
vehicle_status_.nav_state != px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_OFFBOARD) {
offboard_setpoint_counter_ = 0;
RCLCPP_WARN(get_logger(),
"Offboard could not be set, make sure the vehicle position "
"is published or check if there are some preflight checks failing... "
"will try again in 1 second.");
}
else {
this->arm();
}
}
if (offboard_setpoint_counter_ >= 21)
{
update_state();
}
else
{
++offboard_setpoint_counter_;
}
// offboard_control_mode needs to be paired with trajectory_setpoint
publish_offboard_control_mode();
publish_trajectory_setpoint();
};
timer_ = this->create_wall_timer(100ms, timer_callback);
}
private:
void update_state() {
if((this->get_clock()->now() - last_request_ > rclcpp::Duration::from_seconds(1.0)))
{
if(!vehicle_status_.pre_flight_checks_pass || vehicle_status_.nav_state != px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_OFFBOARD)
{
RCLCPP_ERROR(get_logger(), "Pre-flight checks are failing and offboard is not enabled");
rclcpp::shutdown();
}
else if(get_clock()->now().seconds() - twist_stamp_.seconds() < 1) // valid joystick command
{
if(vehicle_status_.arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_DISARMED &&
(twist_.linear.z < -0.4 && twist_.angular.z < -0.4))// left joystick down-right
{
this->arm();
}
else if(
vehicle_status_.arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_ARMED &&
twist_.linear.z < -0.4 && twist_.angular.z > 0.4) // left joystick down-left
{
this->disarm();
}
}
last_request_ = get_clock()->now();
}
}
void arm()
{
arming_stamp_ = get_clock()->now();
current_goal_ = local_pose_;
current_goal_.z = current_goal_.z - 0.8; // take-off 1.3 meter over current position
RCLCPP_INFO(get_logger(), "Vehicle arming..");
RCLCPP_INFO(get_logger(), "Take off at 0.8 meter... to position=(%f,%f,%f) heading=%f",
current_goal_.x,
current_goal_.y,
current_goal_.z,
current_goal_.heading);
control_State_ = kPositionControl;
publish_vehicle_command(
VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM,
VehicleCommand::ARMING_ACTION_ARM);
}
void disarm()
{
RCLCPP_INFO(get_logger(), "Vehicle disarming...");
publish_vehicle_command(
VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM,
VehicleCommand::ARMING_ACTION_DISARM);
}
void publish_offboard_control_mode()
{
OffboardControlMode msg{};
msg.position = true;
msg.velocity = true;
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 publish_trajectory_setpoint()
{
if(control_State_ != kPositionControl &&
rclcpp::Time(local_pose_.timestamp*1000).seconds() - twist_stamp_.seconds() > .1)
{
//switch to position mode with last position
control_State_ = kPositionControl;
current_goal_ = local_pose_;
RCLCPP_INFO(get_logger(), "Switch to position control (x=%f, y=%f, z=%f, yaw=%f)",
current_goal_.x, current_goal_.y, current_goal_.z, current_goal_.heading);
}
px4_msgs::msg::TrajectorySetpoint msg{};
msg.timestamp = this->now().nanoseconds() / 1000;
//double yaw = local_pose_.heading;
//double cosyaw = cos(yaw);
//double sinyaw = sin(yaw);
// Use twist message for XY velocity control
msg.velocity[0] = control_State_ == kVelocityControl? twist_.linear.y : NAN;
msg.velocity[1]= control_State_ == kVelocityControl ? twist_.linear.x : NAN;
msg.velocity[2]= control_State_ == kVelocityControl && !velocity2d_ ? -twist_.linear.z : NAN;
msg.yawspeed = control_State_ == kVelocityControl ? -twist_.angular.z : NAN;
msg.position[0] = control_State_ == kPositionControl ? current_goal_.x : NAN;
msg.position[1] = control_State_ == kPositionControl ? current_goal_.y : NAN;
msg.position[2] = control_State_ == kPositionControl ? current_goal_.z : velocity2d_ ? current_goal_.z : NAN;
msg.yaw = control_State_ == kPositionControl ? current_goal_.heading : NAN; // [-PI:PI]
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
trajectory_setpoint_publisher_->publish(msg);
}
void publish_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0)
{
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);
}
void twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
twist_ = *msg;
twist_stamp_ = this->get_clock()->now();
if(twist_stamp_.seconds() - arming_stamp_.seconds() > 5.0 && control_State_ == kPositionControl)
{
RCLCPP_INFO(get_logger(), "Switch to velocity control");
control_State_ = kVelocityControl;
}
}
void local_pos_callback(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg)
{
local_pose_ = *msg;
if(use_sim_time_) {
local_pose_.timestamp = this->get_clock()->now().nanoseconds()/1000;
}
}
void vehicle_status_callback(const px4_msgs::msg::VehicleStatus::SharedPtr msg)
{
vehicle_status_ = *msg;
}
void vehicle_cmd_ack_callback(const px4_msgs::msg::VehicleCommandAck::SharedPtr msg)
{
if(msg->result == px4_msgs::msg::VehicleCommandAck::VEHICLE_CMD_RESULT_ACCEPTED) {
RCLCPP_INFO(get_logger(), "Command accepted!");
}
else {
RCLCPP_ERROR(get_logger(), "Command rejected!");
}
}
void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg){
if(msg->buttons[10] == 1)
{
// When holding right trigger, accept velocity in Z
velocity2d_ = false;
}
else
{
velocity2d_ = true;
}
}
private:
rclcpp::TimerBase::SharedPtr timer_;
double target_altitude_ = -0.8;
rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_subscriber_;
rclcpp::Subscription<VehicleLocalPosition>::SharedPtr local_pos_subscriber_;
rclcpp::Subscription<VehicleStatus>::SharedPtr status_subscriber_;
rclcpp::Subscription<VehicleCommandAck>::SharedPtr ack_subscriber_;
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_subscriber_;
std::atomic<uint64_t> timestamp_; //!< common synced timestamped
uint64_t offboard_setpoint_counter_; //!< counter for the number of setpoints sent
geometry_msgs::msg::Twist twist_;
rclcpp::Time twist_stamp_;
rclcpp::Time arming_stamp_;
VehicleLocalPosition local_pose_;
VehicleLocalPosition current_goal_;
px4_msgs::msg::VehicleStatus vehicle_status_;
ControlState control_State_;
bool velocity2d_;
bool use_sim_time_;
rclcpp::Time last_request_;
};
int main(int argc, char *argv
)
{
std::cout << “Starting node…” << std::endl;
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared());
rclcpp::shutdown();
return 0;
}