From 1b2b89a59b825710647d41b724d4f21b9e32f7a3 Mon Sep 17 00:00:00 2001
From: lm <pixhawk@switched.com>
Date: Tue, 28 Dec 2010 19:38:33 +0100
Subject: [PATCH] Added decent movement pattern to simulation
---
src/comm/MAVLinkSimulationLink.cc | 23 +++++++++++++----------
1 file changed, 13 insertions(+), 10 deletions(-)
diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index e281aa6..ed67251 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -375,9 +375,12 @@ void MAVLinkSimulationLink::mainloop()
// Move X Position
- x = 8.0*sinf((double)circleCounter);
- y = 3.0*cosf((double)circleCounter);
- z = 1.8 + 1.2*sin((double)circleCounter/2.0);
+ x = 8.0*sin((double)circleCounter/50.0);
+ y = 3.0*cos((double)circleCounter/40.0);
+ z = 1.8 + 1.2*sin((double)circleCounter/60.0);
+
+ circleCounter++;
+
// x = (x > 5.0f) ? 5.0f : x;
// y = (y > 5.0f) ? 5.0f : y;
@@ -402,15 +405,15 @@ void MAVLinkSimulationLink::mainloop()
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;
- // GPS RAW
- mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.00001), 8.548103+(y*0.00001), z, 0, 0, 2.5f, 0.1f);
- bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
- //add data into datastream
- memcpy(stream+streampointer,buffer, bufferlength);
- streampointer += bufferlength;
+// // GPS RAW
+// mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.00001), 8.548103+(y*0.00001), z, 0, 0, 2.5f, 0.1f);
+// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
+// //add data into datastream
+// memcpy(stream+streampointer,buffer, bufferlength);
+// streampointer += bufferlength;
// GLOBAL POSITION
- mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 47.378028137103+(x*0.01), 8.54899892510421+(y*0.01), z+25.0, 0, 0, 0);
+ mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 47.378028137103+(x*0.001), 8.54899892510421+(y*0.001), z+25.0, 0, 0, 0);
bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);