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

update 'valentin'

parent b585bf58
...@@ -57,6 +57,7 @@ ...@@ -57,6 +57,7 @@
#include "d_fall_pps/Controller.h" #include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h" #include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h" #include "d_fall_pps/CustomButton.h"
#include "d_fall_pps/CrazyflieContext.h"
// Include the Parameter Service shared definitions // Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h" #include "nodes/ParameterServiceDefinitions.h"
...@@ -103,17 +104,20 @@ ...@@ -103,17 +104,20 @@
#define DRONEX_BUTTON_3 13 #define DRONEX_BUTTON_3 13
#define DRONEX_BUTTON_4 14 #define DRONEX_BUTTON_4 14
#define DRONEX_INTEGRATOR_ON 15 #define DRONEX_INTEGRATOR_ON 15
#define DRONEX_INTEGRATOR_OFF 16 #define DRONEX_INTEGRATOR_OFF 16
#define DRONEX_INTEGRATOR_RESET 17 #define DRONEX_INTEGRATOR_RESET 17
#define DRONEX_FOLLOW_TRAJECTORY 18
#define DRONEX_RESET 19
#define DRONEX_STATE_GROUND 0 #define DRONEX_STATE_GROUND 0
#define DRONEX_STATE_TAKING_OFF 1 #define DRONEX_STATE_TAKING_OFF 1
#define DRONEX_STATE_LAND_ON_GROUND 2 #define DRONEX_STATE_LAND_ON_GROUND 2
#define DRONEX_STATE_APPROACH 3 #define DRONEX_STATE_APPROACH 3
#define DRONEX_STATE_HOVER 4 #define DRONEX_STATE_HOVER 4
#define DRONEX_STATE_LAND_ON_MOTHERSHIP 5 #define DRONEX_STATE_LAND_ON_MOTHERSHIP 5
#define DRONEX_STATE_ON_MOTHERSHIP 6 #define DRONEX_STATE_ON_MOTHERSHIP 6
#define DRONEX_STATE_FOLLOWING_TRAJECTORY 7
...@@ -200,7 +204,10 @@ float integrator_sum_XYZ[3] = {0,0,0}; ...@@ -200,7 +204,10 @@ float integrator_sum_XYZ[3] = {0,0,0};
// Controller Mode // Controller Mode
int controller_mode = 1; // 0:
// 1: Nested Controller
// 2: trajectory Follower
int controller_mode = 2;
// Variable for choosing flight sequence // Variable for choosing flight sequence
int flightSequence = 0; int flightSequence = 0;
...@@ -213,7 +220,7 @@ bool landedFlag = false; ...@@ -213,7 +220,7 @@ bool landedFlag = false;
// Flag for saving starting coordinates // Flag for saving starting coordinates
bool savedStartCoordinates = false; bool savedStartCoordinates = false;
float startCoordinateX; float startCoordinateX;
float startCoordinateY; float startCoordinateY;
float startCoordinateZ; float startCoordinateZ;
// tolerances (x,y,z) // tolerances (x,y,z)
...@@ -221,8 +228,71 @@ float tol_takeoff[3] = {0.07, 0.07, 0.07}; //{0.03, 0.03, 0.03}; ...@@ -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_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 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 //describes the area of the crazyflie and other parameters
...@@ -254,9 +324,9 @@ float m_max_setpoint_change_per_second_yaw_radians; ...@@ -254,9 +324,9 @@ float m_max_setpoint_change_per_second_yaw_radians;
float m_vicon_frequency; float m_vicon_frequency;
float prev_DroneX_pos[3] = {0,0,0}; float prev_MS_pos[3] = {0,0,0};
float current_DroneX_pos[3]; float current_MS_pos[3];
float drone_X_vel[3]; float mothership_vel[3];
// THE FOLLOWING PARAMETERS ARE USED // THE FOLLOWING PARAMETERS ARE USED
...@@ -284,10 +354,12 @@ std::vector<float> gainMatrixPitchAngle (9,0.0); ...@@ -284,10 +354,12 @@ std::vector<float> gainMatrixPitchAngle (9,0.0);
// Integrator parameters // 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> gainIntegratorRate (3,0.0);
std::vector<float> gainIntegratorAngle (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 // The 16-bit command limits
float cmd_sixteenbit_min; float cmd_sixteenbit_min;
...@@ -449,10 +521,22 @@ void buttonPressed_abort(); ...@@ -449,10 +521,22 @@ void buttonPressed_abort();
void buttonPressed_integrator_on(); void buttonPressed_integrator_on();
void buttonPressed_integrator_off(); void buttonPressed_integrator_off();
void buttonPressed_integrator_reset(); void buttonPressed_integrator_reset();
void buttonPressed_follow_trajectory();
void buttonPressed_reset();
void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context);
void motorsOFF(Controller::Response &response); 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 // CONTROLLER COMPUTATIONS
// > The function that is called to "start" all estimation and control computations // > The function that is called to "start" all estimation and control computations
bool calculateControlOutput(Controller::Request &request, Controller::Response &response); bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
...@@ -470,7 +554,7 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12] ...@@ -470,7 +554,7 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12]
void calculateControlOutput_viaLQRforRates_forLanding( float stateErrorBody[12], Controller::Request &request, Controller::Response &response); void calculateControlOutput_viaLQRforRates_forLanding( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
// Velocity estimator // Velocity estimator
void calculateDroneXVelocity(Controller::Request &request); void calculateMSVelocity(Controller::Request &request);
// Integrator // Integrator
void integrator_XYZ(float (&stateErrorBody)[12]); void integrator_XYZ(float (&stateErrorBody)[12]);
......
...@@ -210,7 +210,8 @@ ros::Publisher commandPublisher; ...@@ -210,7 +210,8 @@ ros::Publisher commandPublisher;
// communication with crazyRadio node. Connect and disconnect // communication with crazyRadio node. Connect and disconnect
ros::Publisher crazyRadioCommandPublisher; ros::Publisher crazyRadioCommandPublisher;
// publish CrazyflieData To droneXService
ros::Publisher dronexContextPublisher;
// Variable for the namespaces for the paramter services // Variable for the namespaces for the paramter services
// > For the paramter service of this agent // > For the paramter service of this agent
std::string namespace_to_own_agent_parameter_service; std::string namespace_to_own_agent_parameter_service;
......
...@@ -444,6 +444,13 @@ void loadCrazyflieContext() { ...@@ -444,6 +444,13 @@ void loadCrazyflieContext() {
context = new_context; context = new_context;
ROS_INFO("[PPSClient] Publishing CrazyflieContext to DronexService");
dronexContextPublisher.publish(context);
ros::NodeHandle nh("CrazyRadio"); ros::NodeHandle nh("CrazyRadio");
nh.setParam("crazyFlieAddress", context.crazyflieAddress); nh.setParam("crazyFlieAddress", context.crazyflieAddress);
} }
...@@ -1091,6 +1098,10 @@ int main(int argc, char* argv[]) ...@@ -1091,6 +1098,10 @@ int main(int argc, char* argv[])
controller_setpoint.z = default_setpoint[2]; controller_setpoint.z = default_setpoint[2];
controller_setpoint.yaw = default_setpoint[3]; controller_setpoint.yaw = default_setpoint[3];
// Publish CrazyflieContext to DronexService
dronexContextPublisher = nodeHandle.advertise<d_fall_pps::CrazyflieContext>("CrazyflieContext", 1);
//ros::service::waitForService("/CentralManagerService/CentralManager"); //ros::service::waitForService("/CentralManagerService/CentralManager");
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false); centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext(); loadCrazyflieContext();
...@@ -1117,6 +1128,8 @@ int main(int argc, char* argv[]) ...@@ -1117,6 +1128,8 @@ int main(int argc, char* argv[])
controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1); controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1);
// crazy radio status // crazy radio status
crazyradio_status = DISCONNECTED; 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