Commit 142c6f9c authored by maruggv's avatar maruggv
Browse files

update 'valentin'

parent b585bf58
......@@ -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)
......@@ -221,8 +228,71 @@ 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;
//TODO we might want to use float64 instead of float
// trajectory generation variables
double trajectory_start_time = 0;
double total_time_since_start = 0;
bool first_trajectory_calculation = true;
// Trajectory: xcf0 -> xm1 -> xm2 -> xms
// xcf0: inital(Button pressed) position of Crazyflie
// xm1: next Point after the Crazyflie: the Drone should reach the final height by xm1
// xm2: Point behind the mothership
// xms: Point on mothership where we will land
std::vector<float> xcf0(3);
std::vector<float> xm1(3);
std::vector<float> xm2(3);
std::vector<float> xms(3);
float trajectory_duration_1 = 0; // Duration of flight between xcf0 and xm1
float trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2
float trajectory_duration_3 = 0; // Duration of flight between xm2 and xms
float trajectory_t0 = 0; // Time when Button pressed to land on mothership
float trajectory_t1 = 0; // Time at xm1
float trajectory_t2 = 0; // Time at xm2
float trajectory_t3 = 0; // Time at xms
float xm1_x_distance = 0.5;
float xm2_distance_to_ms = 0.3;
// Calculate from approximate distance and velocity the total duration of the trajectory
float trajectory_velocity_of_CF = 1.0; // Fly at 1 m/s
float trajectory_total_distance = 0;
float trajectory_duration = 0;
// Origin of the Flying zone
float trajectory_setpoint[4]; // x,y,z,yaw
float trajectory_velocity[3]; // x,y,z
float originX = 0;
float originY = 0;
d_fall_pps::AreaBounds area;
float trajectory_x_radius = 0;
float trajectory_y_radius = 0;
float trajectory_period = 5; // 1 round in 5 sec
float thrust_factor = 1;
float pitchAngle_baseline = 1;
float rollAngle_baseline = 1;
//describes the area of the crazyflie and other parameters
......@@ -254,9 +324,9 @@ 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];
float prev_MS_pos[3] = {0,0,0};
float current_MS_pos[3];
float mothership_vel[3];
// THE FOLLOWING PARAMETERS ARE USED
......@@ -284,10 +354,12 @@ std::vector<float> gainMatrixPitchAngle (9,0.0);
// Integrator parameters
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);
// Feedforward gain (velocity -> angle)
std::vector<float> gainFeedforwardAnglefromVelocity (3,0.0);
// The 16-bit command limits
float cmd_sixteenbit_min;
......@@ -449,10 +521,22 @@ void buttonPressed_abort();
void buttonPressed_integrator_on();
void buttonPressed_integrator_off();
void buttonPressed_integrator_reset();
void buttonPressed_follow_trajectory();
void buttonPressed_reset();
void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context);
void motorsOFF(Controller::Response &response);
// Trajectory methods
void calculateTrajectory(Controller::Request &request);
std::vector<float> trajectory_to_ms(Controller::Request &request, double t);
float calculate_distance_in_xyz(std::vector<float> p1, std::vector<float> p2);
// CONTROLLER COMPUTATIONS
// > The function that is called to "start" all estimation and control computations
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
......@@ -470,7 +554,7 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12]
void calculateControlOutput_viaLQRforRates_forLanding( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
// Velocity estimator
void calculateDroneXVelocity(Controller::Request &request);
void calculateMSVelocity(Controller::Request &request);
// Integrator
void integrator_XYZ(float (&stateErrorBody)[12]);
......
......@@ -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;
......
......@@ -444,6 +444,13 @@ void loadCrazyflieContext() {
context = new_context;
ROS_INFO("[PPSClient] Publishing CrazyflieContext to DronexService");
dronexContextPublisher.publish(context);
ros::NodeHandle nh("CrazyRadio");
nh.setParam("crazyFlieAddress", context.crazyflieAddress);
}
......@@ -1091,6 +1098,10 @@ int main(int argc, char* argv[])
controller_setpoint.z = default_setpoint[2];
controller_setpoint.yaw = default_setpoint[3];
// Publish CrazyflieContext to DronexService
dronexContextPublisher = nodeHandle.advertise<d_fall_pps::CrazyflieContext>("CrazyflieContext", 1);
//ros::service::waitForService("/CentralManagerService/CentralManager");
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext();
......@@ -1117,6 +1128,8 @@ int main(int argc, char* argv[])
controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1);
// crazy radio status
crazyradio_status = DISCONNECTED;
......
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