From 1b2b89a59b825710647d41b724d4f21b9e32f7a3 Mon Sep 17 00:00:00 2001 From: lm 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 e281aa6cc..ed67251bf 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); -- 2.22.0