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
......@@ -205,7 +205,7 @@ void buttonPressed_land(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Start flight-sequence LA...");
// OLD:flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
// NEW:
flightSequence = 1;
flightSequence = SEQUENCE_LAND_ON_MOTHERSHIP;
}
void buttonPressed_abort(){
......@@ -231,6 +231,25 @@ void buttonPressed_integrator_reset(){
integratorFlag = DRONEX_INTEGRATOR_RESET;
}
void integratorCallback (const Setpoint& integrParams ) {
integrator_sum_XYZ[0] = integrParams.x;
integrator_sum_XYZ[1] = integrParams.y;
integrator_sum_XYZ[2] = integrParams.z;
}
void WeightParamCallback (const Setpoint& weightParam ) {
// TODO for changing yaml: set weight in yaml OR just set m_mass_CF_grams?
// m_mass_CF_grams = weightParam.x;
}
void PitchBaselineParamCallback(const Setpoint& pitchAngleParam ) {
// TODO
}
......@@ -350,16 +369,25 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
switch(flying_state){
case DRONEX_STATE_APPROACH:
{
//ROS_INFO("DRONEX_STATE_APPROACH");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.2;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.4;//0.6;
/*ROS_INFO_STREAM("APPROACH: (x,y,z) Difference: ("
<< request.ownCrazyflie.x-dronexSetpoint.x << ", "
<< request.ownCrazyflie.y-dronexSetpoint.y << ", "
<< request.ownCrazyflie.z-dronexSetpoint.z << ")");
*/
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_eps[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_eps[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_eps[2] ){
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_approach[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_approach[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_approach[2] ){
approachedFlag = true;
ROS_INFO("approached");
}
......@@ -371,7 +399,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
//ROS_INFO("DRONEX_STATE_GROUND");
// Variable for choosing flight sequence off
flightSequence = 0;
flightSequence = SEQUENCE_NONE;
// Flags of landing sequence reset
tookOffFlag = false;
......@@ -382,10 +410,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z;
/* std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
return true;*/
}
break;
......@@ -393,7 +417,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
//ROS_INFO("DRONEX_STATE_ON_MOTHERSHIP");
// Variable for choosing flight sequence off
flightSequence = 0;
flightSequence = SEQUENCE_NONE;
// Flags of landing sequence reset
tookOffFlag = false;
......@@ -404,63 +428,30 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z;
/* std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
return true;*/
}
break;
case DRONEX_STATE_LAND_ON_MOTHERSHIP:
{
//ROS_INFO("DRONEX_STATE_LAND_ON_MOTHERSHIP");
//to land on mothership (17.10. vm)
dronexSetpoint.x = request.otherCrazyflies[0].x; //set setpoint to droneX x y and z coordinates
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + eps_height;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.05;
}
break;
case DRONEX_STATE_LAND_ON_GROUND:
{
//ROS_INFO_STREAM("DRONEX_STATE_LAND_ON_GROUND: Crazyflie z coordinate " << request.ownCrazyflie.z);
//ROS_INFO_STREAM("DRONEX_STATE_LAND_ON_GROUND: Mothership z coordinate " << request.otherCrazyflies[0].z);
// to land on mothership: -> commented out (17.10. vm)
//dronexSetpoint.x = request.otherCrazyflies[0].x; //set setpoint to droneX x y and z coordinates
//dronexSetpoint.y = request.otherCrazyflies[0].y;
//dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
if(tookOffFlag){
ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
// to land on ground (17.10. vm)
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = eps_height;
/*if(request.ownCrazyflie.z < request.otherCrazyflies[0].z+0.25){ // >0.1
std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
//flyingStatePublisher.publish(flying_state_msg);
ROS_INFO_STREAM("DRONEX_MOTORS_OFF..." << request.ownCrazyflie.z - request.otherCrazyflies[0].z);
return true;
}*/
/*
}else{
//if(dronexSetpoint.z>=request.otherCrazyflies[0].z){
float down_per_cycle = 0.003;
if(dronexSetpoint.z < request.otherCrazyflies[0].z+0.5){
down_per_cycle = 0.0005;
}
dronexSetpoint.z -= down_per_cycle;
ROS_INFO_STREAM("DRONEX_L: z" << dronexSetpoint.z);
//}else{
//}
}*/
dronexSetpoint.z = 0.0;
tookOffFlag = false;
}
}
break;
......@@ -486,12 +477,19 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.x = startCoordinateX;
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + startHeight;
dronexSetpoint.z = startCoordinateZ +