Commit f6ea3604 authored by mastefan's avatar mastefan
Browse files

test to update

parent bea40b0d
......@@ -261,9 +261,8 @@ private slots:
void on_integrate_on_dronex_button_clicked();
void on_integrate_off_dronex_button_clicked();
void on_integrate_reset_dronex_button_clicked();
void on_set_weigth_param_clicked();
void on_set_pitch_baseline_param_clicked();
private:
Ui::MainWindow *ui;
......@@ -354,7 +353,9 @@ private:
ros::Publisher dronexSetpointPublisher;
ros::Subscriber dronexSetpointSubscriber;
ros::Subscriber dronexSetpointToGUISubscriber;
ros::Publisher droneXIntegrParamPublisher;
ros::Publisher droneXWeightParamPublisher;
ros::Publisher droneXPitchBaselineParamPublisher;
// > For the PICKER CONTROLLER
......@@ -454,6 +455,7 @@ private:
void highlightTuningControllerTab();
void highlightPickerControllerTab();
void highlightDroneXControllerTab();
void on_set_Integr_Param_button_clicked();
bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
......
......@@ -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);
dronexSetpointPublisher = nodeHandle.advertise<Setpoint>("DroneXControllerService/Setpoint", 1);
dronexSetpointSubscriber = nodeHandle.subscribe("DroneXControllerService/Setpoint", 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
ui->picker_z_slider->setValue( 40 );
ui->picker_mass_slider->setValue( 29 );
......@@ -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 @@
</attribute>
<layout class="QGridLayout" name="gridLayout_22">
<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">
<number>0</number>
</property>
......@@ -416,7 +416,7 @@
<number>6</number>
</property>
<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>
<layout class="QVBoxLayout" name="verticalLayout_10" stretch="0,0,0">
<item>
......@@ -531,6 +531,77 @@
</property>
</widget>
</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>
<spacer name="verticalSpacer_8">
<property name="orientation">
......
......@@ -107,10 +107,10 @@
#define DRONEX_INTEGRATOR_OFF 16
#define DRONEX_INTEGRATOR_RESET 17
#define DRONEX_STATE_GROUND 0
#define DRONEX_STATE_TAKING_OFF 1
#define DRONEX_STATE_LAND_ON_GROUND 2
#define DRONEX_STATE_APPROACH 3
#define DRONEX_STATE_GROUND 0
#define DRONEX_STATE_TAKING_OFF 1
#define DRONEX_STATE_LAND_ON_GROUND 2
#define DRONEX_STATE_APPROACH 3
#define DRONEX_STATE_HOVER 4
#define DRONEX_STATE_LAND_ON_MOTHERSHIP 5
#define DRONEX_STATE_ON_MOTHERSHIP 6
......@@ -169,8 +169,9 @@
using namespace d_fall_pps;
// Flight sequences
#define SEQUENCE_NONE 0
#define SEQUENCE_LAND_ON_MOTHERSHIP 1
#define SEQUENCE_NONE 0
#define SEQUENCE_LAND_ON_MOTHERSHIP 1
#define SEQUENCE_HOVER_OVER_MOTHERSHIP 2
......@@ -195,13 +196,14 @@ Setpoint dronexSetpoint;
int flying_state = DRONEX_STATE_GROUND;
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
int flightSequence = SEQUENCE_NONE;
int flightSequence = 0;
// Flags of landing sequence
bool tookOffFlag = false;
......@@ -215,11 +217,12 @@ float startCoordinateY;
float startCoordinateZ;
// tolerances (x,y,z)
float tol_eps[3] = {0.5, 0.5, 0.5};
float eps_height = 0.05;
float startHeight = 0.6;
float tol_takeoff[3] = {0.07, 0.07, 0.07}; //{0.03, 0.03, 0.03};
float tol_approach[3] = {0.3, 0.3, 0.07}; // added to differ between approach and land *
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
......@@ -227,11 +230,11 @@ float startHeight = 0.6;
// > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
float m_mass_total_grams;
float gravity = 9.81;
// > 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_for_controller[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.0,0.0};
// > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f;
......@@ -251,6 +254,11 @@ float m_max_setpoint_change_per_second_yaw_radians;
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
// FOR THE LOW-LEVEL CONTROLLER
......@@ -266,8 +274,20 @@ std::vector<float> gainMatrixRollRate (9,0.0);
std::vector<float> gainMatrixPitchRate (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
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
float cmd_sixteenbit_min;
......@@ -297,6 +317,7 @@ float previous_xzy_rpy_measurement[6];
// difference state estimator
float stateInterialEstimate_viaFiniteDifference[12];
// > The full 12 state estimate maintained by the point mass
// kalman filter state estimator
float stateInterialEstimate_viaPointMassKalmanFilter[12];
......@@ -435,7 +456,7 @@ void motorsOFF(Controller::Response &response);
// CONTROLLER COMPUTATIONS
// > The function that is called to "start" all estimation and control computations
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response);
// > The various functions that implement an LQR controller
void calculateControlOutput_viaLQR( 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]
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_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
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
......
......@@ -191,8 +191,8 @@ std::vector<float> m_dropoff_coordinates_xy_for_H(2);
float m_picker_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};
float m_setpoint_for_controller[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.0,0.0};
// > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f;
......@@ -215,6 +215,7 @@ float m_vicon_frequency;
// THE FOLLOWING PARAMETERS ARE USED
// FOR THE LOW-LEVEL CONTROLLER
......
......@@ -48,7 +48,7 @@ shouldDisplayDebugInfo : false
# - 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"
#
controller_mode : 3
controller_mode : 6
# A flag for which estimator to use, defined as:
......@@ -124,4 +124,4 @@ 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
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
# Mass of the crazyflie
mass_CF : 32
mass_CF : 28 #32
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 1.00 # [meters]
max_setpoint_change_per_second_vertical : 0.40 # [meters]
max_setpoint_change_per_second_horizontal : 2.00 # [meters]
max_setpoint_change_per_second_vertical : 0.4 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# Frequency of the controller, in hertz
......@@ -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
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
......@@ -84,6 +81,29 @@ 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 : [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()
// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
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