To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 09ef0e47 authored by Lars's avatar Lars
Browse files

Rotation fixed

parent e1c93f59
......@@ -83,8 +83,8 @@ from imutils.video import VideoStream
MARKER_SIZE = 180.0 #size in mm
CAMERA = 0 #which Port is the Reciever connected to
PMKF = True #Do you want a PointMassKalmanFilter
TEXT_OUTPUT = False #Do you want text output of the raw data
TEXT_OUTPUT = True #Do you want text output of the raw data
TIMEDELAY = 0.1
#OLD_STUFF
......@@ -135,23 +135,26 @@ if TEXT_OUTPUT:
outr2 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/r2.txt', 'w')
if PMKF: #PointMassKalmanFilter
Ahat_t = np.matrix([[0.8780690906,0.0292689697],[-0.2103665025,0.9929877833]])
Kinf_t = np.matrix([[0.1219309094],[0.2103665025]])
Ahat_t = np.matrix([[0.6186416433,0.0206213881],[-1.8440583718,0.9385313876]])
Kinf_t = np.matrix([[0.3813583567],[1.8440583718]])
Ahat_r = np.matrix([[0.9313707507,0.0310456917],[-0.0683245284,0.9977225157]])
Kinf_r = np.matrix([[0.0686292493],[0.0683245284]])
def pmkf(state_old, input, type): #type = True for trans, False for rot
state = np.matrix([[0],[0]])
if type == True:
return Ahat_t*state_old+ Kinf_t*input
state = Ahat_t*state_old+ Kinf_t*input
return state, state[0] + TIMEDELAY*state[1]
else:
return Ahat_r*state_old+ Kinf_r*input
state = Ahat_r*state_old+ Kinf_r*input
return state, state[0] + TIMEDELAY*state[1]
state_t = [np.matrix([[0],[0]]),np.matrix([[0],[0]]),np.matrix([[0],[0]])]
state_r = [np.matrix([[0],[0]]),np.matrix([[0],[0]]),np.matrix([[0],[0]])]
def fixRot(rot):
if rot<= 0:
rot = rot + math.pi
if rot[0]<= 0:
rot[0] = rot[0] + math.pi
#rot[2] = -rot[2]
else:
rot = rot -math.pi
rot[0] = rot[0] -math.pi
return rot
......@@ -160,8 +163,8 @@ if PMKF: #PointMassKalmanFilter
#initialize font
fontface = cv2.FONT_HERSHEY_SIMPLEX
fontscale = 0.5
fontcolor = (255, 255, 255)
fontscale = 0.6
fontcolor = (255, 0, 255)
framecount = 0
data_out = np.array([])
......@@ -231,14 +234,13 @@ while(cam.isOpened()):
#PointMassKalmanFilter
if PMKF:
print('before: ' + str(rvec[0]))
rvec[0] = fixRot(rvec[0])
print('after: ' + str(rvec[0]))
rvec = fixRot(rvec)
for i in range(0,3):
state_t[i] = pmkf(state_t[i], tvec[i], True) #translation
state_r[i] = pmkf(state_r[i], rvec[i], False) #rotation
tvec[i] = state_t[i][0]
rvec[i] = state_r[i][0]
state_t[i], tvec[i] = pmkf(state_t[i], tvec[i], True) #translation
state_r[i],rvec[i] = pmkf(state_r[i], rvec[i], False) #rotation
rvec = fixRot(rvec)
R = cv2.Rodrigues(rvec)
if TEXT_OUTPUT:
outt0.write(str(tvec[0]))
outt1.write(str(tvec[1]))
......@@ -246,13 +248,20 @@ while(cam.isOpened()):
outr0.write(str(rvec[0]))
outr1.write(str(rvec[1]))
outr2.write(str(rvec[2]))
#print(R)
#publish
cfcamera_pub.publish(rvec[0],rvec[1],rvec[2],tvec[0]/1100,tvec[1]/1000,tvec[2]/2050)
cfcamera_pub.publish(rvec[0],rvec[1],rvec[2],tvec[0]/1100,tvec[1]/1000,tvec[2]/2050, R[0][0][0],R[0][0][1],R[0][0][2],R[0][1][0],R[0][1][1],R[0][1][2],R[0][2][0],R[0][2][1],R[0][2][2])
#print to screen
outputr = 'r: '+str(rvec)
outputt = 't: '+str(tvec)
textR1 = 'R '+ str(R[0][0])
textR2 = 'R '+str(R[0][1])
textR3 = 'R '+str(R[0][2])
cv2.putText(QueryImg, outputr, (0,20), fontface, fontscale, fontcolor)
cv2.putText(QueryImg, outputt, (0,50), fontface, fontscale, fontcolor)
cv2.putText(QueryImg, textR1, (0,80), fontface, fontscale, fontcolor)
cv2.putText(QueryImg, textR2, (0,100), fontface, fontscale, fontcolor)
cv2.putText(QueryImg, textR3, (0,120), fontface, fontscale, fontcolor)
if TEXT_OUTPUT:
outt0.write('\n')
outt1.write('\n')
......
......@@ -4,4 +4,12 @@ float64 yaw
float64 x
float64 y
float64 z
float64 R11
float64 R12
float64 R13
float64 R21
float64 R22
float64 R23
float64 R31
float64 R32
float64 R33
......@@ -2,11 +2,11 @@
battery_polling_period: 200
# Battery thresholds while in the "motors off" state, in [Volts]
battery_voltage_threshold_lower_while_standby: 3.30
battery_voltage_threshold_lower_while_standby: 3.60
battery_voltage_threshold_upper_while_standby: 4.20
# Battery thresholds while in the "flying" state, in [Volts]
battery_voltage_threshold_lower_while_flying: 2.80
battery_voltage_threshold_lower_while_flying: 3.20
battery_voltage_threshold_upper_while_flying: 3.70
# Delay before changing the state of the battery, in [number of measurements]
......
# Mass of the crazyflie
mass : 28
mass : 36
# Frequency of the controller, in hertz
control_frequency : 200
......@@ -9,7 +9,7 @@ motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
# The min and max for saturating 16 bit thrust commands
command_sixteenbit_min : 1000
command_sixteenbit_max : 60000
command_sixteenbit_max : 54000
# The default setpoint, the ordering is (x,y,z,yaw),
# with unit [meters,meters,meters,radians]
......@@ -22,7 +22,11 @@ shouldPublishDebugMessage : false
shouldDisplayDebugInfo : false
# The LQR Controller parameters for rate mode
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
gainMatrixRollRate : [ 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00]
gainMatrixPitchRate : [ 6.20, 0.00, 0.00, 5.00, 0.00, 0.00, 0.00, 5.20, 0.00] #half of original
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.69, 0.00, 0.00, 0.27, 0.00, 0.00, 0.00]
gainMatrixRollRate : [ 0.00,-4.39, 0.00, 0.00,-2.69, 0.00, 6.99, 0.00, 0.00]
gainMatrixPitchRate : [ 4.39, 0.00, 0.00, 2.69, 0.00, 0.00, 0.00, 6.99, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 6.20]
#Maximum difference in setpoint between czcles
maxSetpointDifference : 0.01
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
......@@ -37,8 +37,8 @@
#include "nodes/TemplateControllerService.h"
#include <cmath>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>
......@@ -183,10 +183,11 @@ float xdiff;
float ydiff;
float zdiff;
float yawdiff;
float max_diff;
float actualPos[6] = {0,0,0,0,0,0};
float markerPos[6] = {0,0,0,0,0,0};
bool follow_marker[3] = {false,false,false};
bool follow_marker[4] = {false,false,false,true};
bool calculateControlOutput(Controller::Request &request, Controller::Response
&response) {
......@@ -241,7 +242,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response
while(yawError < -PI) {yawError += 2 * PI;}
// > Third, put the "yawError" into the "stateError" variable
stateErrorInertial[8] = yawError;
//ROS_INFO_STREAM(yawError);
// CONVERSION INTO BODY FRAME
// Conver the state erorr from the Inertial frame into the Body frame
......@@ -707,6 +708,11 @@ void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg)
// LOADING OF THE YAML PARAMTERS
void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
{
// Here we load the parameters that are specified in the file:
// TemplateController.yaml
......@@ -745,6 +751,10 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
// be displayed or not
yaml_shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayDebugInfo");
max_diff = getParameterFloat(nodeHandle_for_paramaters , "maxSetpointDifference");
// The LQR Controller parameters
// The LQR Controller parameters for "LQR_MODE_RATE"
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
......@@ -847,12 +857,24 @@ void CrazyCameraCallback(const CameraData & msg)
rvec_marker_in_cameraframe[0]= msg.pitch;
rvec_marker_in_cameraframe[1]= msg.roll;
rvec_marker_in_cameraframe[2]= msg.yaw;
tvec_marker_in_cameraframe[0]= msg.x;
tvec_marker_in_cameraframe[1]= msg.y;
tvec_marker_in_cameraframe[0]= -msg.x;
tvec_marker_in_cameraframe[1]= -msg.y;
tvec_marker_in_cameraframe[2]= msg.z;
float Rodrigues[3][3];
Rodrigues[0][0]=msg.R11;
Rodrigues[0][1]=msg.R12;
Rodrigues[0][2]=msg.R13;
Rodrigues[1][0]=msg.R21;
Rodrigues[1][1]=msg.R22;
Rodrigues[1][2]=msg.R23;
Rodrigues[2][0]=msg.R31;
Rodrigues[2][1]=msg.R32;
Rodrigues[2][2]=msg.R33;
//fix coordinate system of camera
float R_camera_to_crazyflie[3][3] = {{0,0,1},{-1,0,0},{0,-1,0}};
float R_camera_to_crazyflie[3][3] = {{0,0,1},{1,0,0},{0,1,0}};
......@@ -909,62 +931,86 @@ void CrazyCameraCallback(const CameraData & msg)
tvec_marker_in_viconframe_from_origin[i] = tvec_camera_in_viconframe[i] + tvec_marker_in_viconframe[i];
}
ROS_INFO_STREAM("R: " << R_crazyflie_to_vicon[0][0] << " "<<R_crazyflie_to_vicon[0][1]<< " "<<R_crazyflie_to_vicon[0][2]);
ROS_INFO_STREAM("R: " << R_crazyflie_to_vicon[1][0] << " "<<R_crazyflie_to_vicon[1][1]<< " "<<R_crazyflie_to_vicon[1][2]);
ROS_INFO_STREAM("R: " << R_crazyflie_to_vicon[2][0] << " "<<R_crazyflie_to_vicon[2][1]<< " "<<R_crazyflie_to_vicon[2][2]);
// ROS_INFO_STREAM("R: " << R_crazyflie_to_vicon[0][0] << " "<<R_crazyflie_to_vicon[0][1]<< " "<<R_crazyflie_to_vicon[0][2]);
// ROS_INFO_STREAM("R: " << R_crazyflie_to_vicon[1][0] << " "<<R_crazyflie_to_vicon[1][1]<< " "<<R_crazyflie_to_vicon[1][2]);
// ROS_INFO_STREAM("R: " << R_crazyflie_to_vicon[2][0] << " "<<R_crazyflie_to_vicon[2][1]<< " "<<R_crazyflie_to_vicon[2][2]);
ROS_INFO_STREAM("Marker in vicon: " << tvec_marker_in_viconframe[0] << " \t Marker in camera: " << tvec_marker_in_cameraframe[0]);
ROS_INFO_STREAM("Marker in vicon: " << tvec_marker_in_viconframe[1] << " \t Marker in camera:: " << tvec_marker_in_cameraframe[1]);
ROS_INFO_STREAM("Marker in vicon: " << tvec_marker_in_viconframe[2] << " \t Marker in camera:: " << tvec_marker_in_cameraframe[2]);
ROS_INFO_STREAM(" ");
// ROS_INFO_STREAM("Marker in vicon: " << tvec_marker_in_viconframe[0] << " \t Marker in camera: " << tvec_marker_in_cameraframe[0]);
// ROS_INFO_STREAM("Marker in vicon: " << tvec_marker_in_viconframe[1] << " \t Marker in camera:: " << tvec_marker_in_cameraframe[1]);
// ROS_INFO_STREAM("Marker in vicon: " << tvec_marker_in_viconframe[2] << " \t Marker in camera:: " << tvec_marker_in_cameraframe[2]);
// ROS_INFO_STREAM(" ");
ROS_INFO_STREAM("Dist from camera: " << tvec_marker_in_viconframe[0] << " \t Dist from vicon: " << -tvec_camera_in_viconframe[0]+tvec_marker_in_viconframe_v[0]);
ROS_INFO_STREAM("Dist from camera: " << tvec_marker_in_viconframe[1] << " \t Dist from vicon: " << -tvec_camera_in_viconframe[1]+tvec_marker_in_viconframe_v[1]);
ROS_INFO_STREAM("Dist from camera: " << tvec_marker_in_viconframe[2] << " \t Dist from vicon: " << -tvec_camera_in_viconframe[2]+tvec_marker_in_viconframe_v[2]);
ROS_INFO_STREAM(" ");
// ROS_INFO_STREAM("Dist from camera: " << tvec_marker_in_viconframe[0] << " \t Dist from vicon: " << -tvec_camera_in_viconframe[0]+tvec_marker_in_viconframe_v[0]);
// ROS_INFO_STREAM("Dist from camera: " << tvec_marker_in_viconframe[1] << " \t Dist from vicon: " << -tvec_camera_in_viconframe[1]+tvec_marker_in_viconframe_v[1]);
// ROS_INFO_STREAM("Dist from camera: " << tvec_marker_in_viconframe[2] << " \t Dist from vicon: " << -tvec_camera_in_viconframe[2]+tvec_marker_in_viconframe_v[2]);
// ROS_INFO_STREAM(" ");
ROS_INFO_STREAM("Indirect Marker: " << tvec_marker_in_viconframe_from_origin[0] << " \t Direct Marker: " << tvec_marker_in_viconframe_v[0]);
ROS_INFO_STREAM("Indirect Marker: " << tvec_marker_in_viconframe_from_origin[1] << " \t Direct Marker: " << tvec_marker_in_viconframe_v[1]);
ROS_INFO_STREAM("Indirect Marker: " << tvec_marker_in_viconframe_from_origin[2] << " \t Direct Marker: " << tvec_marker_in_viconframe_v[2]);
ROS_INFO_STREAM(" ");
// ROS_INFO_STREAM("Indirect Marker: " << tvec_marker_in_viconframe_from_origin[0] << " \t Direct Marker: " << tvec_marker_in_viconframe_v[0]);
// ROS_INFO_STREAM("Indirect Marker: " << tvec_marker_in_viconframe_from_origin[1] << " \t Direct Marker: " << tvec_marker_in_viconframe_v[1]);
// ROS_INFO_STREAM("Indirect Marker: " << tvec_marker_in_viconframe_from_origin[2] << " \t Direct Marker: " << tvec_marker_in_viconframe_v[2]);
// ROS_INFO_STREAM(" ");
using namespace std;
ofstream output;
output.open("/home/vision-crazyflie/work/RotationVectors.txt",std::ios::app);
output<< actualPos[3] <<","<< actualPos[4] <<","<< actualPos[5] <<","<< markerPos[3] <<","<< markerPos[4] <<","<< markerPos[5] <<","<< rvec_marker_in_cameraframe[0] <<","<< rvec_marker_in_cameraframe[1] <<","<< rvec_marker_in_cameraframe[2] <<"\n";
output.close();
//define setpoint
float Setpoint_in_arucoframe[3] = {1,0,0};
float Setpoint_in_arucoframe[3] = {0,0,dist};
float Setpoint_in_cameraframe_from_marker[3];
float Setpoint_in_cameraframe_from_camera[3];
float Setpoint_in_viconframe_from_camera[3];
float Setpoint_in_viconframe_from_origin[3];
//TODO calculate rotationmatrix from aruco to camera
// calculate rotation from marker to camera
//TODO fix angles from rvec to rvec - atan...
a=rvec_marker_in_cameraframe[0];
b=rvec_marker_in_cameraframe[1];
c=rvec_marker_in_cameraframe[2];//+3.1415;
c=rvec_marker_in_cameraframe[2];// + atan(tvec_marker_in_cameraframe[0]/tvec_marker_in_cameraframe[2]);
float R_aruco_to_camera[3][3];
ROS_INFO_STREAM(" atan: " <<atan(tvec_marker_in_cameraframe[1]/tvec_marker_in_cameraframe[2]) << "\t rvec: " << rvec_marker_in_cameraframe[0]);
//claculate the setpoint
Setpoint_in_cameraframe_from_marker[0] = dist*sin(c);
Setpoint_in_cameraframe_from_marker[1] = dist*sin(a);
Setpoint_in_cameraframe_from_marker[2] = -dist*cos(a)*cos(c);
//claculate the setpoint
// Setpoint_in_cameraframe_from_marker[0] = dist*sin(c)*sin(a);
// Setpoint_in_cameraframe_from_marker[1] = -dist*sin(a);
// Setpoint_in_cameraframe_from_marker[2] = dist*cos(-a)*cos(c);
float RodriguesKorr[3][3] = {{-1,0,0},{0,-1,0},{0,0,1}};
float Setpoint_in_cameraframe_from_marker_wrong[3];
mult3x1(Rodrigues,Setpoint_in_arucoframe , Setpoint_in_cameraframe_from_marker_wrong);
mult3x1(RodriguesKorr,Setpoint_in_cameraframe_from_marker_wrong,Setpoint_in_cameraframe_from_marker);
for(int i =0;i<3;i++){
Setpoint_in_cameraframe_from_camera[i] = Setpoint_in_cameraframe_from_marker[i]+tvec_marker_in_cameraframe[i];
}
mult3x1(R_camera_to_vicon, Setpoint_in_cameraframe_from_camera, Setpoint_in_viconframe_from_camera);
float Setpoint_in_crazyflieframe[3];
mult3x1(R_camera_to_crazyflie,Setpoint_in_cameraframe_from_camera, Setpoint_in_crazyflieframe);
mult3x1(R_crazyflie_to_vicon, Setpoint_in_crazyflieframe, Setpoint_in_viconframe_from_camera);
for(int i = 0; i<3 ; i++){
Setpoint_in_viconframe_from_origin[i] = Setpoint_in_viconframe_from_camera[i] + tvec_camera_in_viconframe[i];
}
Setpoint_in_viconframe_from_origin[2] += 0.1;
ROS_INFO_STREAM("Setpoint in camera from m: " << Setpoint_in_cameraframe_from_marker[0]);
ROS_INFO_STREAM("Setpoint in camera from m: " << Setpoint_in_cameraframe_from_marker[1]);
ROS_INFO_STREAM("Setpoint in camera from m: " << Setpoint_in_cameraframe_from_marker[2]);
ROS_INFO_STREAM("Setpoint in camera from m: " << Setpoint_in_cameraframe_from_marker[0]<< " \t tvec_marker_in_cameraframe: " << tvec_marker_in_cameraframe[0]);
ROS_INFO_STREAM("Setpoint in camera from m: " << Setpoint_in_cameraframe_from_marker[1]<< " \t tvec_marker_in_cameraframe: " << tvec_marker_in_cameraframe[1]);
ROS_INFO_STREAM("Setpoint in camera from m: " << Setpoint_in_cameraframe_from_marker[2]<< " \t tvec_marker_in_cameraframe: " << tvec_marker_in_cameraframe[2]);
ROS_INFO_STREAM(" ");
ROS_INFO_STREAM("Setpoint in cf from cf: " << Setpoint_in_crazyflieframe[0]<< " \t tvec_marker_in_cameraframe: " << tvec_marker_in_cameraframe[0]);
ROS_INFO_STREAM("Setpoint in cf from cf: " << Setpoint_in_crazyflieframe[1]<< " \t tvec_marker_in_cameraframe: " << tvec_marker_in_cameraframe[1]);
ROS_INFO_STREAM("SSetpoint in cf from cf: " << Setpoint_in_crazyflieframe[2]<< " \t tvec_marker_in_cameraframe: " << tvec_marker_in_cameraframe[2]);
ROS_INFO_STREAM(" ");
ROS_INFO_STREAM("Setpoint in camera from c: " << Setpoint_in_cameraframe_from_camera[0] << " \t Setpoint in vicon from c: " << Setpoint_in_viconframe_from_camera[0]);
......@@ -972,13 +1018,20 @@ void CrazyCameraCallback(const CameraData & msg)
ROS_INFO_STREAM("Setpoint in camera from c: " << Setpoint_in_cameraframe_from_camera[2] << " \t Setpoint in vicon from c: " << Setpoint_in_viconframe_from_camera[2]);
ROS_INFO_STREAM(" ");
ROS_INFO_STREAM("Setpoint in viconframe from o " << Setpoint_in_viconframe_from_origin[0] << " \t markerpos: " << tvec_marker_in_viconframe_v[0]);
ROS_INFO_STREAM("Setpoint in viconframe from o: " << Setpoint_in_viconframe_from_origin[1] << " \t markerpos: " << tvec_marker_in_viconframe_v[1]);
ROS_INFO_STREAM("Setpoint in viconframe from o: " << Setpoint_in_viconframe_from_origin[2] << " \t markerpos: " << tvec_marker_in_viconframe_v[2]);
ROS_INFO_STREAM(" ");
// ROS_INFO_STREAM("Setpoint in viconframe from o " << Setpoint_in_viconframe_from_origin[0] << " \t markerpos: " << tvec_marker_in_viconframe_v[0]);
// ROS_INFO_STREAM("Setpoint in viconframe from o: " << Setpoint_in_viconframe_from_origin[1] << " \t markerpos: " << tvec_marker_in_viconframe_v[1]);
// ROS_INFO_STREAM("Setpoint in viconframe from o: " << Setpoint_in_viconframe_from_origin[2] << " \t markerpos: " << tvec_marker_in_viconframe_v[2]);
// ROS_INFO_STREAM(" ");
float yaw_from_camera=atan(-tvec_marker_in_cameraframe[0]/tvec_marker_in_cameraframe[2]);
//ROS_INFO_STREAM("Yaw from camera to marker"<<yaw_from_camera);
//float Setpoint_yaw = actualPos[3] + rvec_marker_in_cameraframe[2];
float Setpoint_yaw = actualPos[3]-yaw_from_camera;
//ROS_INFO_STREAM("Yaw camera: "<< rvec_marker_in_cameraframe[2]<<"\t Yaw vicon: "<< actualPos[3]);
//max speed
float max_diff = 0.05;
//max speedqq
for (int i=0;i<3;i++){
if(Setpoint_in_viconframe_from_origin[i] - m_setpoint[i] > max_diff) Setpoint_in_viconframe_from_origin[i] = m_setpoint[i]+max_diff;
if(Setpoint_in_viconframe_from_origin[i] - m_setpoint[i] < -max_diff) Setpoint_in_viconframe_from_origin[i] = m_setpoint[i]-max_diff;
......@@ -986,14 +1039,17 @@ void CrazyCameraCallback(const CameraData & msg)
float SetNextPoint[3] = {m_setpoint[0],m_setpoint[1],m_setpoint[2]};
float SetNextPoint[4] = {m_setpoint[0],m_setpoint[1],m_setpoint[2],m_setpoint[3]};
for (int i=0;i<3;i++){
if( follow_marker[i] ){
SetNextPoint[i] = Setpoint_in_viconframe_from_origin[i];
}
}
setNewSetpoint(SetNextPoint[0],SetNextPoint[1],SetNextPoint[2], m_setpoint[3]);
if( follow_marker[3] ){
SetNextPoint[3] = Setpoint_yaw;
}
setNewSetpoint(SetNextPoint[0],SetNextPoint[1],SetNextPoint[2], SetNextPoint[3]);
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment