Commit d3ff7e61 authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Final attempt at tuning the full state LQR, no success

parent 2775b69c
safeController: "SafeControllerService/RateController"
demoController: "DemoControllerService/DemoController"
strictSafety: true
strictSafety: false
angleMargin: 0.6
battery_threshold_while_flying: 2.8 # in V
battery_threshold_while_motors_off: 3.30 # in V
......
# Mass of the crazyflie
mass : 31
mass : 30
# Frequency of the controller, in hertz
control_frequency : 200
......@@ -27,7 +27,7 @@ follow_in_a_line_agentIDs : [1, 2, 3]
shouldPublishDebugMessage : false
# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
shouldDisplayDebugInfo : true
shouldDisplayDebugInfo : false
# A flag for which LQR controller mode to use, defined as:
......@@ -50,7 +50,7 @@ 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 : 2
estimator_method : 1
# The LQR Controller parameters for "mode = 1"
......
......@@ -471,10 +471,41 @@ 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;
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 = stateErrorBody[6];
debugMsg.value_2 = stateErrorBody[9];
debugMsg.value_3 = motor1_thrustAdjustment;
debugMsg.value_4 = motor2_thrustAdjustment;
debugMsg.value_5 = motor3_thrustAdjustment;
debugMsg.value_6 = motor4_thrustAdjustment;
// Publish the "debugMsg"
debugPublisher.publish(debugMsg);
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;
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
......@@ -497,28 +528,7 @@ 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
......
Supports Markdown
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