Commit 9ce1df44 authored by mastefan's avatar mastefan
Browse files

added integrator (x,y,z) (not tested yet), buttons for integrator

parent 574f6b06
......@@ -145,6 +145,10 @@
#define DRONEX_LAND 9
#define DRONEX_ABORT 10
#define DRONEX_INTEGRATOR_ON 11
#define DRONEX_INTEGRATOR_OFF 12
#define DRONEX_INTEGRATOR_RESET 13
// Universal constants
#define PI 3.141592653589
......@@ -254,7 +258,9 @@ private slots:
void on_land_on_dronex_button_clicked();
void on_abort_dronex_button_clicked();
void on_integrate_on_dronex_button_clicked();
void on_integrate_off_dronex_button_clicked();
void on_integrate_reset_dronex_button_clicked();
......
......@@ -1965,6 +1965,19 @@ void MainWindow::on_abort_dronex_button_clicked(){
}
void MainWindow::on_integrate_on_dronex_button_clicked(){
send_dronex_button_clicked_message(DRONEX_INTEGRATOR_ON);
}
void MainWindow::on_integrate_off_dronex_button_clicked(){
send_dronex_button_clicked_message(DRONEX_INTEGRATOR_OFF);
}
void MainWindow::on_integrate_reset_dronex_button_clicked(){
send_dronex_button_clicked_message(DRONEX_INTEGRATOR_RESET);
}
......
......@@ -405,11 +405,20 @@
</attribute>
<layout class="QGridLayout" name="gridLayout_22">
<item row="0" column="0">
<layout class="QGridLayout" name="gridLayout_21">
<item row="0" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_14">
<layout class="QGridLayout" name="gridLayout_21" rowstretch="0,0,0,0,0">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="verticalSpacing">
<number>6</number>
</property>
<item row="1" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_14" stretch="1,0,1">
<item>
<layout class="QVBoxLayout" name="verticalLayout_10">
<layout class="QVBoxLayout" name="verticalLayout_10" stretch="0,0,0">
<item>
<widget class="QPushButton" name="take_off_dronex_button">
<property name="sizePolicy">
......@@ -483,6 +492,60 @@
</item>
</layout>
</item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_11">
<item>
<widget class="QPushButton" name="integrate_on_dronex_button">
<property name="text">
<string>Integrator ON</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="integrate_off_dronex_button">
<property name="text">
<string>Integrator OFF</string>
</property>
<property name="autoDefault">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="integrate_reset_dronex_button">
<property name="text">
<string>Integrator RESET</string>
</property>
</widget>
</item>
<item>
<spacer name="verticalSpacer_8">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</item>
</layout>
......
......@@ -111,7 +111,9 @@
#define DRONEX_STATE_LAND_ON_MOTHERSHIP 5
#define DRONEX_STATE_ON_MOTHERSHIP 6
#define INTEGRATOR_ON 1
#define INTEGRATOR_OFF 2
#define INTEGRATOR_RESET 3
// These constants define the modes that can be used for controller the Crazyflie 2.0,
// the constants defined here need to be in agreement with those defined in the
......@@ -184,6 +186,9 @@ float m_mass_CF_grams;
Setpoint dronexSetpoint;
int flying_state = DRONEX_STATE_GROUND;
int integratorFlag = INTEGRATOR_OFF;
float integrator_var[3] = {0,0,0};
//describes the area of the crazyflie and other parameters
//CrazyflieContext context;
......@@ -229,6 +234,9 @@ std::vector<float> gainMatrixRollRate (9,0.0);
std::vector<float> gainMatrixPitchRate (9,0.0);
std::vector<float> gainMatrixYawRate (9,0.0);
// Integrator parameters
std::vector<float> gainIntegrator (3,0.0);
// The 16-bit command limits
float cmd_sixteenbit_min;
float cmd_sixteenbit_max;
......@@ -385,6 +393,9 @@ void buttonPressed_take_off();
void buttonPressed_land();
void buttonPressed_abort();
void buttonPressed_integrator_on();
void buttonPressed_integrator_off();
void buttonPressed_integrator_reset();
void motorsOFF(Controller::Response &response);
......
......@@ -82,3 +82,6 @@ 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]
# for our integrator (so far just random values)
gainIntegrator : [0.01, 0.01, 0.01]
......@@ -192,21 +192,21 @@ void perControlCycleOperations()
void buttonPressed_take_off(){
if(flying_state == DRONEX_STATE_GROUND || flying_state == DRONEX_STATE_ON_MOTHERSHIP){
//if(flying_state == DRONEX_STATE_GROUND || flying_state == DRONEX_STATE_ON_MOTHERSHIP){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Taking off...");
flying_state = DRONEX_STATE_TAKING_OFF;
}else{
ROS_ERROR("Cannot change to DRONEX_STATE_TAKING_OFF");
}
//}else{
// ROS_ERROR("Cannot change to DRONEX_STATE_TAKING_OFF");
//}
}
void buttonPressed_land(){
if(flying_state == DRONEX_STATE_HOVER){
//if(flying_state == DRONEX_STATE_HOVER){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Landing...");
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
}else{
ROS_ERROR("Cannot change to DRONEX_STATE_LAND_ON_MOTHERSHIP");
}
//}else{
// ROS_ERROR("Cannot change to DRONEX_STATE_LAND_ON_MOTHERSHIP");
//}
// flying_state = DRONEX_STATE_APPROACHING; iff already took off
......@@ -217,6 +217,21 @@ void buttonPressed_abort(){
flying_state = DRONEX_STATE_LAND_ON_GROUND;
}
void buttonPressed_integrator_on(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Turn ON integrator");
integratorFlag = INTEGRATOR_ON;
}
void buttonPressed_integrator_off(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Turn OFF integrator");
integratorFlag = INTEGRATOR_OFF;
}
void buttonPressed_integrator_reset(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET integrator to zero");
integratorFlag = INTEGRATOR_RESET;
}
......@@ -652,6 +667,29 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
float yawRate_forResponse = 0;
float thrustAdjustment = 0;
//integrator for x,y,z
if(integratorFlag == INTEGRATOR_ON)
{
for(int i=0; i < 3; i++)
{
integrator_var[i] -= stateErrorBody[i]*(1.0/control_frequency) + integrator_var[i]*(1.0/(1.0-control_frequency));
}
}
if(integratorFlag == INTEGRATOR_RESET)
{
for(int i=0; i < 3; i++)
{
integrator_var[i] = 0;
}
}
for(int i=0; i < 3; i++)
{
stateErrorBody[i] += integrator_var[i] * gainIntegrator[i];
}
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 9; ++i)
{
......@@ -1187,6 +1225,10 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_dronexController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// for our integrator
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegrator", gainIntegrator, 3);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[DRONEX CONTROLLER] DEBUGGING: the fetched DroneXController/mass_CF = " << m_mass_CF_grams);
......
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