diff --git a/pps_ws/src/d_fall_pps/include/DemoControllerService.h b/pps_ws/src/d_fall_pps/include/DemoControllerService.h
index 11d52c373c97f4bc11428f83798599b6a75e3736..e3a9d084b64b1603f52482b9425f6e2e187b0f5c 100644
--- a/pps_ws/src/d_fall_pps/include/DemoControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/DemoControllerService.h
@@ -200,10 +200,10 @@ std::vector<float> gainMatrixMotor3 (12,0.0);
std::vector<float> gainMatrixMotor4 (12,0.0);
// The LQR Controller parameters for "LQR_MODE_ACTUATOR"
-std::vector<float> gainMatrixThrust_TwelveStateVector = { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00, 0.00, 0.00, 0.00};
-std::vector<float> gainMatrixRollTorque = { 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00, 0.00, 0.00, 0.00};
-std::vector<float> gainMatrixPitchTorque = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84, 0.00, 0.00, 0.00};
-std::vector<float> gainMatrixYawTorque = { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};
+std::vector<float> gainMatrixThrust_TwelveStateVector (12,0.0);
+std::vector<float> gainMatrixRollTorque (12,0.0);
+std::vector<float> gainMatrixPitchTorque (12,0.0);
+std::vector<float> gainMatrixYawTorque (12,0.0);
// The LQR Controller parameters for "LQR_MODE_RATE"
std::vector<float> gainMatrixThrust_NineStateVector = { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00};
@@ -251,13 +251,13 @@ float stateInterialEstimate_viaPointMassKalmanFilter[12];
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
-std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0)
-std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0)
-std::vector<float> PMKF_Kinf_for_positions (2,0.0)
+std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
+std::vector<float> PMKF_Kinf_for_positions (2,0.0);
// > For the (roll,pitch,yaw) angles
-std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0)
-std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0)
-std::vector<float> PMKF_Kinf_for_angles (2,0.0)
+std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0);
+std::vector<float> PMKF_Kinf_for_angles (2,0.0);
diff --git a/pps_ws/src/d_fall_pps/param/DemoController.yaml b/pps_ws/src/d_fall_pps/param/DemoController.yaml
index 19c54f729164017171432b85ed386f373bc56055..8b0e4f103b8a684465ae62bff87ca4793297f9e3 100644
--- a/pps_ws/src/d_fall_pps/param/DemoController.yaml
+++ b/pps_ws/src/d_fall_pps/param/DemoController.yaml
@@ -50,20 +50,20 @@ lqr_controller_mode : 3
# each of (x,y,z,roll,pitch,yaw)
# 3 - Quad-rotor Model Based Method
# Uses the model of the quad-rotor and the previous inputs
-estimator_method : 1
+estimator_method : 2
# The LQR Controller parameters for "mode = 1"
-gainMatrixMotor1 : [ -0.0620, 0.0620, 0.0496,-0.0445, 0.0445, 0.0282,-0.1567,-0.1568,-0.0688,-0.0063,-0.0063,-0.0130]
-gainMatrixMotor2 : [ 0.0620, 0.0620, 0.0496, 0.0445, 0.0445, 0.0282,-0.1567, 0.1568, 0.0688,-0.0063, 0.0063, 0.0130]
-gainMatrixMotor3 : [ 0.0620,-0.0620, 0.0496, 0.0445,-0.0445, 0.0282, 0.1567, 0.1568,-0.0688, 0.0063, 0.0063,-0.0130]
-gainMatrixMotor4 : [ -0.0620,-0.0620, 0.0496,-0.0445,-0.0445, 0.0282, 0.1567,-0.1568, 0.0688, 0.0063,-0.0063, 0.0130]
+gainMatrixMotor1 : [ -0.0180, 0.0180, 0.0350,-0.0110, 0.0110, 0.0260,-0.0220,-0.0220,-0.0110,-0.00024,-0.00024,-0.00045]
+gainMatrixMotor2 : [ 0.0180, 0.0180, 0.0350, 0.0110, 0.0110, 0.0260,-0.0220, 0.0220, 0.0110,-0.00024, 0.00024, 0.00045]
+gainMatrixMotor3 : [ 0.0180,-0.0180, 0.0350, 0.0110,-0.0110, 0.0260, 0.0220, 0.0220,-0.0110, 0.00024, 0.00024,-0.00045]
+gainMatrixMotor4 : [ -0.0180,-0.0180, 0.0350,-0.0110,-0.0110, 0.0260, 0.0220,-0.0220, 0.0110, 0.00024,-0.00024, 0.00045]
# The LQR Controller parameters for "mode = 2"
-gainMatrixThrust_TwelveStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00]
-gainMatrixRollTorque : [ 0.00,-1.80, 0.00, 0.00,-1.38, 0.00, 5.20, 0.00, 0.00, 0.00, 0.00, 0.00]
-gainMatrixPitchTorque : [ 1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.00, 5.20, 0.00, 0.00, 0.00, 0.00]
-gainMatrixYawTorque : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30, 0.00, 0.00, 0.00]
+gainMatrixThrust_TwelveStateVector : [ 0.0000, 0.0000, 0.9800, 0.0000, 0.0000, 0.25, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
+gainMatrixRollTorque : [ 0.0000,-0.0017, 0.0000, 0.0000,-0.0009, 0.00, 0.0019, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
+gainMatrixPitchTorque : [ 0.0017, 0.0000, 0.0000, 0.0009, 0.0000, 0.00, 0.0000, 0.0019, 0.0000, 0.0000, 0.0000, 0.0000]
+gainMatrixYawTorque : [ 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.00, 0.0000, 0.0000, 0.0002, 0.0000, 0.0000, 0.0000]
# The LQR Controller parameters for "mode = 3"
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
@@ -87,8 +87,13 @@ command_sixteenbit_max : 60000
PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
PMKF_Kinf_for_positions : [ 0.3277,12.9648]
+
+
# > For the (roll,pitch,yaw) angles
-PMKF_Ahat_row1_for_angles : [ 0.5896, 0.0029]
-PMKF_Ahat_row2_for_angles : [-21.5535, 0.8922]
-PMKF_Kinf_for_angles : [ 0.4104,21.5535]
+PMKF_Ahat_row1_for_angles : [ 0.6954, 0.0035]
+PMKF_Ahat_row2_for_angles : [-11.0342, 0.9448]
+PMKF_Kinf_for_angles : [ 0.3046,11.0342]
+#PMKF_Ahat_row1_for_angles : [ 0.6723, 0.0034]
+#PMKF_Ahat_row2_for_angles : [-12.9648, 0.9352]
+#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp b/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
index 8ee9e68460d2ebff61f2ba06de24693776f6021b..5f0bea33c2c39da9062849f1dc3d3ab63fbcd934 100644
--- a/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
@@ -345,6 +345,34 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
// > yaw position and velocity:
stateInterialEstimate_viaPointMassKalmanFilter[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[5];
stateInterialEstimate_viaPointMassKalmanFilter[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[5];
+
+
+ //DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ //debugMsg.vicon_x = request.ownCrazyflie.x;
+ //debugMsg.vicon_y = request.ownCrazyflie.y;
+ //debugMsg.vicon_z = request.ownCrazyflie.z;
+ //debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ //debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ //debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ //debugMsg.value_1 = stateInterialEstimate_viaPointMassKalmanFilter[6];
+ //debugMsg.value_2 = stateInterialEstimate_viaPointMassKalmanFilter[9];
+ //debugMsg.value_3 = current_xzy_rpy_measurement[3];
+
+
+ //debugMsg.value_4 = stateInterialEstimate_viaPointMassKalmanFilter[0];
+ //debugMsg.value_5 = stateInterialEstimate_viaPointMassKalmanFilter[3];
+ //debugMsg.value_6 = current_xzy_rpy_measurement[0];
+
+
+ // Publish the "debugMsg"
+ //debugPublisher.publish(debugMsg);
}
@@ -443,13 +471,12 @@ void calculateControlOutput_viaLQRforMotors(float stateErrorBody[12], Controller
motor4_thrustAdjustment -= gainMatrixMotor4[i] * stateErrorBody[i];
}
+ //motor1_thrustAdjustment = -gravity_force_quarter*0.9;
+ //motor2_thrustAdjustment = -gravity_force_quarter*0.9;
+ //motor3_thrustAdjustment = -gravity_force_quarter*0.9;
+ //motor4_thrustAdjustment = -gravity_force_quarter*0.9;
- motor1_thrustAdjustment = -gravity_force_quarter/2.0;
- motor2_thrustAdjustment = -gravity_force_quarter/2.0;
- motor3_thrustAdjustment = -gravity_force_quarter/2.0;
- motor4_thrustAdjustment = -gravity_force_quarter/2.0;
-
- // UPDATE THE "RETURN" THE VARIABLE NAME "response"
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the roll, pitch, and yaw command to zero
response.controlOutput.roll = 0;
@@ -470,6 +497,30 @@ void calculateControlOutput_viaLQRforMotors(float stateErrorBody[12], Controller
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+ DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ //debugMsg.vicon_x = request.ownCrazyflie.x;
+ //debugMsg.vicon_y = request.ownCrazyflie.y;
+ //debugMsg.vicon_z = request.ownCrazyflie.z;
+ //debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ //debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ //debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ debugMsg.value_1 = motor1_thrustAdjustment;
+ debugMsg.value_2 = motor2_thrustAdjustment;
+ debugMsg.value_3 = motor3_thrustAdjustment;
+ debugMsg.value_4 = motor4_thrustAdjustment;
+
+
+ // Publish the "debugMsg"
+ debugPublisher.publish(debugMsg);
+
+
// An alternate debugging technique is to print out data directly to the
// command line from which this node was launched.
if (shouldDisplayDebugInfo)
@@ -499,48 +550,88 @@ void calculateControlOutput_viaLQRforActuators(float stateErrorBody[12], Control
// Instantiate the local variables for the per motor thrust
// adjustment. These will be requested from the Crazyflie's
// on-baord "inner-loop" controller
- float motor1_thrustAdjustment = 0;
- float motor2_thrustAdjustment = 0;
- float motor3_thrustAdjustment = 0;
- float motor4_thrustAdjustment = 0;
+ float thrustAdjustment = 0;
+ float rollTorque = 0;
+ float pitchTorque = 0;
+ float yawTorque = 0;
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 12; ++i)
{
// MOTORS 1,2,3,4
- motor1_thrustAdjustment -= gainMatrixMotor1[i] * stateErrorBody[i];
- motor2_thrustAdjustment -= gainMatrixMotor2[i] * stateErrorBody[i];
- motor3_thrustAdjustment -= gainMatrixMotor3[i] * stateErrorBody[i];
- motor4_thrustAdjustment -= gainMatrixMotor4[i] * stateErrorBody[i];
+ thrustAdjustment -= gainMatrixThrust_TwelveStateVector[i] * stateErrorBody[i];
+ rollTorque -= gainMatrixRollTorque[i] * stateErrorBody[i];
+ pitchTorque -= gainMatrixPitchTorque[i] * stateErrorBody[i];
+ yawTorque -= gainMatrixYawTorque[i] * stateErrorBody[i];
}
+ // DISTRIBUTE POWER
+ float motor1_thrustAdjustment;
+ float motor2_thrustAdjustment;
+ float motor3_thrustAdjustment;
+ float motor4_thrustAdjustment;
+
+ float x = 0.0325;
+ float y = 0.0325;
+ float c = 0.0060;
+
+ float tt = thrustAdjustment/4.0;
+ float tx = rollTorque / y;
+ float ty = pitchTorque / x;
+ float tc = yawTorque / c;
- // UPDATE THE "RETURN" THE VARIABLE NAME "response"
+
+ motor1_thrustAdjustment = tt + 0.25 * ( -tx - ty - tc );
+ motor2_thrustAdjustment = tt + 0.25 * ( -tx + ty + tc );
+ motor3_thrustAdjustment = tt + 0.25 * ( tx + ty - tc );
+ motor4_thrustAdjustment = tt + 0.25 * ( tx - ty + tc );
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
// > For roll, pitch, and yaw:
- response.controlOutput.roll = motor2_thrustAdjustment;
- response.controlOutput.pitch = motor3_thrustAdjustment;
- response.controlOutput.yaw = motor4_thrustAdjustment;
+ response.controlOutput.roll = 0;
+ response.controlOutput.pitch = 0;
+ response.controlOutput.yaw = 0;
// > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
- float thrustAdjustment = motor1_thrustAdjustment / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(motor1_thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(motor2_thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(motor3_thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(motor4_thrustAdjustment + gravity_force_quarter);
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+ DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ //debugMsg.vicon_x = request.ownCrazyflie.x;
+ //debugMsg.vicon_y = request.ownCrazyflie.y;
+ //debugMsg.vicon_z = request.ownCrazyflie.z;
+ //debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ //debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ //debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ debugMsg.value_1 = motor1_thrustAdjustment;
+ debugMsg.value_2 = motor2_thrustAdjustment;
+ debugMsg.value_3 = motor3_thrustAdjustment;
+ debugMsg.value_4 = motor4_thrustAdjustment;
+
+
+ // Publish the "debugMsg"
+ debugPublisher.publish(debugMsg);
+
// An alternate debugging technique is to print out data directly to the
// command line from which this node was launched.
if (shouldDisplayDebugInfo)
@@ -589,7 +680,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
}
- // UPDATE THE "RETURN" THE VARIABLE NAME "response"
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
// > For roll, pitch, and yaw:
@@ -717,7 +808,7 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
}
- // UPDATE THE "RETURN" THE VARIABLE NAME "response"
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
// > For roll, pitch, and yaw:
@@ -1120,6 +1211,12 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixMotor3", gainMatrixMotor3, 12);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixMotor4", gainMatrixMotor4, 12);
+ // The LQR Controller parameters for "LQR_MODE_MOTOR"
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_TwelveStateVector", gainMatrixThrust_TwelveStateVector, 12);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollTorque", gainMatrixRollTorque, 12);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchTorque", gainMatrixPitchTorque, 12);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixYawTorque", gainMatrixYawTorque, 12);
+
// The LQR Controller parameters for "LQR_MODE_RATE"
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollRate", gainMatrixRollRate, 9);
getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);