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

first implementation of MPC function with eigen: linearization_fast_euler_6_states

parent e42fe276
......@@ -244,7 +244,7 @@ add_executable(PPSClient src/nodes/PPSClient.cpp)
add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
add_executable(DemoControllerService src/nodes/DemoControllerService.cpp)
add_executable(StudentControllerService src/nodes/StudentControllerService.cpp)
add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
add_executable(MpcControllerService src/nodes/MpcControllerService.cpp src/MPC_functions.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
......
#pragma once
#include <math.h>
#include <stdlib.h>
#include <iostream>
#include <Eigen/Dense>
#define N_x 6
#define N_u 4
typedef Eigen::Matrix<float, N_x, N_x> MatrixAtype;
typedef Eigen::Matrix<float, N_x, N_u> MatrixBtype;
typedef Eigen::Matrix<float, N_x, 1> VectorXtype;
typedef Eigen::Matrix<float, N_u, 1> VectorUtype;
void linearization_fast_euler_6_states(VectorXtype x_vec, VectorUtype u_vec, float cf_mass, MatrixAtype &A, MatrixBtype &B);
#pragma once
// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
......
#include "MPC_functions.h"
using namespace Eigen;
using namespace std;
void linearization_fast_euler_6_states(VectorXtype x_vec, VectorUtype u_vec, float cf_mass, MatrixAtype &A, MatrixBtype &B)
{
A = MatrixAtype::Zero();
A.topRightCorner(3,3) = Matrix3f::Identity();
float gamma = u_vec(0);
float beta = u_vec(1);
float alpha = u_vec(2);
float ft = u_vec(3);
float c_alpha = cos(alpha);
float s_alpha = sin(alpha);
float s_beta = sin(beta);
float c_beta = cos(beta);
float c_gamma = cos(gamma);
float s_gamma = sin(gamma);
B = MatrixBtype::Zero();
B.row(3) << (ft*(c_gamma*s_alpha - c_alpha*s_beta*s_gamma))/cf_mass, (ft*c_alpha*c_beta*c_gamma)/cf_mass, (ft*(c_alpha*s_gamma - c_gamma*s_alpha*s_beta))/cf_mass, (s_alpha*s_gamma + c_alpha*c_gamma*s_beta)/cf_mass;
B.row(4) << -(ft*(c_alpha*c_gamma + s_alpha*s_beta*s_gamma))/cf_mass, (ft*c_beta*c_gamma*s_alpha)/cf_mass, (ft*(s_alpha*s_gamma + c_alpha*c_gamma*s_beta))/cf_mass, -(c_alpha*s_gamma - c_gamma*s_alpha*s_beta)/cf_mass;
B.row(5) << -(ft*c_beta*s_gamma)/cf_mass, -(ft*c_gamma*s_beta)/cf_mass, 0, (c_beta*c_gamma)/cf_mass;
}
......@@ -63,8 +63,7 @@
#include <std_msgs/Int32.h>
#include <Eigen/Dense>
#include "MPC_functions.h"
// ----------------------------------------------------------------------------------
......@@ -434,6 +433,7 @@ void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], int
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
// 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
......@@ -448,6 +448,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
float x0_full_state[12] = {-setpoint[0], -setpoint[1], -setpoint[2], 0, 0, 0, 0, 0, 0, 0, 0, 0};
// just for debugging purposes: control z axis with some PD gain, check how the angle setpoint works
float correction_z = -0.82*(0.4 - stateInertialEstimate[2]) - 0.22 * (0 - stateInertialEstimate[5]) + cf_mass/1000*9.8;
float input_angle[4] = {-setpoint[0], 0, 0, correction_z};
......@@ -630,10 +631,33 @@ float computeMotorPolyBackward(float thrust)
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
void setpointCallback(const Setpoint& newSetpoint)
{
std::cout << "testing cout" << std::endl;
setpoint[0] = newSetpoint.x;
setpoint[1] = newSetpoint.y;
setpoint[2] = newSetpoint.z;
setpoint[3] = newSetpoint.yaw;
// testzone:
VectorXtype x_vec;
x_vec.setZero();
VectorUtype u_vec;
u_vec << 0, 0, 0, cf_mass/1000*9.81;
MatrixAtype A;
MatrixBtype B;
linearization_fast_euler_6_states(x_vec, u_vec, cf_mass/1000, A, B);
// debug
Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
std::cout << A.format(OctaveFmt) << std::endl;
std::cout << B.format(OctaveFmt) << std::endl;
}
......
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