From 392531e1315f2351ebfcdc421687b09895ea7f81 Mon Sep 17 00:00:00 2001
From: Marius Klimavicius <marius@humanitas.io>
Date: Wed, 27 Jun 2018 16:52:58 -0400
Subject: [PATCH] solo sim parameters updated

---
 buzz_scripts/include/act/states.bzz |  3 ++-
 launch/launch_config/solo.yaml      | 14 ++------------
 launch/rosbuzz.launch               |  2 +-
 src/roscontroller.cpp               |  3 +--
 4 files changed, 6 insertions(+), 16 deletions(-)

diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index 9668295..0b80cea 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -127,7 +127,8 @@ function pursuit() {
   if(neighbors.count() > 0)
     r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
 	r = math.vec2.length(r_vec)
-	gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r)
+  sqr = (r-rd)*(r-rd)+pc*pc*r*r
+  gamma = vd / math.sqrt(sqr)
 	dr = -gamma * (r-rd)
 	dT = gamma * pc
   vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml
index b8a465d..c18075b 100644
--- a/launch/launch_config/solo.yaml
+++ b/launch/launch_config/solo.yaml
@@ -6,17 +6,7 @@ topics:
   setpoint:   mavros/setpoint_position/local
   armclient:  mavros/cmd/arming
   modeclient: mavros/set_mode
-  localpos: /mavros/local_position/pose
-  stream:  mavros/set_stream_rate
+  localpos: mavros/local_position/pose
+  stream: mavros/set_stream_rate
   altitude: mavros/global_position/rel_alt
-type:
-  gps  :  sensor_msgs/NavSatFix
-  # for SITL Solo 
-  # battery :   mavros_msgs/BatteryState
-  # for solo
-  battery :   mavros_msgs/BatteryStatus
-  status  :   mavros_msgs/State
-  altitude: std_msgs/Float64
-environment : 
-  environment : solo-simulator
 
diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch
index f632f68..7f3c63b 100644
--- a/launch/rosbuzz.launch
+++ b/launch/rosbuzz.launch
@@ -3,7 +3,7 @@
 <!-- This file is included in all ROS workspace launch files  -->
 <!-- Modify with great care!  -->
 <launch>
-	<arg name="name" default="robot0"/>
+  <arg name="name" default="robot0"/>
   <arg name="xbee_plugged" default="true"/>
   <arg name="script" default="testalone"/>
   <arg name="launch_config" default="topics"/>
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 413ccf9..1f25370 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -1001,8 +1001,7 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off)
     ROS_INFO("Set stream rate call failed!, trying again...");
     ros::Duration(0.1).sleep();
   }
-  //  DEBUG
-  // ROS_INFO("Set stream rate call successful");
+   ROS_WARN("Set stream rate call successful");
 }
 
 void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)