diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-21 11:12:04 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-28 18:25:36 +0100 |
commit | 7ff84c0dcf48e128ae98c52ae6affd43aa46d341 (patch) | |
tree | e983898f0b3e701bdf13c3bcbb7c9ac8fbab9278 /src/platforms/ros/nodes | |
parent | 5b0423109f631763f7e71311d4af7ffa4805edda (diff) | |
download | px4-firmware-7ff84c0dcf48e128ae98c52ae6affd43aa46d341.tar.gz px4-firmware-7ff84c0dcf48e128ae98c52ae6affd43aa46d341.tar.bz2 px4-firmware-7ff84c0dcf48e128ae98c52ae6affd43aa46d341.zip |
ros: offboard attitude demo node: make quad jump
Diffstat (limited to 'src/platforms/ros/nodes')
-rw-r--r-- | src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp | 3 |
1 files changed, 2 insertions, 1 deletions
diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp index 402d47783..027b29a87 100644 --- a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -44,6 +44,7 @@ #include <platforms/px4_middleware.h> #include <geometry_msgs/PoseStamped.h> #include <std_msgs/Float64.h> +#include <math.h> DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() : _n(), @@ -69,7 +70,7 @@ int DemoOffboardAttitudeSetpoints::main() _attitude_sp_pub.publish(pose); 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); } return 0; |