I’m currently learning PX4 firmware, and I wrote this code; it was built successfully into px4-firmware but not able to fly the drone to a particular position. There were no errors. The drone simply stays in HOLD mode when operating with jmavsim simulator.
Note: Initially the code worked and started fly the drone but suddenly it went hold position then afterwards it doesn’t do anything. You can see the simulation in picture.
Any help appreciated. Following is the code I’ve written:
#include <px4_log.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <signal.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
extern "C" __EXPORT int my_straight_line_module_main(int argc, char *argv[]);
static volatile sig_atomic_t _should_exit = false;
static int _task = -1;
// Target position (latitude, longitude, altitude)
static constexpr double target_pos[] = {47.3976677, 8.545474, 40.0};
int my_straight_line_module_thread(int argc, char *argv[]) {
PX4_INFO("My Straight Line Module is running!");
orb_advert_t pos_sp_adv = orb_advertise(ORB_ID(vehicle_local_position_setpoint), nullptr);
// Main loop
while (!_should_exit) {
// Create a new setpoint
struct vehicle_local_position_setpoint_s sp{};
sp.x = target_pos[0];
sp.y = target_pos[1];
sp.z = target_pos[2];
// Publish the setpoint
orb_publish(ORB_ID(vehicle_local_position_setpoint), pos_sp_adv, &sp);
px4_usleep(1000000); // Sleep for 1 second
}
PX4_INFO("My Straight Line Module exiting.");
return 0;
}
extern "C" int my_straight_line_module_main(int argc, char *argv[]) {
// Start the module's main thread
_task = px4_task_spawn_cmd(
"my_straight_line_module",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40,
2000,
my_straight_line_module_thread,
(char *const *)nullptr);
if (_task < 0) {
PX4_ERR("Task start failed");
return -errno;
}
return 0;
}
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_local_position_setpoint.h>
class MyStraightLineModule
{
public:
MyStraightLineModule() : _position_sub(ORB_ID(vehicle_local_position)) {}
void run()
{
// Create a publication handle for the setpoint
uORB::Publication<vehicle_local_position_setpoint_s> _position_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
while (!should_exit())
{
// Sleep for a while (adjust as needed)
usleep(10000); // 100 ms
if (_position_sub.update())
{
// Access the current position
vehicle_local_position_s pos;
_position_sub.copy(&pos);
PX4_INFO("Position: x=%.2f, y=%.2f, z=%.2f", (double)pos.x, (double)pos.y, (double)pos.z);
PX4_INFO("Velocity: vx=%.2f, vy=%.2f, vz=%.2f", (double)pos.vx, (double)pos.vy, (double)pos.vz);
// Update the setpoint to move the drone with a specific velocity (adjust as needed)
vehicle_local_position_setpoint_s position_sp{};
position_sp.vx = 10.0; // Desired x velocity in m/s
position_sp.vy = 0.0; // Desired y velocity in m/s
position_sp.vz = 0.0; // Desired z velocity in m/s
// Publish the velocity setpoint
_position_sp_pub.publish(position_sp);
}
}
}
private:
uORB::SubscriptionData<vehicle_local_position_s> _position_sub;
bool should_exit()
{
// Check if the module should exit (you can add more conditions if needed)
return px4_task_should_exit();
}
};
extern "C" __EXPORT int my_straight_line_module_main(int argc, char *argv[]);
int my_straight_line_module_main(int argc, char *argv[])
{
// Create an instance of MyStraightLineModule
MyStraightLineModule straight_line_module;
// Run the module
straight_line_module.run();
// This code will only be reached if the module task exits
return 0;
}
I am also tried this code to publish the velocity and to read both position and velocity I am able to read both data but not able to move the drone using these commands in jmav simulator. any help can be appreciated.
I suggest you have a look at the flight tasks module. Maybe you can add your module in there. Otherwise, you might be fighting the other controllers running with your module.
Thank you @JulianOes for your reply, I followed the flight task documentation and successfully build my task into px4.
But not able to fly the drone with my task.
I followed the whole documentation and did everything as they said and for testing, I enabled the MPC_POS_MODE 5 for running my task.
The code is giving the outputs that both activate and update functions are called successfully but I am not able to fly the drone to my desired position.
When testing the task created by me for jmavsim, I was not able to change to position mode after using commander takeoff and if I first switched position mode then I am not able to arm the vehicle.
can you please explain what the problem in might be switching modes and could you please take a look at code and check it is this the correct method?
if my method is wrong, could you please tell me what the correct way is to run my task for jmavsim.
Thank you in advance.
#include "FlightTaskMyTask.hpp"
bool FlightTaskMyTask::activate(const trajectory_setpoint_s &setpoint)
{
PX4_INFO("FlightTaskMyTask activate was called!");
bool ret = FlightTask::activate(setpoint);
// Set the target position to 10 meters in X, Y, and Z
_position_setpoint(0) = 10.0f;
_position_setpoint(1) = 10.0f;
_position_setpoint(2) = 10.0f;
// Set a target yaw angle if needed
_yawspeed_setpoint = 45.0f * 3.142f / 180.f;
// Set a target velocity if needed
_velocity_setpoint(2) = -1.0f;
PX4_INFO("FlightTaskMyTask activate was called! ret: %d", ret); // report if activation was successful
return ret;
}
bool FlightTaskMyTask::update()
{
// No need to check the current position for a fixed target
// Adjust the setpoints as needed for your specific use case
// For example, you can set a constant velocity in X, Y, and Z to reach the target
_velocity_setpoint(0) = 1.0f; // Velocity in X direction
_velocity_setpoint(1) = 1.0f; // Velocity in Y direction
_velocity_setpoint(2) = 1.0f; // Velocity in Z direction
// You can also set a constant yaw rate if needed
_yawspeed_setpoint = 45.0f * 3.142f / 180.f;
PX4_INFO("FlightTaskMyTask update was called!");
return true;
}
I will be waiting for your friend’s reply in the meantime I will be also trying to achieve the desired output.
After this I was thinking to make proper documentation for possible errors while creating our own flight task, I was hoping you could share your inputs.
