mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 19:07:45 +08:00
ros: offboard attitude demo node: make quad jump
This commit is contained in:
+2
-1
@@ -44,6 +44,7 @@
|
|||||||
#include <platforms/px4_middleware.h>
|
#include <platforms/px4_middleware.h>
|
||||||
#include <geometry_msgs/PoseStamped.h>
|
#include <geometry_msgs/PoseStamped.h>
|
||||||
#include <std_msgs/Float64.h>
|
#include <std_msgs/Float64.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() :
|
DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() :
|
||||||
_n(),
|
_n(),
|
||||||
@@ -69,7 +70,7 @@ int DemoOffboardAttitudeSetpoints::main()
|
|||||||
_attitude_sp_pub.publish(pose);
|
_attitude_sp_pub.publish(pose);
|
||||||
|
|
||||||
std_msgs::Float64 thrust;
|
std_msgs::Float64 thrust;
|
||||||
thrust.data = 0.5;
|
thrust.data = 0.4f + 0.25 * (sinf(2.0f * (float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump'
|
||||||
_thrust_sp_pub.publish(thrust);
|
_thrust_sp_pub.publish(thrust);
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
|
|||||||
Reference in New Issue
Block a user