new controller tweaks

Onboard-SDK-ROS/include/dji_osdk_ros/bus_driver.h

@@ -111,7 +111,7 @@ ServiceAck obtainCtrlAuthority();
 
 bool takeoff();
 
-bool relativePosition(float x, float y, float z, float yaw);
+std::vector<float> relativePosition(float x, float y, float z, float yaw);
 
 float applyRotationAngle(float targetYaw);
 
@@ -127,10 +127,12 @@ void calibratePositionReference(std::vector<float> requestedPosition, std::vecto
 
 void updateMagnitudeScaler(float newValue);
 
-std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec);
+float flatAngleBetween(std::vector<float> a, std::vector<float> b);
+
+void updateAngleScaler(float newValue);
 
 bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res);
-std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec);
+// std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec);
 
 void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
 void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg);

Onboard-SDK-ROS/scripts/qualisysFeedback.py

@@ -25,7 +25,7 @@ def main():
     name = rospy.get_param("~trackName")
     namespace = rospy.get_param("~qualisysName")
     rospy.loginfo("looking for track of name " + str(name) + " ...")
-    pub = rospy.Publisher(namespace + "/pose_feedback", Transform, queue_size = 1)
+    pub = rospy.Publisher(namespace, Transform, queue_size = 1)
     rospy.Subscriber("/" + namespace + "/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1)
     rospy.spin()
 

Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp

@@ -58,14 +58,16 @@ geometry_msgs::Transform ex_pos; // position from external source (qualisys or s
 geometry_msgs::Transform track_pos; // position from external source (qualisys or some such)
 geometry_msgs::Transform previous_ex_pos; // position from external source (qualisys or some such)
 geometry_msgs::Transform previous_track_pos; // position from external source (qualisys or some such)
+std::vector<float> expectedVel; // expected position after applying relative position move
 float feedbackYaw; // yaw as measured by pos ref
 float feedbackPitch; // pitch as measured by pos ref
 float feedbackRoll; // roll as measured by pos ref
 float trackingYaw; // yaw to track
 float trackingPitch; // pitch to track
 float trackingRoll; // roll to track
-int calibrationWindow = 5; // the last _ position calls that are included in coordinate system calibration
+int calibrationWindow = 25; // the last _ position calls that are included in coordinate system calibration
 std::vector<float> calibrationWindowValues;
+float angleOffset;
 float controlLoopTime;
 float updateRateParam;
 std::vector<std::vector<float>> poseRotationMatrix; // need to run calibration to get
@@ -206,6 +208,15 @@ float dotProduct(std::vector<float> vect_A, std::vector<float> vect_B)
     return product;
 }
 
+float flatAngleBetween(std::vector<float> a, std::vector<float> b)
+{
+  // flatten each vector into the horizontal plane because im lazy
+  std::vector<float> aFlat = std::vector<float> {a[0], a[1], 0.0};
+  std::vector<float> bFlat = std::vector<float> {b[0], b[1], 0.0};
+  float dotProd = dotProduct(aFlat, bFlat);
+  return normalizeAngle(acosf(dotProd/(Magnitude3(a)*Magnitude3(b))));
+}
+
 // Function to find
 // cross product of two vector array.
 std::vector<float> crossProduct(std::vector<float> vect_A, std::vector<float> vect_B)
@@ -365,11 +376,6 @@ void posTrackingCallback(const geometry_msgs::Transform::ConstPtr& msg)
   trackingPitch = asin(2.0*(track_pos.rotation.x*track_pos.rotation.z - track_pos.rotation.w*track_pos.rotation.y));
   trackingRoll = atan2(2.0*(track_pos.rotation.x*track_pos.rotation.w + track_pos.rotation.y*track_pos.rotation.z), 1 - 2.0*(track_pos.rotation.z*track_pos.rotation.z + track_pos.rotation.w*track_pos.rotation.w));
 
-  // update wall time for tracking trajectory
-  // ros::param::get("controller_damping", damping);
-  // ros::param::get("controller_frequency", frequency);
-  // kp = (18.0*frequency*frequency)/2.0;
-  // kd = (9.0/2.0)*frequency*damping;
 }
 
 bool setupWaypointMission(int responseTimeout)
@@ -435,249 +441,86 @@ bool runWaypointMission()
 
 bool runPositionMission()
 {
+
   std::vector<float> velocityCommand {0.0, 0.0, 0.0};
-  if (positionPlaybackParam)
+  ros::Rate updateRate(updateRateParam); // update period for topics/commands
+  positionMissionFlag = true;
+  int currentPositionIndex = 0;
+  int numPositions;
+  ros::param::get("/robot_list/robot_" + to_string(droneId) + "/numPositions", numPositions);
+
+
+  while (positionMissionFlag && ros::ok())
   {
-    // replaying a capture from reference
-    // executing a set of positions in reference
-    ros::Rate updateRate(updateRateParam); // update period for topics/commands
-    positionMissionFlag = true;
+    // let topics update
+    ros::spinOnce();
+
+
+    dt = (ros::WallTime::now() - previousWallTime).toNSec()*1e-9;
+    g = 1/(1 + kd*dt + kp*dt*dt);
+    ksg = kp*g;
+    kdg = (kd + kp*dt)*g;
+    previousWallTime = ros::WallTime::now();
 
-    while (positionMissionFlag && ros::ok())
+    std::vector<float> currentPos
     {
-      // let topics update
-      ros::spinOnce();
+      static_cast<float>(ex_pos.translation.x),
+      static_cast<float>(ex_pos.translation.y),
+      static_cast<float>(ex_pos.translation.z)
+    };
 
-      dt = (ros::WallTime::now() - previousWallTime).toNSec()*1e-9;
-      g = 1/(1 + kd*dt + kp*dt*dt);
-      ksg = kp*g;
-      kdg = (kd + kp*dt)*g;
-      previousWallTime = ros::WallTime::now();
+    std::vector<float> currentVel
+    {
+      static_cast<float>(ex_pos.translation.x - previous_ex_pos.translation.x),
+      static_cast<float>(ex_pos.translation.y - previous_ex_pos.translation.y),
+      static_cast<float>(ex_pos.translation.z - previous_ex_pos.translation.z)
+    };
 
-      std::vector<float> currentPos
-      {
-        static_cast<float>(ex_pos.translation.x),
-        static_cast<float>(ex_pos.translation.y),
-        static_cast<float>(ex_pos.translation.z)
-      };
+    std::vector<float> targetPos;
+    std::vector<float> targetVel;
+    float targetYaw;
 
-      std::vector<float> currentVel
-      {
-        static_cast<float>(ex_pos.translation.x - previous_ex_pos.translation.x),
-        static_cast<float>(ex_pos.translation.y - previous_ex_pos.translation.y),
-        static_cast<float>(ex_pos.translation.z - previous_ex_pos.translation.z)
-      };
+    // check expected velocity verses actual velocity
+    if (Magnitude3(velocityCommand) != 0.0 && Magnitude3(currentVel) != 0.0)
+    {
+      float ang = flatAngleBetween(velocityCommand, currentVel);
+      updateAngleScaler(ang);
+      ROS_INFO("new angle discrepancy!: %f", angleOffset);
+    }
 
-      std::vector<float> targetPos
+    // next depends on what mode we're in ------------------------------------
+    if (positionPlaybackParam)
+    {
+      targetPos = std::vector<float>
       {
         static_cast<float>(track_pos.translation.x),
         static_cast<float>(track_pos.translation.y),
         static_cast<float>(track_pos.translation.z)
       };
 
-      std::vector<float> targetVel
+      targetVel = std::vector<float>
       {
         static_cast<float>(track_pos.translation.x - previous_track_pos.translation.x),
         static_cast<float>(track_pos.translation.y - previous_track_pos.translation.y),
         static_cast<float>(track_pos.translation.z - previous_track_pos.translation.z)
       };
 
-      // if (!rotationMatrixCalibratedFlag) // could also do this before loop?
-      // {
-      //   // run rotation matrix calc
-      //   std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
-      //   std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
-      //   calibratePositionReference(requestedPosition, actualPosition);
-      //   rotationMatrixCalibratedFlag = true;
-      // }
-
-      // float targetYaw = applyRotationAngle(trackingYaw); // might need to use different axis here, bc rotations
-      float targetYaw = trackingYaw; // might need to use different axis here, bc rotations
-      float currentYaw = feedbackYaw;
-      ROS_INFO("Current postion %f, %f, %f", currentPos[0], currentPos[1], currentPos[2]);
-      ROS_INFO("Target position %f, %f, %f", targetPos[0], targetPos[1], targetPos[2]);
-      ROS_INFO("Target yaw: %f", targetYaw);
-      ROS_INFO("Current yaw: %f", currentYaw);
-      ROS_INFO("Elapsed frame time: %f", static_cast<float>(dt));
-
-      float yawCommand = normalizeAngle(currentYaw - targetYaw);
-      // yawCommand = 0.0; // HACK
-      // check dir we should go
-      ROS_INFO("yaw command: %f", yawCommand);
-
-      // convert both target and current position, and move between
-      // std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
-      // std::vector<float> targetVelMapped = applyRotationMatrixScaling(targetVel);
-      // std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
-      // std::vector<float> currentVelMapped = applyRotationMatrixScaling(currentVel);
-      // map yaw command (in deg) to rotation offset (in rads)
-
-      // std::vector<float> moveCommand
-      // {
-      //   targetPosMapped[0] - currentPosMapped[0],
-      //   targetPosMapped[1] - currentPosMapped[1],
-      //   targetPosMapped[2] - currentPosMapped[2]
-      // };
-
-      // inverse 2nd order controller edits!
-      // need: ksg, kdg, kp, kd, g, dt, and finally F
-
-      std::vector<float> force
-      {
-        // using commanded velocity instead of actual
-        static_cast<float>(((targetPos[0] - currentPos[0])*ksg + (targetVel[0] - velocityCommand[0])*kdg)*speedScalarParam),
-        static_cast<float>(((targetPos[1] - currentPos[1])*ksg + (targetVel[1] - velocityCommand[1])*kdg)*speedScalarParam),
-        static_cast<float>(((targetPos[2] - currentPos[2])*ksg + (targetVel[2] - velocityCommand[2])*kdg)*speedScalarParam)
-      };
-
-      ROS_INFO("target velocity: %f, %f, %f", targetVel[0], targetVel[1], targetVel[2]);
-      ROS_INFO("current velocity: %f, %f, %f", currentVel[0], currentVel[1], currentVel[2]);
-
-      // go to position
-      // ROS_INFO("calling move command %f, %f, %f", moveCommand[0], moveCommand[1], moveCommand[2]);
-
-      velocityCommand = std::vector<float> {velocityCommand[0] + dt*force[0], velocityCommand[1] + dt*force[1], velocityCommand[2] + dt*force[2]};
-      // if (Magnitude3(velocityCommand) > maxSpeedParam)
-      // {
-      //   ROS_INFO("bad force val %f, %f, %f", force[0], force[1], force[2]);
-      //   std::vector<float> tempVelocity = normalizeVector(velocityCommand);
-      //   velocityCommand = std::vector<float> {static_cast<float>(maxSpeedParam*tempVelocity[0]), static_cast<float>(maxSpeedParam*tempVelocity[1]), static_cast<float>(maxSpeedParam*tempVelocity[2])};
-      // }
-      if (Magnitude3(velocityCommand) > maxSpeedParam)
-      {
-        velocityCommand = normalizeVector(velocityCommand);
-        velocityCommand = std::vector<float> {maxSpeedParam*velocityCommand[0], maxSpeedParam*velocityCommand[1], maxSpeedParam*velocityCommand[2]};
-        ROS_INFO("normalizing velocity command...");
-      }
-      ROS_INFO("calling vel command %f, %f, %f", velocityCommand[0], velocityCommand[1], velocityCommand[2]);
-      relativePosition(velocityCommand[0], velocityCommand[1], velocityCommand[2], yawCommand);
-
-      // updateRate.sleep();
-
-      // check if position call was correct in terms of distance TODO also do this process for the rotation matrix
-      // ---------------------------------
-          // std::vector<float> calCheckPosition
-          // {
-          //   static_cast<float>(ex_pos.translation.x),
-          //   static_cast<float>(ex_pos.translation.y),
-          //   static_cast<float>(ex_pos.translation.z)
-          // };
-          // // what the distance actually was in ref coords
-          // float refDist = eucDistance(currentPos, calCheckPosition);
-          // // what the distance was in command coords
-          // float cmdDist = Magnitude3(force);
-          // if (cmdDist > stdMoveDist)
-          // {
-          //   cmdDist = stdMoveDist;
-          // }
-          // updateMagnitudeScaler(refDist/cmdDist);
+      targetYaw = trackingYaw;
     }
-  }
-  else
-  {
-    std::vector<float> velocityCommand {0.0, 0.0, 0.0};
-    // executing a set of positions in reference
-    int numPositions;
-    ros::param::get("/robot_list/robot_" + to_string(droneId) + "/numPositions", numPositions);
-    int currentPositionIndex = 0; // will increment
-    ros::Rate updateRate(updateRateParam); // update period for topics/commands
-    positionMissionFlag = true;
-
-    while (positionMissionFlag && ros::ok())
+    else
     {
-      // let topics update
-      ros::spinOnce();
-
-      dt = (ros::WallTime::now() - previousWallTime).toNSec()*1e-9;
-      g = 1/(1 + kd*dt + kp*dt*dt);
-      ksg = kp*g;
-      kdg = (kd + kp*dt)*g;
-      previousWallTime = ros::WallTime::now();
-
-      std::vector<float> currentPos
-      {
-        static_cast<float>(ex_pos.translation.x),
-        static_cast<float>(ex_pos.translation.y),
-        static_cast<float>(ex_pos.translation.z)
-      };
-
-      std::vector<float> currentVel
-      {
-        static_cast<float>(ex_pos.translation.x - previous_ex_pos.translation.x),
-        static_cast<float>(ex_pos.translation.y - previous_ex_pos.translation.y),
-        static_cast<float>(ex_pos.translation.z - previous_ex_pos.translation.z)
-      };
-
       std::vector<float> tempPos;
       ros::param::get("/robot_list/robot_" + to_string(droneId) + "/positions/position_" + to_string(currentPositionIndex), tempPos);
       // pull out x,y,z
-      std::vector<float> targetPos
-      {
+      targetPos = std::vector<float> {
         tempPos[1],
         tempPos[2],
         tempPos[3]
       };
 
-      std::vector<float> targetVel {0.0, 0.0, 0.0};
-
-      // if (!rotationMatrixCalibratedFlag) // could also do this before loop?
-      // {
-      //   // run rotation matrix calc
-      //   std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
-      //   std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
-      //   calibratePositionReference(requestedPosition, actualPosition);
-      //   rotationMatrixCalibratedFlag = true;
-      // }
-
-      // may need to rotate this yaw to match coordinate system
-      float targetYaw = normalizeAngle((M_PI/180.0)*tempPos[4]);
-      float currentYaw = feedbackYaw;
-      ROS_INFO("current postion %f, %f, %f", currentPos[0], currentPos[1], currentPos[2]);
-      ROS_INFO("target position %f, %f, %f", targetPos[0], targetPos[1], targetPos[2]);
-      ROS_INFO("target yaw: %f", targetYaw);
-      ROS_INFO("current yaw: %f", currentYaw);
-
-      float yawCommand = normalizeAngle(currentYaw - targetYaw);
-      // check dir we should go
-      ROS_INFO("yaw command: %f", yawCommand);
-
-      // convert both target and current position, and move between
-      // ROS_INFO("applying rotation matrix scaling...");
-      // std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
-      // std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
-      // ROS_INFO("successfully applied rotation matrix");
-      // map yaw command (in deg) to rotation offset (in rads)
-      // float targetYawMapped = applyRotationAngle(targetYaw); // this assumes much, like that we're rotating about the pos z axis
-
-      std::vector<float> force
-      {
-        // using commanded velocity instead of actual
-        static_cast<float>(((targetPos[0] - currentPos[0])*ksg + (targetVel[0] - velocityCommand[0])*kdg)*speedScalarParam),
-        static_cast<float>(((targetPos[1] - currentPos[1])*ksg + (targetVel[1] - velocityCommand[1])*kdg)*speedScalarParam),
-        static_cast<float>(((targetPos[2] - currentPos[2])*ksg + (targetVel[2] - velocityCommand[2])*kdg)*speedScalarParam)
-      };
-
-      ROS_INFO("target velocity: %f, %f, %f", targetVel[0], targetVel[1], targetVel[2]);
-      ROS_INFO("current velocity: %f, %f, %f", currentVel[0], currentVel[1], currentVel[2]);
+      targetVel = std::vector<float> {0.0, 0.0, 0.0};
 
-      // go to position
-      // ROS_INFO("calling move command %f, %f, %f", moveCommand[0], moveCommand[1], moveCommand[2]);
-
-      velocityCommand = std::vector<float> {velocityCommand[0] + dt*force[0], velocityCommand[1] + dt*force[1], velocityCommand[2] + dt*force[2]};
-      // if (Magnitude3(velocityCommand) > maxSpeedParam)
-      // {
-      //   ROS_INFO("bad force val %f, %f, %f", force[0], force[1], force[2]);
-      //   std::vector<float> tempVelocity = normalizeVector(velocityCommand);
-      //   velocityCommand = std::vector<float> {static_cast<float>(maxSpeedParam*tempVelocity[0]), static_cast<float>(maxSpeedParam*tempVelocity[1]), static_cast<float>(maxSpeedParam*tempVelocity[2])};
-      // }
-      if (Magnitude3(velocityCommand) > maxSpeedParam)
-      {
-        velocityCommand = normalizeVector(velocityCommand);
-        velocityCommand = std::vector<float> {maxSpeedParam*velocityCommand[0], maxSpeedParam*velocityCommand[1], maxSpeedParam*velocityCommand[2]};
-        ROS_INFO("normalizing velocity command...");
-      }
-      ROS_INFO("calling vel command %f, %f, %f", velocityCommand[0], velocityCommand[1], velocityCommand[2]);
-      relativePosition(velocityCommand[0], velocityCommand[1], velocityCommand[2], yawCommand);
+      targetYaw = normalizeAngle((M_PI/180.0)*tempPos[4]);
 
       // check if we're close to position
       float checkDist = eucDistance(currentPos, targetPos);
@@ -690,30 +533,41 @@ bool runPositionMission()
         positionMissionFlag = false;
       }
 
-      // updateRate.sleep();
-      // check if position call was correct in terms of distance TODO also do this process for the rotation matrix
-      // ---------------------------------
-      // std::vector<float> calCheckPosition
-      // {
-      //   static_cast<float>(ex_pos.translation.x),
-      //   static_cast<float>(ex_pos.translation.y),
-      //   static_cast<float>(ex_pos.translation.z)
-      // };
-      // // what the distance actually was in ref coords
-      // float refDist = eucDistance(currentPos, calCheckPosition);
-      // // what the distance was in command coords
-      // float cmdDist = Magnitude3(moveCommand);
-      // if (cmdDist > stdMoveDist)
-      // {
-      //   cmdDist = stdMoveDist;
-      // }
-      // // if movecommand, mapped to coords, does not match up with difference in position, scream
-      //
-      // updateMagnitudeScaler(refDist/cmdDist);
-      // // ---------------------------------
     }
-  }
 
+    float currentYaw = feedbackYaw;
+    ROS_INFO("Current postion %f, %f, %f", currentPos[0], currentPos[1], currentPos[2]);
+    ROS_INFO("Target position %f, %f, %f", targetPos[0], targetPos[1], targetPos[2]);
+    ROS_INFO("Target yaw: %f", targetYaw);
+    ROS_INFO("Current yaw: %f", currentYaw);
+    ROS_INFO("Elapsed frame time: %f", static_cast<float>(dt));
+
+    float yawCommand = normalizeAngle(currentYaw - targetYaw);
+    ROS_INFO("yaw command: %f", yawCommand);
+
+    std::vector<float> force
+    {
+      // using commanded velocity instead of actual
+      static_cast<float>(((targetPos[0] - currentPos[0])*ksg + (targetVel[0] - velocityCommand[0])*kdg)*speedScalarParam),
+      static_cast<float>(((targetPos[1] - currentPos[1])*ksg + (targetVel[1] - velocityCommand[1])*kdg)*speedScalarParam),
+      static_cast<float>(((targetPos[2] - currentPos[2])*ksg + (targetVel[2] - velocityCommand[2])*kdg)*speedScalarParam)
+    };
+
+    ROS_INFO("target velocity: %f, %f, %f", targetVel[0], targetVel[1], targetVel[2]);
+    ROS_INFO("current velocity: %f, %f, %f", currentVel[0], currentVel[1], currentVel[2]);
+
+    velocityCommand = std::vector<float> {velocityCommand[0] + dt*force[0], velocityCommand[1] + dt*force[1], velocityCommand[2] + dt*force[2]};
+
+    if (Magnitude3(velocityCommand) > maxSpeedParam)
+    {
+      velocityCommand = normalizeVector(velocityCommand);
+      velocityCommand = std::vector<float> {maxSpeedParam*velocityCommand[0], maxSpeedParam*velocityCommand[1], maxSpeedParam*velocityCommand[2]};
+      ROS_INFO("normalizing velocity command...");
+    }
+    velocityCommand = MatrixMultiply(rotateAxisAngle(std::vector<float> {0.0, 0.0, 1.0}, angleOffset), velocityCommand);
+    ROS_INFO("calling vel command %f, %f, %f", velocityCommand[0], velocityCommand[1], velocityCommand[2]);
+    expectedVel = relativePosition(velocityCommand[0], velocityCommand[1], velocityCommand[2], yawCommand);
+  }
   return true;
 }
 
@@ -854,37 +708,15 @@ bool land()
   return flightTaskControl.response.result;
 }
 
-
-// converts local yaw in rads to mapped yaw in rads
-float applyRotationAngle(float localYaw)
-{
-  if (!rotationMatrixCalibratedFlag) // could also do this before loop?
-  {
-    // run rotation matrix calc
-    std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
-    std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
-    calibratePositionReference(requestedPosition, actualPosition);
-    rotationMatrixCalibratedFlag = true;
-  }
-  return localYaw + poseRotationRadians;
-}
-
-bool relativePosition(float x, float y, float z, float yaw)
+std::vector<float> relativePosition(float x, float y, float z, float yaw)
 {
   // convert relative position to velocity command
   std::vector<float> posVec {x, y, z};
-  float totalDist = Magnitude3(posVec);
+  // TESTING SMALL ANGLE ERROR IN PLACEMENT OF DRONE
+  // ------------------------------
+  // posVec = MatrixMultiply(rotateAxisAngle(std::vector<float> {0.0, 0.0, 1.0}, (M_PI/180.0)*180.0), posVec);
 
-  // normalize maximum distance to a parameter, say 1m steps so we can do position updates as we go
-  // if (totalDist > stdMoveDist)
-  // {
-  //   // normalize the move to be of std length
-  //   std::vector<float> normPos = normalizeVector(posVec);
-  //   posVec[0] = normPos[0]*stdMoveDist;
-  //   posVec[1] = normPos[1]*stdMoveDist;
-  //   posVec[2] = normPos[2]*stdMoveDist;
-  //   totalDist = stdMoveDist;
-  // }
+  float totalDist = Magnitude3(posVec);
 
   float speed;
   float totalTimeMs;
@@ -938,7 +770,8 @@ bool relativePosition(float x, float y, float z, float yaw)
   flightTaskControl.request.yawThresholdInDeg = 1;
   flight_control_client.call(flightTaskControl); // actually waits for drone to "reach" position
 
-  return flightTaskControl.response.result;
+  // return flightTaskControl.response.result;
+  return std::vector<float> {(totalTimeMs/1000.0)*velVec[0], (totalTimeMs/1000.0)*velVec[1], (totalTimeMs/1000.0)*velVec[2]};
 }
 
 bool stopMotion()
@@ -968,7 +801,21 @@ bool goHome()
 }
 
 
-void updateMagnitudeScaler(float newValue)
+// void updateMagnitudeScaler(float newValue)
+// {
+//   // remove the last value and insert new one
+//   calibrationWindowValues.pop_back();
+//   auto firstIndex = calibrationWindowValues.begin();
+//   calibrationWindowValues.insert(firstIndex, newValue);
+//   float sumer = 0.0;
+//   for (int i = 0; i < calibrationWindowValues.size(); i++)
+//   {
+//     sumer += calibrationWindowValues[i];
+//   }
+//   poseMagnitudeScaler = sumer/calibrationWindowValues.size();
+// }
+
+void updateAngleScaler(float newValue)
 {
   // remove the last value and insert new one
   calibrationWindowValues.pop_back();
@@ -979,7 +826,16 @@ void updateMagnitudeScaler(float newValue)
   {
     sumer += calibrationWindowValues[i];
   }
-  poseMagnitudeScaler = sumer/calibrationWindowValues.size();
+  // check for first iteration, everything else is zero
+  // if (sumer == newValue)
+  // {
+  //   // fill up with first value??
+  //   for (int i = 0; i < calibrationWindowValues.size(); i++)
+  //   {
+  //     calibrationWindowValues[i] = newValue;
+  //   }
+  // }
+  angleOffset = sumer/calibrationWindowValues.size();
 }
 
 void calibratePositionReference(std::vector<float> requestedPosition, std::vector<float> actualPosition)
@@ -1005,18 +861,6 @@ void calibratePositionReference(std::vector<float> requestedPosition, std::vecto
   ROS_INFO("pose magnitude scalar %f", poseMagnitudeScaler);
 }
 
-std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec)
-{
-  std::vector<float> rotatedVec = MatrixMultiply(poseRotationMatrix, inputVec);
-  std::vector<float> outputVec
-  {
-    rotatedVec[0]*poseMagnitudeScaler,
-    rotatedVec[1]*poseMagnitudeScaler,
-    rotatedVec[2]*poseMagnitudeScaler
-  };
-  return outputVec;
-}
-
 // bool add(beginner_tutorials::AddTwoInts::Request &req, beginner_tutorials::AddTwoInts::Response &res)
 bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res)
 {
@@ -1072,15 +916,15 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
       takeoff();
       break;
     }
-    case 7:
-    {
-      ROS_INFO("received overwatch: calibrate pos ref");
-
-      std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
-      std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
-      calibratePositionReference(requestedPosition, actualPosition);
-      break;
-    }
+    // case 7:
+    // {
+    //   ROS_INFO("received overwatch: calibrate pos ref");
+    //
+    //   std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
+    //   std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
+    //   calibratePositionReference(requestedPosition, actualPosition);
+    //   break;
+    // }
     default:
       break;
   }
@@ -1089,26 +933,6 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
   return res.result;
 }
 
-std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec)
-{
-  // move a bit and return diff in position
-  //ros::spinOnce(); // let callbacks update
-  geometry_msgs::Transform preT = ex_pos;
-  relativePosition(calibrationVec[0], calibrationVec[1], calibrationVec[2], 0.0);
-  ros::spinOnce(); // let callbacks update
-  geometry_msgs::Transform postT = ex_pos;
-  std::vector<float> deltaPosition // what we got
-  {
-    static_cast<float>(postT.translation.x) - static_cast<float>(preT.translation.x),
-    static_cast<float>(postT.translation.y) - static_cast<float>(preT.translation.y),
-    static_cast<float>(postT.translation.z) - static_cast<float>(preT.translation.z),
-  };
-  // go back
-  relativePosition(-calibrationVec[0], -calibrationVec[1], -calibrationVec[2], 0.0);
-  ROS_INFO("delta position from calibration: %f, %f, %f", deltaPosition[0], deltaPosition[1], deltaPosition[2]);
-  return deltaPosition;
-}
-
 int main(int argc, char** argv)
 {
 
@@ -1129,6 +953,13 @@ int main(int argc, char** argv)
   ros::param::get("controller_frequency", frequency);
   kp = (6.0*frequency)*(6.0*frequency)* 0.25;
   kd = 4.5*frequency*damping;
+  expectedVel = std::vector<float> {0.0, 0.0, 0.0};
+  angleOffset = 0.0;
+  calibrationWindowValues = std::vector<float> {};
+  for (int i = 0; i < calibrationWindow; i++)
+  {
+    calibrationWindowValues.push_back(0.0);
+  }
   // kp = (18.0*frequency*frequency)/2.0;
   // kd = (9.0/2.0)*frequency*damping;
 
@@ -1150,8 +981,8 @@ int main(int argc, char** argv)
 
   syncPub = nh.advertise<std_msgs::Float32>("sync_signal", 1);
   gps_pos_subscriber = nh.subscribe<sensor_msgs::NavSatFix>("dji_osdk_ros/gps_position", 1, &gpsPosCallback);
-  pos_feedback_subscriber = nh.subscribe<geometry_msgs::Transform>("feedback/pose_feedback", 1, &posFeedbackCallback);
-  pos_tracking_subscriber = nh.subscribe<geometry_msgs::Transform>("tracking/pose_feedback", 1, &posTrackingCallback);
+  pos_feedback_subscriber = nh.subscribe<geometry_msgs::Transform>("feedback", 1, &posFeedbackCallback);
+  pos_tracking_subscriber = nh.subscribe<geometry_msgs::Transform>("tracking", 1, &posTrackingCallback);
 
   ros::ServiceServer overwatchService = nh.advertiseService("overwatch", overwatchFunction);