changed controller to roll pitch

real_robot/coordination_formation_control_pkg/config/missionConfig.yaml

 n_drones: 2
 n_ugvs: 0
 hz_freq: 50 # controller frquency
+mass: 33 # grams
 include_obstacle: false
 include_orientation: false
 n_obs: 0

real_robot/coordination_formation_control_pkg/src/drone_node.cpp

@@ -9,7 +9,7 @@
 #include "coordination_formation_control_pkg/logData.h"
 #include <rosbag/bag.h>
 
-#include "crazyswarm/Hover.h"
+#include "crazyswarm/ZDistance.h"
 #include "crazyswarm/Land.h"
 #include "std_msgs/Empty.h"
 #include <unistd.h>
@@ -60,6 +60,7 @@ int main(int argc, char **argv) {
   ROS_INFO("Drone %d turned ON", robotid);
   int n_drones;
   int hz_freq, n_obs;
+  float mass = 0;
   bool use_obstacle,use_orientation;
   nh.getParam("/n_drones", n_drones);
   std::cout << "NUmber of drones " << n_drones << std::endl;
@@ -67,6 +68,8 @@ int main(int argc, char **argv) {
   nh.getParam("/include_obstacle", use_obstacle);
   nh.getParam("/include_orientation", use_orientation);
   nh.getParam("/n_obs", n_obs);
+  nh.getParam("/mass", mass);
+  mass = mass/1000.0;
 
   float dt = 1/(float)hz_freq;
 
@@ -90,9 +93,9 @@ int main(int argc, char **argv) {
   logdata->current_acc.resize(n_drones);
 
   /* current command*/
-  crazyswarm::Hover current_command;
-  current_command.vx = 0;
-  current_command.vy = 0;
+  crazyswarm::ZDistance current_command;
+  current_command.roll = 0;
+  current_command.pitch = 0;
   current_command.yawrate = 0;
   current_command.zDistance = 0;
 
@@ -179,7 +182,7 @@ int main(int argc, char **argv) {
   /* publisher*/
   std::stringstream ss;
   ss << "/cf"  << std::to_string(robotid) << "/cmd_hover";
-  ros::Publisher vel_cmd_publisher = nh.advertise<crazyswarm::Hover>(ss.str(),1);
+  ros::Publisher vel_cmd_publisher = nh.advertise<crazyswarm::ZDistance>(ss.str(),1);
 
   std::stringstream ss_stop;
   ss_stop << "/cf"  << std::to_string(robotid) << "/cmd_stop";
@@ -254,8 +257,8 @@ int main(int argc, char **argv) {
     {
       // send the robot to hover at zero velocity in plane
       // std::cout << "Here" << std::endl;
-      current_command.vx = 0;
-      current_command.vy = 0;
+      current_command.roll = 0;
+      current_command.pitch = 0;
       current_command.yawrate = 0;
       current_command.zDistance = 0.5;
       vel_cmd_publisher.publish(current_command);
@@ -309,14 +312,14 @@ int main(int argc, char **argv) {
         input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
         // std::cout << input << std::endl;
         /* integrate acceleration*/
-        Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0),uav_state_p(0, robotid-1),uav_state_p(1, robotid-1) );
-        Eigen::Vector2d vel_command_;
-         vel_command_ << input(0,0), input(1,0);
+        // Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0),uav_state_p(0, robotid-1),uav_state_p(1, robotid-1) );
+        // Eigen::Vector2d vel_command_;
+        //  vel_command_ << input(0,0), input(1,0);
         /* reached a certain waypoint and we want to stop sending command*/
         if(next_waypoint->stop) // change back TODO
         {
-          current_command.vx = 0;
-          current_command.vy = 0;
+          current_command.roll = 0;
+          current_command.pitch = 0;
         }
         else
         {
@@ -324,19 +327,23 @@ int main(int argc, char **argv) {
           /* get latest yaw*/
           robot_state_t current_state_;
           current_state_ = uavs[robotid-1]->get_state();
+
           float yaw = current_state_.p(2)*M_PI/180.0;  // TODO confirm that indeed it is in degrees
-          float vxl =  vel_command_(0)*std::cos(yaw) +  vel_command_(1)*std::sin(yaw) ;
-          float vyl = - vel_command_(0)*std::sin(yaw)  +  vel_command_(1)*std::cos(yaw) ;
+          float ax =  input(0,0)*std::cos(yaw) +  input(1,0)*std::sin(yaw) ;
+          float ay = - input(0,0)*std::sin(yaw)  +  input(1,0)*std::cos(yaw) ;
+          //
+          // float roll_command = ;
+          // float pitch_command = ;
 
           // current_command.vx = vxl;
           // current_command.vy = vyl;
           row_vector_2d_t in;
-          in << vxl, vyl;
+          in << ax, ay;
           row_vector_2d_t range;
           range << -0.15, 0.15;
           row_vector_2d_t out = saturate(in, range);
-          current_command.vx = out(0);
-          current_command.vy = out(1);
+          current_command.roll = out(0)/mass; // conver it to the desired angle
+          current_command.pitch = out(1)/mass;
           // vxx = vxl;
           // vyy = vyl;
         }
@@ -424,8 +431,8 @@ int main(int argc, char **argv) {
       logdata->input_orientation.y = input(1,4);
       logdata->input_integration.x = input(0,5);
       logdata->input_integration.y = input(1,5);
-      logdata->input_to_system.x = current_command.vx;
-      logdata->input_to_system.y = current_command.vy;
+      logdata->input_to_system.x = current_command.roll;
+      logdata->input_to_system.y = current_command.pitch;