To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit a1e3233b authored by pragash1's avatar pragash1
Browse files

Added Follow Trajectory Button and Reset button

Reset Button not implemented

Added new Controller mode for trajectory tracking
Added elliptic trajectory calculation
parent 65db71f1
......@@ -148,7 +148,8 @@
#define DRONEX_INTEGRATOR_ON 15
#define DRONEX_INTEGRATOR_OFF 16
#define DRONEX_INTEGRATOR_RESET 17
#define DRONEX_FOLLOW_TRAJECTORY 18
#define DRONEX_RESET 19
// Universal constants
#define PI 3.141592653589
......@@ -257,12 +258,15 @@ private slots:
void on_take_off_dronex_button_clicked();
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();
void on_set_weigth_param_clicked();
void on_set_pitch_baseline_param_clicked();
void on_follow_dronex_trajectory_button_clicked();
void on_dronex_reset_button_clicked();
private:
Ui::MainWindow *ui;
......
......@@ -121,7 +121,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
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 );
......@@ -2011,6 +2011,14 @@ void MainWindow::on_integrate_reset_dronex_button_clicked(){
send_dronex_button_clicked_message(DRONEX_INTEGRATOR_RESET);
}
void MainWindow::on_follow_dronex_trajectory_button_clicked(){
send_dronex_button_clicked_message(DRONEX_FOLLOW_TRAJECTORY);
}
void MainWindow::on_dronex_reset_button_clicked(){
send_dronex_button_clicked_message(DRONEX_RESET);
}
......
......@@ -418,7 +418,7 @@
<item row="1" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_14" stretch="1,0,0">
<item>
<layout class="QVBoxLayout" name="verticalLayout_10" stretch="0,0,0">
<layout class="QVBoxLayout" name="verticalLayout_10" stretch="0,0,0,0,0">
<item>
<widget class="QPushButton" name="take_off_dronex_button">
<property name="sizePolicy">
......@@ -462,6 +462,9 @@
</item>
<item>
<widget class="QPushButton" name="abort_dronex_button">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Ignored">
<horstretch>0</horstretch>
......@@ -490,6 +493,32 @@
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="follow_dronex_trajectory_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Ignored">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Follow Trajectory</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="dronex_reset_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Ignored">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Reset</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
......
......@@ -57,6 +57,7 @@
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h"
#include "d_fall_pps/CrazyflieContext.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
......@@ -103,17 +104,20 @@
#define DRONEX_BUTTON_3 13
#define DRONEX_BUTTON_4 14
#define DRONEX_INTEGRATOR_ON 15
#define DRONEX_INTEGRATOR_OFF 16
#define DRONEX_INTEGRATOR_RESET 17
#define DRONEX_INTEGRATOR_ON 15
#define DRONEX_INTEGRATOR_OFF 16
#define DRONEX_INTEGRATOR_RESET 17
#define DRONEX_FOLLOW_TRAJECTORY 18
#define DRONEX_RESET 19
#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
#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
#define DRONEX_STATE_FOLLOWING_TRAJECTORY 7
......@@ -200,7 +204,10 @@ float integrator_sum_XYZ[3] = {0,0,0};
// Controller Mode
int controller_mode = 1;
// 0:
// 1: Nested Controller
// 2: trajectory Follower
int controller_mode = 2;
// Variable for choosing flight sequence
int flightSequence = 0;
......@@ -213,7 +220,7 @@ bool landedFlag = false;
// Flag for saving starting coordinates
bool savedStartCoordinates = false;
float startCoordinateX;
float startCoordinateY;
float startCoordinateY;
float startCoordinateZ;
// tolerances (x,y,z)
......@@ -224,6 +231,34 @@ float tol_land[3] = {0.03, 0.03, 0.03}; // *
// Origin of the Flying zone
float trajectory_setpoint[4]; // x,y,z,yaw
float originX = 0;
float originY = 0;
d_fall_pps::AreaBounds area;
double trajectory_start_time = 0;
double total_time_since_start = 0;
float trajectory_x_radius = 0;
float trajectory_y_radius = 0;
float trajectory_period = 10; // 1 round in 10 sec
//describes the area of the crazyflie and other parameters
//CrazyflieContext context;
......@@ -448,8 +483,13 @@ void buttonPressed_abort();
void buttonPressed_integrator_on();
void buttonPressed_integrator_off();
void buttonPressed_integrator_reset();
void buttonPressed_follow_trajectory();
void buttonPressed_reset();
void calculateTrajectory();
void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context);
void motorsOFF(Controller::Response &response);
// CONTROLLER COMPUTATIONS
......
......@@ -210,7 +210,8 @@ ros::Publisher commandPublisher;
// communication with crazyRadio node. Connect and disconnect
ros::Publisher crazyRadioCommandPublisher;
// publish CrazyflieData To droneXService
ros::Publisher dronexContextPublisher;
// Variable for the namespaces for the paramter services
// > For the paramter service of this agent
std::string namespace_to_own_agent_parameter_service;
......
......@@ -14,7 +14,8 @@ vicon_frequency : 200
# controller_mode : 0: Basic Controller
# controller_mode : 1: Angle Controller
controller_mode : 1
# controller_mode : 2: Trajectory Controller
controller_mode : 2
......@@ -106,4 +107,4 @@ gainMatrixPitchAngle : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00]
#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
gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30]
......@@ -204,7 +204,7 @@ void buttonPressed_land(){
//if(flying_state == DRONEX_STATE_HOVER){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Start flight-sequence LA...");
// OLD:flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
// NEW:
// NEW:
flightSequence = SEQUENCE_LAND_ON_MOTHERSHIP;
}
......@@ -231,6 +231,29 @@ void buttonPressed_integrator_reset(){
integratorFlag = DRONEX_INTEGRATOR_RESET;
}
void buttonPressed_follow_trajectory(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] FOLLOW trajectory");
flightSequence = SEQUENCE_NONE;
flying_state = DRONEX_FOLLOW_TRAJECTORY;
total_time_since_start = 0;
trajectory_x_radius = 0;
trajectory_y_radius = 0;
trajectory_start_time = ros::Time::now().toSec();
}
void buttonPressed_reset(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET");
}
void integratorCallback (const Setpoint& integrParams ) {
integrator_sum_XYZ[0] = integrParams.x;
integrator_sum_XYZ[1] = integrParams.y;
......@@ -244,7 +267,7 @@ void WeightParamCallback (const Setpoint& weightParam ) {
}
void PitchBaselineParamCallback(const Setpoint& pitchAngleParam ) {
// TODO
// TODO
}
......@@ -380,9 +403,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
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 << ", "
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_approach[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_approach[1] &&
......@@ -471,16 +494,16 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
ROS_INFO_STREAM("x = " << startCoordinateX);
ROS_INFO_STREAM("y = " << startCoordinateY);
ROS_INFO_STREAM("z = " << startCoordinateZ);
}
dronexSetpoint.x = startCoordinateX;
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.4;
ROS_INFO_STREAM("TO: (x,y,z) Difference: ("
<< request.ownCrazyflie.x-dronexSetpoint.x << ", "
<< request.ownCrazyflie.y-dronexSetpoint.y << ", "
ROS_INFO_STREAM("TO: (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_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] &&
......@@ -496,7 +519,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_HOVER:
{
//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
// keep setpoint constant
/*
dronexSetpoint.x = dronexSetpoint.x;
......@@ -506,12 +529,16 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
break;
case DRONEX_FOLLOW_TRAJECTORY:
break;
} // END switch case
// flightSeqeunce 1: simple approaching and landing on static mothership
if (flightSequence == SEQUENCE_LAND_ON_MOTHERSHIP){
//ROS_INFO_STREAM("Entering: DRONEX_STATE_TAKING_OFF");
......@@ -580,7 +607,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
performEstimatorUpdate_forStateInterial(request);
......@@ -594,7 +621,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
flying_state = DRONEX_STATE_GROUND;
}
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (abs(request.ownCrazyflie.x - request.otherCrazyflies[0].x) < tol_land[0]) &&
(abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_land[1]) &&
(abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_land[1]) &&
(abs(request.ownCrazyflie.z - 0.03 - request.otherCrazyflies[0].z) < tol_land[2]) ){
ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
flying_state = DRONEX_STATE_ON_MOTHERSHIP;
......@@ -629,7 +656,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
return true;
}
void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context){
area = context.localArea;
originX = (area.xmin + area.xmax) / 2.0;
originY = (area.ymin + area.ymax) / 2.0;
ROS_INFO_STREAM("New OriginX: " << originX << " New OriginY: " << originY);
}
// State Error Body
......@@ -650,7 +683,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// DroneX Controller
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response){
if(controller_mode == 0){
......@@ -666,10 +699,10 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
integrator_XYZ(current_bodyFrameError);
calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
}else{
}else if(controller_mode == 1){
// LQR for Angles
......@@ -697,7 +730,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
ms_velocity[0] = drone_X_vel[0];
ms_velocity[1] = drone_X_vel[1];
ms_velocity[2] = drone_X_vel[2];
// CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
......@@ -706,12 +739,12 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
float stateErrorBody[12];
// > Call the function to perform the conversion
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, ms_coordinates, stateErrorBody);
float rollAngle_forResponse = 0;
float pitchAngle_forResponse = 0;
float thrustAdjustment = 0;
// TODO: Do not forget to implement the yawController
float yawAngle_forResponse = 0;
......@@ -726,7 +759,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
pitchAngle_forResponse -= gainIntegratorAngle[1] * integrator_sum_XYZ[0];
// > ALITUDE CONTROLLER (i.e., z-controller):
thrustAdjustment -= gainIntegratorAngle[2] * integrator_sum_XYZ[2];
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 6; ++i){
......@@ -740,18 +773,18 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
}
// Calculate the angle in x-y plane of DroneX for transformation to local coordinates
float droneXAngle = ;
ms_velocity_norm = sqrt(ms_velocity[0]^2 * ms_velocity[1]^2);
float droneXAngle;
float ms_velocity_norm = sqrt(pow(ms_velocity[0], 2.0) + pow(ms_velocity[1], 2.0)); //(ms_velocity[0])^2 * (ms_velocity[1])^2);
if(ms_velocity_norm != 0){
if (ms_velocity[1] >= 0)
{
droneXAngle = arccos(ms_velocity[0] / ms_velocity_norm);
droneXAngle = acos(ms_velocity[0] / ms_velocity_norm);
}else{
droneXAngle = -arccos(ms_velocity[0] / ms_velocity_norm);
droneXAngle = -acos(ms_velocity[0] / ms_velocity_norm);
}
}
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership
float rollAngle_baseline = 1;
float pitchAngle_baseline = 1;
......@@ -857,9 +890,196 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
}*/
}else if(controller_mode == 2){// Trajectory Follower
//TODO
// Pragash: I've added this controller mode 2 to implement the trajectory tracking in the future
// At the moment it's the same as in controller mode 1
// LQR for Angles
// Read Mothership coordinates
// x,y,z,yaw
float ms_coordinates[4];
/*ms_coordinates[0] = request.otherCrazyflies[0].x;
ms_coordinates[1] = request.otherCrazyflies[0].y;
ms_coordinates[2] = request.otherCrazyflies[0].z;
ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/
/*ms_coordinates[0] = dronexSetpoint.x;
ms_coordinates[1] = dronexSetpoint.y;
ms_coordinates[2] = dronexSetpoint.z;
ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/
if(flying_state == DRONEX_FOLLOW_TRAJECTORY){
calculateTrajectory();
ms_coordinates[0] = trajectory_setpoint[0];
ms_coordinates[1] = trajectory_setpoint[1];
ms_coordinates[2] = trajectory_setpoint[2];
ms_coordinates[3] = trajectory_setpoint[3];
}else{
ms_coordinates[0] = m_setpoint_for_controller[0];
ms_coordinates[1] = m_setpoint_for_controller[1];
ms_coordinates[2] = m_setpoint_for_controller[2];
ms_coordinates[3] = request.otherCrazyflies[0].yaw;
}
// Load Mothership velocity
// x dot, y dot, z dot
float ms_velocity[3];
ms_velocity[0] = drone_X_vel[0];
ms_velocity[1] = drone_X_vel[1];
ms_velocity[2] = drone_X_vel[2];
// CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
// THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
// > Define a local array to fill in with the body frame error
float stateErrorBody[12];
// > Call the function to perform the conversion
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, ms_coordinates, stateErrorBody);
float rollAngle_forResponse = 0;
float pitchAngle_forResponse = 0;
float thrustAdjustment = 0;
// TODO: Do not forget to implement the yawController
float yawAngle_forResponse = 0;
integrator_XYZ(stateErrorBody);
// integrator
// BODY FRAME Y CONTROLLER
rollAngle_forResponse -= gainIntegratorAngle[0] * integrator_sum_XYZ[1];
// BODY FRAME X CONTROLLER
pitchAngle_forResponse -= gainIntegratorAngle[1] * integrator_sum_XYZ[0];
// > ALITUDE CONTROLLER (i.e., z-controller):
thrustAdjustment -= gainIntegratorAngle[2] * integrator_sum_XYZ[2];
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 6; ++i){
// BODY FRAME Y CONTROLLER
rollAngle_forResponse -= 1.1*gainMatrixRollAngle[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
pitchAngle_forResponse -= 1.1*gainMatrixPitchAngle[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
//thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
}
// Calculate the angle in x-y plane of DroneX for transformation to local coordinates
float droneXAngle;
float ms_velocity_norm = sqrt(pow(ms_velocity[0], 2.0) + pow(ms_velocity[1], 2.0)); //(ms_velocity[0])^2 * (ms_velocity[1])^2);
if(ms_velocity_norm != 0){
if (ms_velocity[1] >= 0)
{
droneXAngle = acos(ms_velocity[0] / ms_velocity_norm);
}else{
droneXAngle = -acos(ms_velocity[0] / ms_velocity_norm);
}
}
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership
float rollAngle_baseline = 1;
float pitchAngle_baseline = 1;
// TODO
rollAngle_forResponse += rollAngle_baseline * (cos(droneXAngle) * ms_velocity[0]- sin(droneXAngle) * ms_velocity[1]);
pitchAngle_forResponse += pitchAngle_forResponse * (sin(droneXAngle) * ms_velocity[0] + cos(droneXAngle) * ms_velocity[1]);
// Calculate the Force Feedforward
float F_in_newtons = (gravity * m_mass_total_grams)/(cos(rollAngle_forResponse)*cos(pitchAngle_forResponse)*1000.0);
// LQR for Rates
// Calculate the Roll and Pitch Angle error
float AngleError[3] = {
stateErrorBody[6] - rollAngle_forResponse,
stateErrorBody[7] - pitchAngle_forResponse,
stateErrorBody[8]
};
float rollRate_forResponse = 0;
float pitchRate_forResponse = 0;
float yawRate_forResponse = 0;
for(int i = 0; i < 4; i++){
rollRate_forResponse -= gainMatrixRollRatefromAngle[i] * AngleError[i];
pitchRate_forResponse -= gainMatrixPitchRatefromAngle[i] * AngleError[i];
yawRate_forResponse -= gainMatrixYawRatefromAngle[i] * AngleError[i];
}
/*for(int i = 0; i < 9; ++i){
thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
}*/
for(int i = 0; i < 6; ++i){
thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
}