Commit 2ce4c8cc authored by mastefan's avatar mastefan
Browse files

DroneXControllerService and DroneX Yaml files adapted

parent c6a3ca1d
......@@ -166,31 +166,11 @@ float m_time_seconds;
// > Mass of the Crazyflie quad-rotor, in [grams]
float m_mass_CF_grams;
// > Mass of the letters to be lifted, in [grams]
float m_mass_E_grams;
float m_mass_T_grams;
float m_mass_H_grams;
// > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
float m_mass_total_grams;
// Thickness of the object at pick-up and put-down, in [meters]
// > This should also account for extra height due to
// the surface where the object is
float m_thickness_of_object_at_pickup;
float m_thickness_of_object_at_putdown;
// (x,y) coordinates of the pickup location
std::vector<float> m_pickup_coordinates_xy(2);
// (x,y) coordinates of the drop off location
std::vector<float> m_dropoff_coordinates_xy_for_E(2);
std::vector<float> m_dropoff_coordinates_xy_for_T(2);
std::vector<float> m_dropoff_coordinates_xy_for_H(2);
// Length of the string from the Crazyflie
// to the end of the DroneX, in [meters]
float m_dronex_string_length;
// > The setpoints for (x,y,z) position and yaw angle, in that order
float m_setpoint[4] = {0.0,0.0,0.4,0.0};
......
#equlibrium offset
mass : 29
# Mass of the crazyflie
mass_CF : 32
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 0.20 # [meters]
max_setpoint_change_per_second_vertical : 0.10 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# Frequency of the controller, in hertz
vicon_frequency : 200
# THE FOLLOWING PARAMETERS ARE USED
# FOR THE LOW-LEVEL CONTROLLER
# Frequency of the controller, in hertz
control_frequency : 200
#quadratic motor regression equation (a0, a1, a2)
motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
# Quadratic motor regression equation (a0, a1, a2)
motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
# Boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame : true
# Boolean indiciating whether the (x,y,z,yaw) of this agent should be published or not
shouldPublishCurrent_xyz_yaw : true
# Boolean indiciating whether the "Debug Message" of this agent should be published or not
shouldPublishDebugMessage : true
# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
shouldDisplayDebugInfo : false
# A flag for which estimator to use, defined as:
# 1 - Finite Different Method,
# Takes the poisition and angles directly as measured,
# and estimates the velocities as a finite different to the
# previous measurement
# 2 - Point Mass Per Dimension Method
# Uses a 2nd order random walk estimator independently for
# 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
# 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]
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, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
# The max and minimum thrust for a 16-bit command
command_sixteenbit_min : 1000
command_sixteenbit_max : 60000
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > For the (x,y,z) position
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]
#feedback gain matrix
gainMatrixRoll: [0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0]
gainMatrixPitch: [1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0]
gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
gainMatrixThrust: [0, 0, 0.19195826, 0, 0, 0.08362477, 0, 0, 0]
# > For the (roll,pitch,yaw) angles
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]
......@@ -201,7 +201,7 @@ void buttonPressed_land(){
// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
/*
void buttonPressed_gotoStart()
{
ROS_INFO("[DRONEX CONTROLLER] Goto Start button pressed");
......@@ -276,7 +276,7 @@ void buttonPressed_disconnect()
m_mass_total_grams = m_mass_CF_grams;
publishCurrentSetpoint();
}
*/
......@@ -1083,26 +1083,8 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// > The mass of the crazyflie
m_mass_CF_grams = getParameterFloat(nodeHandle_for_dronexController , "mass_CF");
// > The mass of the letters
m_mass_E_grams = getParameterFloat(nodeHandle_for_dronexController , "mass_E");
m_mass_T_grams = getParameterFloat(nodeHandle_for_dronexController , "mass_T");
m_mass_H_grams = getParameterFloat(nodeHandle_for_dronexController , "mass_H");
// Thickness of the object at pick-up and put-down, in [meters]
// > This should also account for extra height due to the surface where the object is
m_thickness_of_object_at_pickup = getParameterFloat(nodeHandle_for_dronexController , "thickness_of_object_at_pickup");
m_thickness_of_object_at_putdown = getParameterFloat(nodeHandle_for_dronexController , "thickness_of_object_at_putdown");
// (x,y) coordinates of the pickup location
getParameterFloatVector(nodeHandle_for_dronexController, "pickup_coordinates_xy", m_pickup_coordinates_xy, 2);
// (x,y) coordinates of the drop off location
getParameterFloatVector(nodeHandle_for_dronexController, "dropoff_coordinates_xy_for_E", m_dropoff_coordinates_xy_for_E, 2);
getParameterFloatVector(nodeHandle_for_dronexController, "dropoff_coordinates_xy_for_T", m_dropoff_coordinates_xy_for_T, 2);
getParameterFloatVector(nodeHandle_for_dronexController, "dropoff_coordinates_xy_for_H", m_dropoff_coordinates_xy_for_H, 2);
// Length of the string from the Crazyflie to the end of the DroneX, in [meters]
m_dronex_string_length = getParameterFloat(nodeHandle_for_dronexController , "dronex_string_length");
// Max setpoint change per second
m_max_setpoint_change_per_second_horizontal = getParameterFloat(nodeHandle_for_dronexController , "max_setpoint_change_per_second_horizontal");
......
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