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 8147411e authored by beuchatp's avatar beuchatp
Browse files

Continued with implementing the Default Controller manoeurves

parent d4f47854
......@@ -35,7 +35,7 @@
// TO REQUEST MANOEUVRES
#define DEFAULT_CONTROLLER_REQUEST_TAKE_OFF 1
#define DEFAULT_CONTROLLER_REQUEST_TAKEOFF 1
#define DEFAULT_CONTROLLER_REQUEST_LANDING 2
......@@ -45,9 +45,9 @@
#define DEFAULT_CONTROLLER_STATE_STANDBY 99
// > The sequence of states for a TAKE-OFF manoeuvre
#define DEFAULT_CONTROLLER_STATE_TAKE_OFF_SPIN_MOTORS 11
#define DEFAULT_CONTROLLER_STATE_TAKE_OFF_MOVE_UP 12
#define DEFAULT_CONTROLLER_STATE_TAKE_OFF_GOTO_SETPOINT 13
#define DEFAULT_CONTROLLER_STATE_TAKEOFF_SPIN_MOTORS 11
#define DEFAULT_CONTROLLER_STATE_TAKEOFF_MOVE_UP 12
#define DEFAULT_CONTROLLER_STATE_TAKEOFF_GOTO_SETPOINT 13
// > The sequence of states for a LANDING manoeuvre
#define DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN 21
......
......@@ -102,7 +102,30 @@ using namespace dfall_pkg;
// These constants are defined to make the code more readable and adaptable.
// NOTE: manz constants are already defined in the "Constant.h" header file
// NOTE: many constants are already defined in the "Constant.h" header file
// These constants define the method used for estimating the Inertial
// frame state.
// All methods are run at all times, this flag indicates which estimate
// is passed onto the controller.
// The following is a short description about each mode:
//
// ESTIMATOR_METHOD_FINITE_DIFFERENCE
// Takes the poisition and angles directly as measured,
// and estimates the velocities as a finite different to the
// previous measurement
//
// ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION
// Uses a 2nd order random walk estimator independently for
// each of (x,y,z,roll,pitch,yaw)
//
// ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED
// Uses the model of the quad-rotor and the previous inputs
//
#define ESTIMATOR_METHOD_FINITE_DIFFERENCE 1
#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
......@@ -116,13 +139,62 @@ using namespace dfall_pkg;
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
// VARIABLES FOR PERFORMING THE TAKE-OFF AND LANDING MANOEUVRES
// The current state of the Default Controller
int m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;
// A flag for when the state is changed, this is used
// so that a "one-off" operation can be performed
// the first time after changing that state
bool m_current_state_changed = false;
// The elapased time, incremented by counting the motion
// capture callbacks
float m_time_in_seconds = 0.0;
// PARAMETERS FROM THE YAML FILE
// Max setpoint change per second
float yaml_max_setpoint_change_per_second_horizontal = 0.1;
float yaml_max_setpoint_change_per_second_vertical = 0.1;
// Max error for yaw angle
float yaml_max_setpoint_error_yaw_degrees = 60.0;
float yaml_max_setpoint_error_yaw_radians = 60.0 * DEG2RAD;
// The thrust for take off spin motors
float yaml_takeoff_spin_motors_thrust = 8000;
// The time for the take off spin(-up) motors
float takoff_spin_motots_time = 0.8;
// Height change for the take off move-up
float yaml_takeoff_move_up_start_height = 0.1;
float yaml_takeoff_move_up_end_height = 0.4;
// The time for the take off spin motors
float yaml_takoff_move_up_time = 1.2;
// The setpoint to be tracked, the ordering is (x,y,z,yaw),
// with units [meters,meters,meters,radians]
float m_setpoint[4] = {0.0,0.0,0.4,0.0};
// The setpoint that is actually used by the controller, this
// differs from the setpoint when smoothing is turned on
float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
// Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = true;
// ------------------------------------------------------
// VARIABLES THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
// The ID of the agent that this node is monitoring
int m_agentID;
......@@ -142,15 +214,16 @@ std::string m_namespace_to_coordinator_parameter_service;
// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
// > the mass of the crazyflie, in [grams]
float yaml_cf_mass_in_grams = 25.0;
// > the coefficients of the 16-bit command to thrust conversion
//std::vector<float> yaml_motorPoly(3);
std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
// > the weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = 0.0;
// > the frequency at which the controller is running
float yaml_control_frequency = 200.0;
float m_control_deltaT = (1.0/200.0);
// > the coefficients of the 16-bit command to thrust conversion
std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
// > the min and max for saturating 16 bit thrust commands
float yaml_command_sixteenbit_min = 1000;
float yaml_command_sixteenbit_max = 65000;
......@@ -159,25 +232,66 @@ float yaml_command_sixteenbit_max = 65000;
// with units [meters,meters,meters,radians]
std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
// Boolean indiciating whether the "Debug Message" of this
// agent should be published or not
bool yaml_shouldPublishDebugMessage = false;
// Boolean indiciating whether the debugging ROS_INFO_STREAM
// should be displayed or not
bool yaml_shouldDisplayDebugInfo = false;
// The weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = 0.0;
// The location error of the Crazyflie at the "previous" time step
float m_previous_stateErrorInertial[9];
// The setpoint to be tracked, the ordering is (x,y,z,yaw),
// with units [meters,meters,meters,radians]
std::vector<float> m_setpoint{0.0,0.0,0.4,0.0};
// VARIABLES FOR THE CONTROLLER
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
std::vector<float> yaml_gainMatrixThrust_NineStateVector = { 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00};
std::vector<float> yaml_gainMatrixRollRate = { 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00};
std::vector<float> yaml_gainMatrixPitchRate = { 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00};
std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30};
// VARIABLES FOR THE ESTIMATOR
// Frequency at which the controller is running
float m_estimator_frequency = 200.0;
// > A flag for which estimator to use:
int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
// > The current state interial estimate,
// for use by the controller
float m_current_stateInertialEstimate[12];
// The LQR Controller parameters for "LQR_RATE_MODE"
std::vector<float> m_gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
std::vector<float> m_gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
std::vector<float> m_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
std::vector<float> m_gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08, 0.00, 0.00, 0.00};
// > The measurement of the Crazyflie at the "current" time step,
// to avoid confusion
float m_current_xzy_rpy_measurement[6];
// > The measurement of the Crazyflie at the "previous" time step,
// used for computing finite difference velocities
float m_previous_xzy_rpy_measurement[6];
// > The full 12 state estimate maintained by the finite
// difference state estimator
float m_stateInterialEstimate_viaFiniteDifference[12];
// > The full 12 state estimate maintained by the point mass
// kalman filter state estimator
float m_stateInterialEstimate_viaPointMassKalmanFilter[12];
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
std::vector<float> yaml_PMKF_Ahat_row1_for_positions = { 0.6723, 0.0034};
std::vector<float> yaml_PMKF_Ahat_row2_for_positions = {-12.9648, 0.9352};
std::vector<float> yaml_PMKF_Kinf_for_positions = { 0.3277,12.9648};
// > For the (roll,pitch,yaw) angles
std::vector<float> yaml_PMKF_Ahat_row1_for_angles = { 0.6954, 0.0035};
std::vector<float> yaml_PMKF_Ahat_row2_for_angles = {-11.0342, 0.9448};
std::vector<float> yaml_PMKF_Kinf_for_angles = { 0.3046,11.0342};
// VARIABLES RELATING TO PUBLISHING
// ROS Publisher for debugging variables
ros::Publisher m_debugPublisher;
......@@ -190,6 +304,10 @@ ros::Publisher m_setpointChangedPublisher;
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
// F U U NN N C T I O O NN N
......@@ -217,6 +335,20 @@ bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Re
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
// > This function constructs the error in the body frame
// before calling the appropriate control function
void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateInertial[12], Controller::Response &response)
// > The various functions that implement an LQR controller
void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller::Response &response);
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
// PUBLISHING OF THE DEBUG MESSAGE
void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR
// INTO AN (x,y) BODY FRAME ERROR
void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
......
......@@ -254,13 +254,13 @@ float m_stateInterialEstimate_viaPointMassKalmanFilter[12];
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
std::vector<float> yaml_PMKF_Ahat_row1_for_positions (2,0.0);
std::vector<float> yaml_PMKF_Ahat_row2_for_positions (2,0.0);
std::vector<float> yaml_PMKF_Kinf_for_positions (2,0.0);
std::vector<float> yaml_PMKF_Ahat_row1_for_positions = { 0.6723, 0.0034};
std::vector<float> yaml_PMKF_Ahat_row2_for_positions = {-12.9648, 0.9352};
std::vector<float> yaml_PMKF_Kinf_for_positions = { 0.3277,12.9648};
// > For the (roll,pitch,yaw) angles
std::vector<float> yaml_PMKF_Ahat_row1_for_angles (2,0.0);
std::vector<float> yaml_PMKF_Ahat_row2_for_angles (2,0.0);
std::vector<float> yaml_PMKF_Kinf_for_angles (2,0.0);
std::vector<float> yaml_PMKF_Ahat_row1_for_angles = { 0.6954, 0.0035};
std::vector<float> yaml_PMKF_Ahat_row2_for_angles = {-11.0342, 0.9448};
std::vector<float> yaml_PMKF_Kinf_for_angles = { 0.3046,11.0342};
......@@ -285,7 +285,7 @@ ros::Publisher m_debugPublisher;
// VARIABLES RELATING TO COMMUNICATING THE SETPOINT
// ROS Publisher for inform the network about
// changes to the setpoin
// changes to the setpoint
ros::Publisher m_setpointChangedPublisher;
......
# ------------------------------------------------------
# PARAMTERS FOR THE TAKE-OFF AND LANDING MANOEUVRES
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 0.20 # [meters]
max_setpoint_change_per_second_vertical : 0.10 # [meters]
# Max error for yaw angle
max_setpoint_error_yaw_degrees: 60
# The thrust for take off spin motors
takeoff_spin_motors_thrust: 8000
# The time for the take off spin motors
takoff_spin_motots_time: 0.8
# Height change for the take off move-up
takeoff_move_up_start_height: 0.1
takeoff_move_up_end_height: 0.4
# The time for the take off spin motors
takoff_move_up_time: 1.2
# ------------------------------------------------------
# PARAMTERS THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
# The mass of the crazyflie, in [grams]
mass : 28
mass : 30
# Frequency of the controller, in [hertz]
control_frequency : 200
......@@ -9,8 +34,48 @@ motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
# The min and max for saturating 16 bit thrust commands
command_sixteenbit_min : 1000
command_sixteenbit_max : 65000
command_sixteenbit_max : 60000
# The default setpoint, the ordering is (x,y,z,yaw),
# with unit [meters,meters,meters,radians]
default_setpoint : [0.0, 0.0, 0.4, 0.0]
\ No newline at end of file
default_setpoint : [0.0, 0.0, 0.4, 0.0]
# Boolean indiciating whether the "Debug Message" of this agent should be published or not
shouldPublishDebugMessage : false
# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
shouldDisplayDebugInfo : false
# The LQR Controller parameters for "mode = 3"
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
gainMatrixRollRate : [ 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00]
gainMatrixPitchRate : [ 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
# A flag for which estimator to use, defined as:
# 1 - Finite Different Method,
# Takes the poisition and angles directly as measured,
# and estimates the velocities as a finite different to the
# previous measurement
# 2 - Point Mass Per Dimension Method
# Uses a 2nd order random walk estimator independently for
# each of (x,y,z,roll,pitch,yaw)
# 3 - Quad-rotor Model Based Method
# Uses the model of the quad-rotor and the previous inputs
estimator_method : 1
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > For the (x,y,z) position
PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
PMKF_Kinf_for_positions : [ 0.3277,12.9648]
# > For the (roll,pitch,yaw) angles
PMKF_Ahat_row1_for_angles : [ 0.6954, 0.0035]
PMKF_Ahat_row2_for_angles : [-11.0342, 0.9448]
PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Ahat_row1_for_angles : [ 0.6723, 0.0034]
#PMKF_Ahat_row2_for_angles : [-12.9648, 0.9352]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
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