Commit 7c93c2f2 authored by beuchatp's avatar beuchatp
Browse files

Mostly finished with converting the Flying Agent Client to the Default...

Mostly finished with converting the Flying Agent Client to the Default controller. Needs comoiltion testing. Next step is to make the Default controller able to perform take-off and land manoeuvres
parent 61ffeb02
......@@ -492,66 +492,6 @@
</property>
</spacer>
</item>
<item row="3" column="0">
<widget class="QLineEdit" name="lineEdit_measured_x">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLineEdit" name="lineEdit_measured_y">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_row_y">
<property name="maximumSize">
......@@ -568,126 +508,6 @@
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QLineEdit" name="lineEdit_measured_pitch">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLineEdit" name="lineEdit_measured_yaw">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLineEdit" name="lineEdit_measured_roll">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLineEdit" name="lineEdit_measured_z">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_row_pitch">
<property name="maximumSize">
......@@ -1379,6 +1199,210 @@
</property>
</spacer>
</item>
<item row="3" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_6">
<item>
<widget class="QLineEdit" name="lineEdit_measured_x">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item row="4" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_7">
<item>
<widget class="QLineEdit" name="lineEdit_measured_y">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item row="5" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_8">
<item>
<widget class="QLineEdit" name="lineEdit_measured_z">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item row="7" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_9">
<item>
<widget class="QLineEdit" name="lineEdit_measured_pitch">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item row="6" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_10">
<item>
<widget class="QLineEdit" name="lineEdit_measured_yaw">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item row="8" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_11">
<item>
<widget class="QLineEdit" name="lineEdit_measured_roll">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
<family>Courier</family>
</font>
</property>
<property name="text">
<string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="14" column="0">
......
......@@ -60,6 +60,7 @@
// Include the shared definitions
#include "nodes/Constants.h"
#include "nodes/DefaultControllerConstants.h"
// SPECIFY THE PACKAGE NAMESPACE
//using namespace dfall_pkg;
......
......@@ -47,6 +47,9 @@ DefaultControllerTab::DefaultControllerTab(QWidget *parent) :
ui->red_frame_position_left->setVisible(false);
ui->red_frame_position_right->setVisible(false);
// Make the current state label blank
ui->label_current_state->setText("");
#ifdef CATKIN_MAKE
......@@ -249,6 +252,52 @@ void DefaultControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWith
if (yaw < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 3));
// UPDATE THE CURRENT STATE LABEL
int current_state = newSetpoint.buttonID;
switch (current_state)
{
case DEFAULT_CONTROLLER_STATE_STANDBY:
{
ui->label_current_state->setText("standby");
break;
}
case DEFAULT_CONTROLLER_STATE_TAKE_OFF_SPIN_MOTORS:
{
ui->label_current_state->setText("Take-off, spinning motors");
break;
}
case DEFAULT_CONTROLLER_STATE_TAKE_OFF_MOVE_UP:
{
ui->label_current_state->setText("Take-off, moving up");
break;
}
case DEFAULT_CONTROLLER_STATE_TAKE_OFF_GOTO_SETPOINT:
{
ui->label_current_state->setText("Take-off, goto setpoint");
break;
}
case DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN:
{
ui->label_current_state->setText("Landing, move down");
break;
}
case DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS:
{
ui->label_current_state->setText("Landing, spinning motors");
break;
}
case DEFAULT_CONTROLLER_STATE_UNKNOWN:
{
ui->label_current_state->setText("Unknown");
break;
}
default:
{
ui->label_current_state->setText("Not recognised");
break;
}
}
}
#endif
......@@ -801,4 +850,4 @@ bool DefaultControllerTab::getTypeAndIDParameters()
// Return
return return_was_successful;
}
#endif
\ No newline at end of file
#endif
// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Constants that are used across the Default Controler
// Service, its GUI, and the Flying Agent Client
//
// ----------------------------------------------------------------------------------
// TO REQUEST MANOEUVRES
#define DEFAULT_CONTROLLER_REQUEST_TAKE_OFF 1
#define DEFAULT_CONTROLLER_REQUEST_LANDING 2
// TO IDENITFY THE STATE OF THE DEFAULT CONTROLLER
#define DEFAULT_CONTROLLER_STATE_UNKNOWN -1
#define DEFAULT_CONTROLLER_STATE_STANDBY 99
// > The sequence of states for a TAKE-OFF manoeuvre
#define DEFAULT_CONTROLLER_STATE_TAKE_OFF_SPIN_MOTORS 11
#define DEFAULT_CONTROLLER_STATE_TAKE_OFF_MOVE_UP 12
#define DEFAULT_CONTROLLER_STATE_TAKE_OFF_GOTO_SETPOINT 13
// > The sequence of states for a LANDING manoeuvre
#define DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN 21
#define DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS 22
\ No newline at end of file
......@@ -72,6 +72,7 @@
// Include the shared definitions
#include "nodes/Constants.h"
#include "nodes/DefaultControllerConstants.h"
// Include other classes
#include "classes/GetParamtersAndNamespaces.h"
......@@ -114,6 +115,13 @@ using namespace dfall_pkg;
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
// The current state of the Default Controller
int m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;
// The elapased time, incremented by counting the motion
// capture callbacks
float m_time_in_seconds = 0.0;
// The ID of the agent that this node is monitoring
int m_agentID;
......@@ -140,6 +148,7 @@ std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
// > the frequency at which the controller is running
float yaml_control_frequency = 200.0;
float m_control_deltaT = (1.0/200.0);
// > the min and max for saturating 16 bit thrust commands
float yaml_command_sixteenbit_min = 1000;
......@@ -172,8 +181,8 @@ std::vector<float> m_gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.
// ROS Publisher for debugging variables
ros::Publisher m_debugPublisher;
// ROS Publisher for inform the network about
// changes to the setpoin
// ROS Publisher to inform the network about
// changes to the setpoint
ros::Publisher m_setpointChangedPublisher;
......@@ -201,6 +210,9 @@ ros::Publisher m_setpointChangedPublisher;
// need to written below in an order that ensure each function is
// implemented before it is called from another function)
// CALLBACK FOR THE REQUEST MANOEUVRE SERVICE
bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Response &response);
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
......@@ -220,6 +232,9 @@ void setNewSetpoint(float x, float y, float z, float yaw);
// GET CURRENT SETPOINT SERVICE CALLBACK
bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);