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 bf3ab2c3 authored by roangel's avatar roangel
Browse files

cleaned a bit, added optimization level -O3. Now we have more time to solve the MPC problem

parent 6563d5cc
cmake_minimum_required(VERSION 3.0)
project(d_fall_pps)
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
set(CMAKE_CXX_FLAGS "-Wall -Wextra")
set(CMAKE_CXX_FLAGS_DEBUG "-g")
set(CMAKE_CXX_FLAGS_RELEASE "-O3")
## Add support for C++11, supported in ROS Kinetic and newer
# add_definitions(-std=c++11)
......
# Mass of the crazyflie
mass : 29
mass : 30
# Frequency of the controller, in hertz
vicon_frequency : 200
......@@ -92,6 +92,7 @@ gainMatrixPitchAngle : [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
gainMatrixRollAngle_50Hz : [ 0.00,-1.1583, 0.00, 0.00,-0.6278, 0.00]
gainMatrixPitchAngle_50Hz : [ 1.1583, 0.00, 0.00, 0.6278, 0.00, 0.00]
gainMatrixThrust_SixStateVector_50Hz: [ 0.00, 0.00, 0.1255, 0.00, 0.00, 0.0912]
# gainMatrixThrust_SixStateVector_50Hz: [ 0.00, 0.00, 0.95, 0.00, 0.00, 0.2]
gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
......
......@@ -7,9 +7,9 @@ control_frequency : 200
# Quadratic motor regression equation (a0, a1, a2)
motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
gainMatrixRollRate_Nested : [ 7.00, 0.00, 0.00]
gainMatrixPitchRate_Nested : [ 0.00, 7.00, 0.00]
gainMatrixYawRate_Nested : [ 0.00, 0.00, 4.30]
gainMatrixRollRate_Nested : [ 5.00, 0.00, 0.00]
gainMatrixPitchRate_Nested : [ 0.00, 5.00, 0.00]
gainMatrixYawRate_Nested : [ 0.00, 0.00, 3.30]
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > For the (x,y,z) position
......
......@@ -201,7 +201,7 @@ std::atomic<array<float,N_x>> shared_x0;
std::mutex MPC_mutex;
int N = 7;
int N = 15;
// ROS Publisher for debugging variables
......@@ -563,38 +563,13 @@ void do_MPC(int* publish_rate)
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
// debugMsg.vicon_x = request.ownCrazyflie.x;
// debugPublisher.publish(debugMsg);
// end_time = clock();
// double elapsed_secs = double(end_time - begin_time) / CLOCKS_PER_SEC;
// std::cout << "elapsed secs: " << elapsed_secs << std::endl;
// begin_time = clock();
// double elapsed_secs = ros::Time::now().toSec() - begin_t.toSec();
// std::cout << "elapsed secs: " << elapsed_secs << std::endl;
// begin_t = ros::Time::now();
// This is the "start" of the outer loop controller, add all your control
// computation here, or you may find it convienient to create functions
// to keep you code cleaner
// Define a local array to fill in with the state error
perform_estimator_update_state_interial(request, stateInertialEstimate);
// The estimate of the state is inside stateInertialEstimate now. Next, MPC
// Should we convert into body frame for our MPC controller? Let's skip it for now, YAW is going to be zero in our case
// NO, we dont have to convert into body frame, that was only when the linearization was done around hover with yaw = 0.
// we relinearize in every point
// read new x0
MPC_mutex.lock();
......@@ -606,44 +581,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
stateInertialEstimate[5];
MPC_mutex.unlock();
if(outer_loop_counter == 0)
{
// ================= MPC PART ==================
// Make sure we run it at 50Hz
// first, we have full state measurement stored in stateInertialEstimate.
// Apply setpoint change first, and store everything in x0 vector
// std::cout << "roll_MPC_input: " << raw_input[0] << std::endl;
// std::cout << "pitch_MPC_input: " << raw_input[1] << std::endl;
// std::cout << "yaw_MPC_input: " << raw_input[2] << std::endl;
// std::cout << "ft_MPC_input: " << raw_input[3] << std::endl;
// Test zone
// float correction_z = 0.82*(0.4 - stateInertialEstimate[2]) + 0.22 * (0 - stateInertialEstimate[5]) + params.m*params.g;
// float correction_roll = -0.31*(0-stateInertialEstimate[1]) - 0.25 * (0 - stateInertialEstimate[4]);
// raw_input[0] = correction_roll;
// raw_input[1] = setpoint[0];
// raw_input[2] = 0;
// raw_input[3] = correction_z;
}
outer_loop_counter++;
// wrap the counter:
if(outer_loop_counter >= 4) //200 Hz/4 = 50 Hz
outer_loop_counter = 0;
// read raw_input from shared_raw_input atomic variable
float local_input[4];
......@@ -654,19 +591,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
local_input[3] = raw_input[3];
MPC_mutex.unlock();
// ============= END MPC PART ===============
// just for debugging purposes: control z axis with some PD gain, check how the angle setpoint works
// create a function that takes as input angle references, like a crazyflie entity with input angles
// This function has to run at 200Hz
angleControlledCrazyflie(stateInertialEstimate, local_input, response);
// Return "true" to indicate that the control computation was performed successfully
return true;
}
......
Markdown is supported
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