Commit 1571892b authored by mastefan's avatar mastefan
Browse files

Minor changes to Force (Euler angle)

parent db3e3a8e
......@@ -81,7 +81,7 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
# for our integrator (so far just random values)
gainIntegrator : [0.1, 0.1, 0.1]
gainIntegrator : [0.2, 0.2, 0.2]
# The LQR Controller
......
......@@ -653,6 +653,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, m_setpoint_for_controller, current_bodyFrameError);
integrator_XYZ(current_bodyFrameError);
calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
}else{
......@@ -731,7 +732,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// Calculate the Force Feedforward
float F = (gravity * m_mass_total_grams)/(cos(rollAngle_forResponse)*cos(pitchAngle_forResponse));
float F_in_newtons = (gravity * m_mass_total_grams)/(cos(rollAngle_forResponse)*cos(pitchAngle_forResponse)*1000.0);
......@@ -774,7 +775,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// controller needed to be divided by 4 or not.
thrustAdjustment = thrustAdjustment / 4.0;
// > Compute the feed-forward force
float feed_forward_thrust_per_motor = m_mass_total_grams * 9.81/(1000*4);
float feed_forward_thrust_per_motor = F_in_newtons / 4.0; //m_mass_total_grams * 9.81/(1000*4);
// > Put in the per motor commands
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
......@@ -1079,7 +1080,11 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
float thrustAdjustment = 0;
//integrator for x,y,z
//integrator_XYZ(stateErrorBody);
/*
if(integratorFlag == DRONEX_INTEGRATOR_ON)
{
ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON");
......@@ -1101,6 +1106,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
{
stateErrorBody[i] += integrator_var[i] * gainIntegrator[i];
}
*/
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 9; ++i)
......@@ -1331,6 +1337,7 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
yawError = (-PI/6);
}
// CONVERSION INTO BODY FRAME
// Conver the state erorr from the Inertial frame into the Body frame
// > Note: the function "convertIntoBodyFrame" is implemented in this file
......@@ -1757,6 +1764,10 @@ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
}
// ----------------------------------------------------------------------------------
// III N N TTTTT EEEEE GGGG RRRR A TTTTT OOO RRRR
// I NN N T E G R R A A T O O R R
......
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