Commit f6ea3604 authored by mastefan's avatar mastefan
Browse files

test to update

parent bea40b0d
...@@ -261,9 +261,8 @@ private slots: ...@@ -261,9 +261,8 @@ private slots:
void on_integrate_on_dronex_button_clicked(); void on_integrate_on_dronex_button_clicked();
void on_integrate_off_dronex_button_clicked(); void on_integrate_off_dronex_button_clicked();
void on_integrate_reset_dronex_button_clicked(); void on_integrate_reset_dronex_button_clicked();
void on_set_weigth_param_clicked();
void on_set_pitch_baseline_param_clicked();
private: private:
Ui::MainWindow *ui; Ui::MainWindow *ui;
...@@ -354,7 +353,9 @@ private: ...@@ -354,7 +353,9 @@ private:
ros::Publisher dronexSetpointPublisher; ros::Publisher dronexSetpointPublisher;
ros::Subscriber dronexSetpointSubscriber; ros::Subscriber dronexSetpointSubscriber;
ros::Subscriber dronexSetpointToGUISubscriber; ros::Subscriber dronexSetpointToGUISubscriber;
ros::Publisher droneXIntegrParamPublisher;
ros::Publisher droneXWeightParamPublisher;
ros::Publisher droneXPitchBaselineParamPublisher;
// > For the PICKER CONTROLLER // > For the PICKER CONTROLLER
...@@ -454,6 +455,7 @@ private: ...@@ -454,6 +455,7 @@ private:
void highlightTuningControllerTab(); void highlightTuningControllerTab();
void highlightPickerControllerTab(); void highlightPickerControllerTab();
void highlightDroneXControllerTab(); void highlightDroneXControllerTab();
void on_set_Integr_Param_button_clicked();
bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context); bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context); Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
......
...@@ -112,13 +112,16 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) : ...@@ -112,13 +112,16 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
//TODO add Publisher and Subscriber for DroneX // Publisher and Subscriber for DroneX
dronexButtonPressedPublisher = nodeHandle.advertise<std_msgs::Int32>("DroneXControllerService/ButtonPressed", 1); dronexButtonPressedPublisher = nodeHandle.advertise<std_msgs::Int32>("DroneXControllerService/ButtonPressed", 1);
dronexSetpointPublisher = nodeHandle.advertise<Setpoint>("DroneXControllerService/Setpoint", 1); dronexSetpointPublisher = nodeHandle.advertise<Setpoint>("DroneXControllerService/Setpoint", 1);
dronexSetpointSubscriber = nodeHandle.subscribe("DroneXControllerService/Setpoint", 1, &MainWindow::dronexSetpointCallback, this); dronexSetpointSubscriber = nodeHandle.subscribe("DroneXControllerService/Setpoint", 1, &MainWindow::dronexSetpointCallback, this);
dronexSetpointToGUISubscriber = nodeHandle.subscribe("DroneXControllerService/SetpointToGUI", 1, &MainWindow::dronexSetpointCallback, this); dronexSetpointToGUISubscriber = nodeHandle.subscribe("DroneXControllerService/SetpointToGUI", 1, &MainWindow::dronexSetpointCallback, this);
droneXIntegrParamPublisher = nodeHandle.advertise<Setpoint>("DroneXControllerService/IntegrParams", 1);
droneXWeightParamPublisher = nodeHandle.advertise<Setpoint>("DroneXControllerService/WeightParam", 1);
droneXWeightParamPublisher = nodeHandle.advertise<Setpoint>("DroneXControllerService/WeightParam", 1);
droneXPitchBaselineParamPublisher = nodeHandle.advertise<Setpoint>("DroneXControllerService/PitchBaselineParam", 1);
// SET ALL SLIDERS AND DIALS TO DEFAULT VALUES // SET ALL SLIDERS AND DIALS TO DEFAULT VALUES
ui->picker_z_slider->setValue( 40 ); ui->picker_z_slider->setValue( 40 );
ui->picker_mass_slider->setValue( 29 ); ui->picker_mass_slider->setValue( 29 );
...@@ -1618,9 +1621,40 @@ void MainWindow::on_enable_droneX_controller_clicked() ...@@ -1618,9 +1621,40 @@ void MainWindow::on_enable_droneX_controller_clicked()
} }
void MainWindow::on_set_Integr_Param_button_clicked()
{
Setpoint msg;
msg.x = (ui->current_setpoint_x_student->text()).toFloat();
msg.y = (ui->current_setpoint_y_student->text()).toFloat();
msg.z = (ui->current_setpoint_z_student->text()).toFloat();
this->droneXIntegrParamPublisher.publish(msg);
}
void MainWindow::on_set_weigth_param_clicked()
{
Setpoint msg;
msg.x = (ui->weight_param->text()).toFloat();
msg.y = 0;
msg.z = 0;
this->droneXWeightParamPublisher.publish(msg);
}
void MainWindow::on_set_pitch_baseline_param_clicked()
{
Setpoint msg;
msg.x = (ui->pitch_baseline_param->text()).toFloat();
msg.y = 0;
msg.z = 0;
this->droneXPitchBaselineParamPublisher.publish(msg);
}
......
...@@ -405,7 +405,7 @@ ...@@ -405,7 +405,7 @@
</attribute> </attribute>
<layout class="QGridLayout" name="gridLayout_22"> <layout class="QGridLayout" name="gridLayout_22">
<item row="0" column="0"> <item row="0" column="0">
<layout class="QGridLayout" name="gridLayout_21" rowstretch="0,0,0,0,0"> <layout class="QGridLayout" name="gridLayout_21" rowstretch="0,0">
<property name="leftMargin"> <property name="leftMargin">
<number>0</number> <number>0</number>
</property> </property>
...@@ -416,7 +416,7 @@ ...@@ -416,7 +416,7 @@
<number>6</number> <number>6</number>
</property> </property>
<item row="1" column="0"> <item row="1" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_14" stretch="1,0,1"> <layout class="QHBoxLayout" name="horizontalLayout_14" stretch="1,0,0">
<item> <item>
<layout class="QVBoxLayout" name="verticalLayout_10" stretch="0,0,0"> <layout class="QVBoxLayout" name="verticalLayout_10" stretch="0,0,0">
<item> <item>
...@@ -531,6 +531,77 @@ ...@@ -531,6 +531,77 @@
</property> </property>
</widget> </widget>
</item> </item>
<item>
<widget class="QLabel" name="label_62">
<property name="text">
<string>Integrator params</string>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_17">
<property name="topMargin">
<number>0</number>
</property>
<item>
<widget class="QLineEdit" name="x_integr">
<property name="statusTip">
<string/>
</property>
<property name="whatsThis">
<string/>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="y_integr"/>
</item>
<item>
<widget class="QLineEdit" name="z_integr"/>
</item>
</layout>
</item>
<item>
<widget class="QPushButton" name="set_Integr_Param_button">
<property name="text">
<string>Set</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_63">
<property name="text">
<string>Drone weight param[g]</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="weight_param"/>
</item>
<item>
<widget class="QPushButton" name="set_weight_param">
<property name="text">
<string>Set</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_64">
<property name="text">
<string>Pitch angle baseline[?]</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="pitch_baseline_param"/>
</item>
<item>
<widget class="QPushButton" name="set_pitch_baseline">
<property name="text">
<string>Set</string>
</property>
</widget>
</item>
<item> <item>
<spacer name="verticalSpacer_8"> <spacer name="verticalSpacer_8">
<property name="orientation"> <property name="orientation">
......
...@@ -107,10 +107,10 @@ ...@@ -107,10 +107,10 @@
#define DRONEX_INTEGRATOR_OFF 16 #define DRONEX_INTEGRATOR_OFF 16
#define DRONEX_INTEGRATOR_RESET 17 #define DRONEX_INTEGRATOR_RESET 17
#define DRONEX_STATE_GROUND 0 #define DRONEX_STATE_GROUND 0
#define DRONEX_STATE_TAKING_OFF 1 #define DRONEX_STATE_TAKING_OFF 1
#define DRONEX_STATE_LAND_ON_GROUND 2 #define DRONEX_STATE_LAND_ON_GROUND 2
#define DRONEX_STATE_APPROACH 3 #define DRONEX_STATE_APPROACH 3
#define DRONEX_STATE_HOVER 4 #define DRONEX_STATE_HOVER 4
#define DRONEX_STATE_LAND_ON_MOTHERSHIP 5 #define DRONEX_STATE_LAND_ON_MOTHERSHIP 5
#define DRONEX_STATE_ON_MOTHERSHIP 6 #define DRONEX_STATE_ON_MOTHERSHIP 6
...@@ -169,8 +169,9 @@ ...@@ -169,8 +169,9 @@
using namespace d_fall_pps; using namespace d_fall_pps;
// Flight sequences // Flight sequences
#define SEQUENCE_NONE 0 #define SEQUENCE_NONE 0
#define SEQUENCE_LAND_ON_MOTHERSHIP 1 #define SEQUENCE_LAND_ON_MOTHERSHIP 1
#define SEQUENCE_HOVER_OVER_MOTHERSHIP 2
...@@ -195,13 +196,14 @@ Setpoint dronexSetpoint; ...@@ -195,13 +196,14 @@ Setpoint dronexSetpoint;
int flying_state = DRONEX_STATE_GROUND; int flying_state = DRONEX_STATE_GROUND;
int integratorFlag = DRONEX_INTEGRATOR_OFF; int integratorFlag = DRONEX_INTEGRATOR_OFF;
float integrator_var[3] = {0,0,0}; float integrator_sum_XYZ[3] = {0,0,0};
// Controller Mode
int controller_mode = 1;
// Variable for choosing flight sequence // Variable for choosing flight sequence
int flightSequence = SEQUENCE_NONE; int flightSequence = 0;
// Flags of landing sequence // Flags of landing sequence
bool tookOffFlag = false; bool tookOffFlag = false;
...@@ -215,11 +217,12 @@ float startCoordinateY; ...@@ -215,11 +217,12 @@ float startCoordinateY;
float startCoordinateZ; float startCoordinateZ;
// tolerances (x,y,z) // tolerances (x,y,z)
float tol_eps[3] = {0.5, 0.5, 0.5}; float tol_takeoff[3] = {0.07, 0.07, 0.07}; //{0.03, 0.03, 0.03};
float eps_height = 0.05; float tol_approach[3] = {0.3, 0.3, 0.07}; // added to differ between approach and land *
float startHeight = 0.6; float tol_land[3] = {0.03, 0.03, 0.03}; // *
float thrust_factor = 0.95;
int decreaseFlag = 0;
//describes the area of the crazyflie and other parameters //describes the area of the crazyflie and other parameters
...@@ -227,11 +230,11 @@ float startHeight = 0.6; ...@@ -227,11 +230,11 @@ float startHeight = 0.6;
// > Total mass of the Crazyflie plus whatever it is carrying, in [grams] // > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
float m_mass_total_grams; float m_mass_total_grams;
float gravity = 9.81;
// > The setpoints for (x,y,z) position and yaw angle, in that order // > 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}; float m_setpoint[4] = {0.0,0.0,0.0,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0}; float m_setpoint_for_controller[4] = {0.0,0.0,0.0,0.0};
// > Small adjustments to the x-y setpoint // > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f; float m_xAdjustment = 0.0f;
...@@ -251,6 +254,11 @@ float m_max_setpoint_change_per_second_yaw_radians; ...@@ -251,6 +254,11 @@ float m_max_setpoint_change_per_second_yaw_radians;
float m_vicon_frequency; float m_vicon_frequency;
float prev_DroneX_pos[3] = {0,0,0};
float current_DroneX_pos[3];
float drone_X_vel[3];
// THE FOLLOWING PARAMETERS ARE USED // THE FOLLOWING PARAMETERS ARE USED
// FOR THE LOW-LEVEL CONTROLLER // FOR THE LOW-LEVEL CONTROLLER
...@@ -266,8 +274,20 @@ std::vector<float> gainMatrixRollRate (9,0.0); ...@@ -266,8 +274,20 @@ std::vector<float> gainMatrixRollRate (9,0.0);
std::vector<float> gainMatrixPitchRate (9,0.0); std::vector<float> gainMatrixPitchRate (9,0.0);
std::vector<float> gainMatrixYawRate (9,0.0); std::vector<float> gainMatrixYawRate (9,0.0);
std::vector<float> gainMatrixYawRatefromAngle (9,0.0);
std::vector<float> gainMatrixPitchRatefromAngle (9,0.0);
std::vector<float> gainMatrixRollRatefromAngle (9,0.0);
std::vector<float> gainMatrixThrust_SixStateVector (9,0.0);
std::vector<float> gainMatrixRollAngle (9,0.0);
std::vector<float> gainMatrixPitchAngle (9,0.0);
// Integrator parameters // Integrator parameters
std::vector<float> gainIntegrator (3,0.0); std::vector<float> gainIntegrator (3,0.0); // in case we want to go back to old integrator concept, else TODO: delete
std::vector<float> gainIntegratorRate (3,0.0);
std::vector<float> gainIntegratorAngle (3,0.0);
// The 16-bit command limits // The 16-bit command limits
float cmd_sixteenbit_min; float cmd_sixteenbit_min;
...@@ -297,6 +317,7 @@ float previous_xzy_rpy_measurement[6]; ...@@ -297,6 +317,7 @@ float previous_xzy_rpy_measurement[6];
// difference state estimator // difference state estimator
float stateInterialEstimate_viaFiniteDifference[12]; float stateInterialEstimate_viaFiniteDifference[12];
// > The full 12 state estimate maintained by the point mass // > The full 12 state estimate maintained by the point mass
// kalman filter state estimator // kalman filter state estimator
float stateInterialEstimate_viaPointMassKalmanFilter[12]; float stateInterialEstimate_viaPointMassKalmanFilter[12];
...@@ -435,7 +456,7 @@ void motorsOFF(Controller::Response &response); ...@@ -435,7 +456,7 @@ void motorsOFF(Controller::Response &response);
// CONTROLLER COMPUTATIONS // CONTROLLER COMPUTATIONS
// > The function that is called to "start" all estimation and control computations // > The function that is called to "start" all estimation and control computations
bool calculateControlOutput(Controller::Request &request, Controller::Response &response); bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response);
// > The various functions that implement an LQR controller // > The various functions that implement an LQR controller
void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response); void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response); void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
...@@ -445,6 +466,15 @@ void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12] ...@@ -445,6 +466,15 @@ void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12]
void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response); void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response); void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforRates_forLanding( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
// Velocity estimator
void calculateDroneXVelocity(Controller::Request &request);
// Integrator
void integrator_XYZ(float (&stateErrorBody)[12]);
// ESTIMATOR COMPUTATIONS // ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request); void performEstimatorUpdate_forStateInterial(Controller::Request &request);
void performEstimatorUpdate_forStateInterial_viaFiniteDifference(); void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
......
...@@ -191,8 +191,8 @@ std::vector<float> m_dropoff_coordinates_xy_for_H(2); ...@@ -191,8 +191,8 @@ std::vector<float> m_dropoff_coordinates_xy_for_H(2);
float m_picker_string_length; float m_picker_string_length;
// > The setpoints for (x,y,z) position and yaw angle, in that order // > 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}; float m_setpoint[4] = {0.0,0.0,0,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0}; float m_setpoint_for_controller[4] = {0.0,0.0,0.0,0.0};
// > Small adjustments to the x-y setpoint // > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f; float m_xAdjustment = 0.0f;
...@@ -215,6 +215,7 @@ float m_vicon_frequency; ...@@ -215,6 +215,7 @@ float m_vicon_frequency;
// THE FOLLOWING PARAMETERS ARE USED // THE FOLLOWING PARAMETERS ARE USED
// FOR THE LOW-LEVEL CONTROLLER // FOR THE LOW-LEVEL CONTROLLER
......
...@@ -48,7 +48,7 @@ shouldDisplayDebugInfo : false ...@@ -48,7 +48,7 @@ shouldDisplayDebugInfo : false
# - Swaps between pitch set-points to test angle set-point response time # - Swaps between pitch set-points to test angle set-point response time
# i.e., this controller test the assumption that "the inner loop is infinitely fast" # i.e., this controller test the assumption that "the inner loop is infinitely fast"
# #
controller_mode : 3 controller_mode : 6
# A flag for which estimator to use, defined as: # A flag for which estimator to use, defined as:
...@@ -124,4 +124,4 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342] ...@@ -124,4 +124,4 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Ahat_row1_for_angles : [ 0.6723, 0.0034] #PMKF_Ahat_row1_for_angles : [ 0.6723, 0.0034]
#PMKF_Ahat_row2_for_angles : [-12.9648, 0.9352] #PMKF_Ahat_row2_for_angles : [-12.9648, 0.9352]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648] #PMKF_Kinf_for_angles : [ 0.3277,12.9648]
\ No newline at end of file
# Mass of the crazyflie # Mass of the crazyflie
mass_CF : 32 mass_CF : 28 #32
# Max setpoint change per second # Max setpoint change per second
max_setpoint_change_per_second_horizontal : 1.00 # [meters] max_setpoint_change_per_second_horizontal : 2.00 # [meters]
max_setpoint_change_per_second_vertical : 0.40 # [meters] max_setpoint_change_per_second_vertical : 0.4 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees] max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# Frequency of the controller, in hertz # Frequency of the controller, in hertz
...@@ -12,8 +12,9 @@ vicon_frequency : 200 ...@@ -12,8 +12,9 @@ vicon_frequency : 200
# controller_mode : 0: Basic Controller
# controller_mode : 1: Angle Controller
controller_mode : 0
...@@ -55,11 +56,7 @@ shouldDisplayDebugInfo : false ...@@ -55,11 +56,7 @@ shouldDisplayDebugInfo : false
estimator_method : 1 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 # The max and minimum thrust for a 16-bit command
...@@ -84,6 +81,29 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342] ...@@ -84,6 +81,29 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648] #PMKF_Kinf_for_angles : [ 0.3277,12.9648]
# for our integrator (so far just random values) # for our integrator (so far just random values)
gainIntegrator : [1, 1, 1] gainIntegrator : [ 0.50, 0.50, 0.50] # [x,y,z] (in case we want to go back to old integrator concept, else TODO: delete)
gainIntegratorRate : [-0.20, 0.20, 0.20] # [roll, pitch, z]
gainIntegratorAngle : [-0.20, 0.20, 0.20] # [roll, pitch, z]
# The LQR Controller
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]
# LQR: get Angles from position error
gainMatrixThrust_SixStateVector: [ 0.00, 0.00, 0.82, 0.00, 0.00, 0.22]
gainMatrixRollAngle : [ 0.00,-0.31, 0.00, 0.00,-0.25, 0.00]
gainMatrixPitchAngle : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00]
# flightSequence : 0 #LQR: get Rates from Angle error
gainMatrixRollRatefromAngle : [ 4.00, 0.00, 0.00]
gainMatrixPitchRatefromAngle : [ 0.00, 4.00, 0.00]
gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30]
\ No newline at end of file
...@@ -185,10 +185,6 @@ void perControlCycleOperations() ...@@ -185,10 +185,6 @@ void perControlCycleOperations()
// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED // CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
void buttonPressed_gotoStart() void buttonPressed_gotoStart()
......
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