How to conrtol velocity for position control in offboard mode? #146

Hello everyone,
I have a offboard code that gives about 50 setpoints to drone. It draws a spiral with that setpoints. My problem is I couldn’t get smooth travel. In every setpoint, drone gives a high roll or pitch instant and then floats to the next setpoint. Is there a way to have stable velocity while passing the setpoints? I mean I don’t want the drone to stop at each setpoint, I just want to pass on setpoints. Actually is there any other way to do spiral, like drawing a circle and then extending it or something else? If there is I would be very pleasant.

I have one more problem and this one is not so important. I just want to know if there is a short way to make vehicles heading in the moving direction. Mine just locks to certain yaw.
Thank you

Here is the code I have:

#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/timesync.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp>
#include <px4_msgs/msg/vehicle_local_position.hpp>
#include <rclcpp/rclcpp.hpp>

#include <stdint.h>
#include <chrono>
#include <iostream>
#include "std_msgs/msg/string.hpp"
#include <math.h>

float X;
float Y;	

using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;
class setpoint : public rclcpp::Node {
public:
    setpoint() : Node("setpoint") {
     
	    offboard_control_mode_publisher_ =
		    this->create_publisher<OffboardControlMode>("fmu/offboard_control_mode/in", 10);
	    trajectory_setpoint_publisher_ =
		    this->create_publisher<TrajectorySetpoint>("fmu/trajectory_setpoint/in", 10);
	    vehicle_command_publisher_ =
		    this->create_publisher<VehicleCommand>("fmu/vehicle_command/in", 10);

	    // get common timestamp
	    timesync_sub_ =
		    this->create_subscription<px4_msgs::msg::Timesync>("fmu/timesync/out", 10,
			    [this](const px4_msgs::msg::Timesync::UniquePtr msg) {
				    timestamp_.store(msg->timestamp);
			    });

	    offboard_setpoint_counter_ = 0;

    

	    auto sendCommands = [this]() -> void {
		    if (offboard_setpoint_counter_ == 10) {
			    // Change to Offboard mode after 10 setpoints
			    this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);

			    // Arm the vehicle
			    this->arm();

		    }
//-------------
		    subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>(
		    "/fmu/vehicle_local_position/out",
#ifdef ROS_DEFAULT_API
            10,
#endif
		    [this](const px4_msgs::msg::VehicleLocalPosition::UniquePtr msg) {
		    X = msg->x;
		    Y = msg->y;
		    std::cout << "\n\n\n\n\n\n\n\n\n\n";
		    std::cout << "RECEIVED VEHICLE GPS POSITION DATA"   << std::endl;
		    std::cout << "=================================="   << std::endl;
		    std::cout << "ts: "      << msg->timestamp    << std::endl;
		    //std::cout << "lat: " << msg->x  << std::endl;
		    //std::cout << "lon: " << msg->y << std::endl;
		    std::cout << "lat: " << X  << std::endl;
		    std::cout << "lon: " << Y << std::endl;
		    std::cout << "waypoint: " << waypoints[waypointIndex][0] << std::endl;
		    std::cout << "waypoint: " << waypoints[waypointIndex][1] << std::endl;
		    if((waypoints[waypointIndex][0] + 0.3 > X && waypoints[waypointIndex][0] - 0.3 < X)&&(waypoints[waypointIndex][1] + 0.3 > Y && waypoints[waypointIndex][1] - 0.3 < Y)){
		    waypointIndex++;
		    if (waypointIndex >= waypoints.size())
			    exit(0);
			    //waypointIndex = 0;

		    RCLCPP_INFO(this->get_logger(), "Next waypoint: %.2f %.2f %.2f", waypoints[waypointIndex][0], waypoints[waypointIndex][1], waypoints[waypointIndex][2]);
	    }
	    });
//--------------

        		    // offboard_control_mode needs to be paired with trajectory_setpoint
		    publish_offboard_control_mode();
		    publish_trajectory_setpoint();

       		     // stop the counter after reaching 11
		    if (offboard_setpoint_counter_ < 11) {
			    offboard_setpoint_counter_++;
		    }
	    };

    /*
	    auto nextWaypoint = [this]() -> void {
		    
		    waypointIndex++; 

		    if (waypointIndex >= waypoints.size()) 
			    waypointIndex = 0;

		    RCLCPP_INFO(this->get_logger(), "Next waypoint: %.2f %.2f %.2f", waypoints[waypointIndex][0], waypoints[waypointIndex][1], waypoints[waypointIndex][2]);
	    };
	    */
	    commandTimer = this->create_wall_timer(100ms, sendCommands);
	    //waypointTimer = this->create_wall_timer(2s, nextWaypoint); //EA	
    }

    void arm() const;
    void disarm() const;
    void topic_callback() const;
private:
    

std::vector<std::vector<float>> waypoints = {
{2,0,-5,},
{2.35216,0.476806,-5,},
{2.57897,1.09037,-5,},
{2.64107,1.80686,-5,},
{2.50814,2.58248,-5,},
{2.16121,3.36588,-5,},
{1.59437,4.10097,-5,},
{0.815842,4.73016,-5,},
{-0.151838,5.19778,-5,},
{-1.27233,5.45355,-5,},
{-2.49688,5.45578,-5,},
{-3.76641,5.17438,-5,},
{-5.01428,4.59315,-5,},
{-6.1696,3.71161,-5,},
{-7.16089,2.54591,-5,},
{-7.91994,1.12896,-5,},
{-8.38568,-0.490343,-5,},
{-8.50782,-2.24876,-5,},
{-8.25018,-4.07119,-5,},
{-7.59329,-5.87384,-5,},
{-6.53644,-7.56803,-5,},
{-5.09871,-9.06439,-5,},
{-3.31919,-10.2773,-5,},
{-1.25611,-11.1293,-5,},
{1.01499,-11.5555,-5,},
{3.40395,-11.5071,-5,},
{5.8096,-10.9548,-5,},
{8.12407,-9.89139,-5,},
{10.2375,-8.33272,-5,},
{12.0431,-6.31859,-5,},
{13.4424,-3.91182,-5,},
{14.3502,-1.19649,-5,},
{14.6991,1.72493,-5,},
{14.4435,4.73543,-5,},
{13.5626,7.70817,-5,},
{12.0624,10.5118,-5,},
{9.97696,13.0162,-5,},
{7.36759,15.0983,-5,},
{4.32167,16.6482,-5,},
{0.949612,17.5744,-5,},
{-2.619,17.8084,-5,},
{-6.24045,17.3094,-5,},
{-9.76262,16.0665,-5,},
{-13.0314,14.1004,-5,},
{-15.8974,11.4644,-5,},
{-18.2226,8.24237,-5,},
{-19.8868,4.54696,-5,},
{-20.7936,0.515337,-5,},
{-20.8754,-3.69574,-5,},
{-20.0972,-7.91595,-5,},
{0,0,0}
};		// Land
    

    int waypointIndex = 0;

    rclcpp::TimerBase::SharedPtr commandTimer;
    rclcpp::TimerBase::SharedPtr waypointTimer;

    rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
    rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
    rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_;
    rclcpp::Subscription<px4_msgs::msg::Timesync>::SharedPtr timesync_sub_;
    //
    rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr subscription_;
    //
    std::atomic<uint64_t> timestamp_;   //!< common synced timestamped

    uint64_t offboard_setpoint_counter_;   //!< counter for the number of setpoints sent

    void publish_offboard_control_mode() const;
    void publish_trajectory_setpoint() const;
    void publish_vehicle_command(uint16_t command, float param1 = 0.0,
			         float param2 = 0.0) const;
};

void setpoint::arm() const {
    publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);

    RCLCPP_INFO(this->get_logger(), "Arm command send");
}

void setpoint::disarm() const {
    publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);

    RCLCPP_INFO(this->get_logger(), "Disarm command send");
}

void setpoint::publish_offboard_control_mode() const {
    OffboardControlMode msg{};
    msg.timestamp = timestamp_.load();
    msg.position = true;
    msg.velocity = false;
    msg.acceleration = false;
    msg.attitude = false;
    msg.body_rate = false;

    offboard_control_mode_publisher_->publish(msg);
}	

void setpoint::publish_trajectory_setpoint() const {
    TrajectorySetpoint msg{};
    msg.timestamp = timestamp_.load();
    msg.position = {waypoints[waypointIndex][0],waypoints[waypointIndex][1],waypoints[waypointIndex][2]};
    msg.yaw = std::nanf("0");		//-3.14; // [-PI:PI]
    trajectory_setpoint_publisher_->publish(msg);
}

void setpoint::publish_vehicle_command(uint16_t command, float param1,
				          float param2) const {
    VehicleCommand msg{};
    msg.timestamp = timestamp_.load();
    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;

    vehicle_command_publisher_->publish(msg);
}

int main(int argc, char* argv[]) {
    std::cout << "Starting setpoint node..." << std::endl;
    
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<setpoint>());

    rclcpp::shutdown();
    return 0;
}

I’m not sure if this works, but you could try adding the desired velocity to the TrajectorySetpoint msg. PX4 should then use it as feedforward term. But to do so you should compute (either in advance or in real time) the velocity for each setpoint.
Another possibility could be something like this: you send the waypoint with index waypointIndex+1 to the drone but you update the index when you reach waypointIndex. In this way you actually never reach the desired setpoint because you change it as soon as you reach the one preceding it. I hope it is clear. If you never reach the setpoint then you never stop moving.

Lastly, I suggest you to change this condition

with something line norm(current_waypoint-current_position) < threshold.

To have the vehicle heading in the moving direction, you can read its current velocity, from there extract the moving direction yaw angle w.r.t. the the heading angle and then set the yawspeed field of the TrajectorySetpoint msg to drive to zero this difference. Or, as you know the desired trajectory, you can compute a priory the desired yaw angle at each waypoint such that it coincides with the desired moving direction, and than you send this value to the drone with the yaw field of the TrajectorySetpoint msg.

Thank you for your fast reply. Your comments are so clever I will try them right now. Thank you so much. I will let you know after try.

Unfortunately your second suggestion didnt work(if never reach the setpoint never stop moving) Problem is when setpoint is given it gives a high roll or pitch and then floats to the setpint, then stops with reverse pitch or roll

I wanted to give desired velocity to TrajectorySetpoint msg but I couldnt make it work. Do you have an example or syntax?

You can try this: I modified your code to compute in real time the desired setpoints and to send them at 10Hz to the drone. The only differences from your solutions are that

  • The setpoints are continuously updated both in desired position and velocity. There is no lookup table.
  • Also the heading angle is controlled.

You can diff the two versions to see the changes.
To get the desired position and velocity wrote the spiral equations in polar coordinates

theta(t) = theta_0 + omega * t . This is the polar angle. It increases linearly (gain omega) with the time

rho(theta) = rho_0 + K * theta . This is the polar radius. It increases linearly (gain K) with theta.

From your setpoints it results that rho_0=2 and K=2.

Then, going back to the Cartesian coordinates…

x = rho * cos(theta)
y = rho * sin(theta)

To compute the desired velocity it is sufficient to retrieve the time derivatives of x and y and finally the desired heading angle is a function of the desired velocities.

The only free parameter left is omega, which regulates the overall drone speed. Notice however that the speed is not constant but it increases as rho (and therefore the time) increases. You can see it letting the simulation run.

#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/timesync.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp>
#include <px4_msgs/msg/vehicle_local_position.hpp>
#include <rclcpp/rclcpp.hpp>

#include <stdint.h>
#include <chrono>
#include <iostream>
#include "std_msgs/msg/string.hpp"
#include <math.h>

float X;
float Y;	

using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;
class setpoint : public rclcpp::Node {
public:
    setpoint() : Node("setpoint") {
     
	    offboard_control_mode_publisher_ =
		    this->create_publisher<OffboardControlMode>("fmu/offboard_control_mode/in", 10);
	    trajectory_setpoint_publisher_ =
		    this->create_publisher<TrajectorySetpoint>("fmu/trajectory_setpoint/in", 10);
	    vehicle_command_publisher_ =
		    this->create_publisher<VehicleCommand>("fmu/vehicle_command/in", 10);

	    // get common timestamp
	    timesync_sub_ =
		    this->create_subscription<px4_msgs::msg::Timesync>("fmu/timesync/out", 10,
			    [this](const px4_msgs::msg::Timesync::UniquePtr msg) {
				    timestamp_.store(msg->timestamp);
			    });

	    offboard_setpoint_counter_ = 0;

		subscription_ =
			this->create_subscription<px4_msgs::msg::VehicleLocalPosition>(
				"/fmu/vehicle_local_position/out",
				10,
				[this](const px4_msgs::msg::VehicleLocalPosition::UniquePtr msg) {
					X = msg->x;
					Y = msg->y;
					float Z = msg->z;
					if(!start_trj && (p0_x + 1.0 > X && p0_x - 1.0 < X)&&(p0_y + 1.0 > Y && p0_y - 1.0 < Y)&&(p0_z + 1.0 > Z && p0_z - 1.0 < Z)){
						start_trj = true;
						std::cout << "start trj!" << std::endl;
					}
				}
			);


	    auto sendCommands = [this]() -> void {
		    if (offboard_setpoint_counter_ == 10) {
			    // Change to Offboard mode after 10 setpoints
			    this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);

			    // Arm the vehicle
			    this->arm();

		    }
			// the spiral, in polar coordinates (theta, rho), is given by
			// theta = theta_0 + omega*t
			// rho = rho_0 + K*theta
			float theta = theta_0 + omega * 0.1 * discrete_time_index;
			float rho = rho_0 + K * theta;
			
			// from polar to cartesian coordinates
			des_x = rho * cos(theta);
			des_y = rho * sin(theta);

			// velocity computation
			float dot_rho = K*omega;
			dot_des_x = dot_rho*cos(theta) - rho*sin(theta)*omega;
			dot_des_y = dot_rho*sin(theta) + rho*cos(theta)*omega;
			// desired heading direction
			gamma = atan2(dot_des_y, dot_des_x);

        	// offboard_control_mode needs to be paired with trajectory_setpoint
		    publish_offboard_control_mode();
		    publish_trajectory_setpoint();

       		     // stop the counter after reaching 11
		    if (offboard_setpoint_counter_ < 11) {
			    offboard_setpoint_counter_++;
		    }
			if (start_trj){
				discrete_time_index++;
			}
	    };
	    commandTimer = this->create_wall_timer(100ms, sendCommands);
    }

    void arm() const;
    void disarm() const;
    void topic_callback() const;
private:
    
	bool start_trj = false;

	const float omega = 0.3; 	// angular speed of the POLAR trajectory
	const float K = 2;			// [m] gain that regulates the spiral pitch

	
	const float rho_0 = 2;
	const float theta_0 = 0;
	const float p0_z = -5.0;
	float p0_x = rho_0*cos(theta_0);
	float p0_y = rho_0*sin(theta_0);
	float des_x = p0_x, des_y = p0_y, des_z = p0_z;
	float dot_des_x = 0.0, dot_des_y = 0.0;
	float gamma = M_PI_4;
    
	uint32_t discrete_time_index = 0;

    rclcpp::TimerBase::SharedPtr commandTimer;
    rclcpp::TimerBase::SharedPtr waypointTimer;

    rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
    rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
    rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_;
    rclcpp::Subscription<px4_msgs::msg::Timesync>::SharedPtr timesync_sub_;
    //
    rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr subscription_;
    //
    std::atomic<uint64_t> timestamp_;   //!< common synced timestamped

    uint64_t offboard_setpoint_counter_ = 0;   //!< counter for the number of setpoints sent

    void publish_offboard_control_mode() const;
    void publish_trajectory_setpoint() const;
    void publish_vehicle_command(uint16_t command, float param1 = 0.0,
			         float param2 = 0.0) const;
};

void setpoint::arm() const {
    publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);

    RCLCPP_INFO(this->get_logger(), "Arm command send");
}

void setpoint::disarm() const {
    publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);

    RCLCPP_INFO(this->get_logger(), "Disarm command send");
}

void setpoint::publish_offboard_control_mode() const {
    OffboardControlMode msg{};
    msg.timestamp = timestamp_.load();
    msg.position = true;
    msg.velocity = false;
    msg.acceleration = false;
    msg.attitude = false;
    msg.body_rate = false;

    offboard_control_mode_publisher_->publish(msg);
}	

void setpoint::publish_trajectory_setpoint() const {
    TrajectorySetpoint msg{};
    msg.timestamp = timestamp_.load();
    msg.position = {des_x, des_y, des_z};
    msg.velocity = {dot_des_x, dot_des_y, 0.0};
    msg.yaw = gamma;		//-3.14; // [-PI:PI]
    trajectory_setpoint_publisher_->publish(msg);
}

void setpoint::publish_vehicle_command(uint16_t command, float param1,
				          float param2) const {
    VehicleCommand msg{};
    msg.timestamp = timestamp_.load();
    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;

    vehicle_command_publisher_->publish(msg);
}

int main(int argc, char* argv[]) {
    std::cout << "Starting setpoint node..." << std::endl;
    
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<setpoint>());

    rclcpp::shutdown();
    return 0;
}

I tested this solution in gazebo and the trajectory is super smooth. Pay attention that if the velocity increases too much then you will reach the maximum tilting angles and you won’t be able to track the trajectory. In this scenario omega must be reduced as rho increases.

It is so smooth. Your algorithm is so nice. I used this algorithm in a separate code to generate waypoints before. Unfortunately I couldnt directly implement it to the setpoint code.

I am currently trying to implement a first setpoint and vtol transition. My final goal is to make my vtol vehicle go to the first setpoint in fixed wing mode after vertical take off and after spiral turn back to starting point in fixed wing mode then vertical landing. Do you have suggestion for vtol transition? I read about writing a ros service in old docs. Do you know if there is an easy or automated way to do transition?

And there is one thing about your code. You didnt make the msg.velocity = true in publish_offboard_control_mode() . And then give msg.velocity = {dot_des_x, dot_des_y, 0.0}.
Is it have an effect even msg.velocity is false

I apologize that I can’t provide nice sample code like @Benja above, but I can provide a bit of an alternative perspective. Also note that my experience so far is on a platform that is stuck on PX4 ~1.9.2 with some vendor-added extensions.

My offboard code is set up to “reply” to GLOBAL_POSITION_INT messages. When it starts, it bumps the message rate for those up to 20 Hz and then dynamically computes the setpoint based on the aircraft’s current position. To handle the velocity control part of it, I had bad luck using the feedforward terms (since they just get added directly to the control loop, the aircraft ends up with a velocity of V_FF + computed PID velocity).

To control the velocity using the position setpoints, I reasoned that the P term in the position controller is the primary way that the control loops compute a target velocity from a setpoint. So every time I receive a GLOBAL_POSITION_INT message, I compute a setpoint along the line I’m trying to follow that is d = v_des * 1/P distance away from the current position. When the position controller computes the error, it should come up with the same value of d, and when it does v = P*d + (maybe I and D terms), the v it comes back with is approximately the v_des that I started with.

Is there a better way? Probably. This approach has worked quite well through hundreds of hours of flight though.

Hi @tonyarkles , given the desired trajectory and the current position coming from GLOBAL_POSITION_INT, how do you compute v_des, do you always apply the same formula/principle or it is tuned on the specific task?

And regarding the way you compute the position setpoint

To obtain at the end

Shouldn’t it be

v = P*(d - p_curr) + (maybe I and D terms)

Where p_curr is the current position?

It’s very tough, i used all the things mention above!!

@Benja Apologies, I’ll try to explain this better.

v_des in this case is the desired velocity that you want the aircraft to fly. So if you’re trying to move at 5m/s, you’d compute d = 5 * 1/P, where P is the position gain that is configured on the aircraft. I forget the specific variable in PX4, I think it’s called something like MPC_XY_P.

Once you’ve got d computed, you take the position provided from the GLOBAL_POSITION_INT message and compute a new position setpoint that is d meters away from the current position. If you’ve got a target position you’re trying to fly towards at v_des m/s, you compute the line from your current position to the target position, but send a setpoint that is only d meters along that line. That way, the aircraft, when it’s computing the target velocity internally, won’t end up computing some huge value for the velocity term and do the hard bank to chase it.

Does that help?

Thanks! That was very precise :smile:

So, summing it up, can we say that what you are doing is a position controller for which the velocity magnitute is given by an external component?
From the target position and the current position you get the desired moving direction, i.e. the vector part of v_des, and then the scalar part (the magnitude) is provided by some other component.

However, you have to deal also with the PX4 standard position controller, therefore you take this velocity setpoint, you scale it by 1/MPC_XY_P to get your d and finally you add the current position (form GLOBAL_POSITION_INT and some frame conversion I assume) to retrieve a “fake” position setpoint to sent to the drone.
The drone position controller will get this setpoint, compute the error (which is rougly d) and scale it by MPC_XY_P endind up with a velocity setpoint equal to your v_des.

Why don’t you turn off the position controller (set to NaN the position setpoints) and directly impose v_des as velocity setpoint?

@Ege_Alsan , just not to go too off topic, regarding

For what I saw the multicopter position control module only checks if the _vehicle_control_mode.flag_multicopter_position_control_enabled flag is enabled and if that happens then it uses all the information from the trajectory_setpoint message. If the position field is not nan, then position is controlled and velocity and acceleration act as feedforward terms. If the position field is nan and the velocity one is not then the velocity is controlled and the acceleration acts as feedforward term. If only the acceleration is not nan then it is processed and sent to the multicopter attitude controller.
The _vehicle_control_mode.flag_multicopter_position_control_enabled flag of the vehicle_control_mode message is set by the commander module if (but not only if) the vehicle is in offboard mode, and the position or the velocity flags in the offboard_control_mode message are set (see here, here and here).

That was the first approach I tried! A quirk on the specific aircraft and payload that we are using is that we’ve got a pretty limited serial link (56k) between the PX4 and the onboard computer. Because of that, we could only run the Mavlink messages we needed around 20Hz, and as it turned out the loop bandwidth was insufficient for smooth control. Ultimately we were looking for position control, but wanted to chase those positions at a defined velocity; trying to do that by disabling the position controller ended up either having very slow convergence or oscillation, depending on how much we scaled the position error to compute a velocity term.

A nice side effect has to do with error handling. We’re flying at ~20m/s. PX4 does have a built-in timeout that kicks back into Hold mode if for some reason the stream of Offboard setpoints is interrupted. In the window of time between the stream dying and the timeout being reached, we’ve got two possibilities:

  • The aircraft holds onto the most recent position setpoint and decelerates as it approaches that setpoint.
  • The aircraft holds onto the most recent velocity setpoint and continues heading in that direction with that velocity until the timeout gets tripped.

Although it’s not the reason I built it the way I did, it’s a nice comforting side-effect to know that if the code does crash, the aircraft will be heading to a defined point instead of just continuing on with a defined velocity until the error is caught.