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

Finished implementing the Default controller (except for the integrator)....

Finished implementing the Default controller (except for the integrator). Needs compiling and testing. (Also changed the ClientConfig.yaml to be FlyingAgentClientConfig.yaml)
parent 8147411e
......@@ -204,7 +204,7 @@
<string>LoadYAML</string>
</property>
<addaction name="action_LoadYAML_BatteryMonitor"/>
<addaction name="action_LoadYAML_ClientConfig"/>
<addaction name="action_LoadYAML_FlyingAgentClientConfig"/>
</widget>
<widget class="QMenu" name="menuControllers">
<property name="title">
......@@ -239,9 +239,9 @@
<string>BatteryMonitor</string>
</property>
</action>
<action name="action_LoadYAML_ClientConfig">
<action name="action_LoadYAML_FlyingAgentClientConfig">
<property name="text">
<string>ClientConfig</string>
<string>FlyingAgentClientConfig</string>
</property>
</action>
<action name="action_showHideController_default">
......
......@@ -124,7 +124,7 @@ private slots:
// PRIVATE METHODS FOR MENU ITEM CALLBACKS
void on_actionShowHide_Coordinator_triggered();
void on_action_LoadYAML_BatteryMonitor_triggered();
void on_action_LoadYAML_ClientConfig_triggered();
void on_action_LoadYAML_FlyingAgentClientConfig_triggered();
// FOR THE CONTROLLERS MENU
void on_action_showHideController_default_changed();
......
......@@ -194,7 +194,7 @@ void MainWindow::on_action_LoadYAML_BatteryMonitor_triggered()
}
void MainWindow::on_action_LoadYAML_ClientConfig_triggered()
void MainWindow::on_action_LoadYAML_FlyingAgentClientConfig_triggered()
{
#ifdef CATKIN_MAKE
// Inform the user that the menu item was selected
......@@ -203,7 +203,7 @@ void MainWindow::on_action_LoadYAML_ClientConfig_triggered()
// Create a local variable for the message
StringWithHeader yaml_filename_msg;
// Specify the data
yaml_filename_msg.data = "ClientConfig";
yaml_filename_msg.data = "FlyingAgentClientConfig";
// Set for whom this applies to
yaml_filename_msg.shouldCheckForAgentID = false;
// Send the message
......
......@@ -41,13 +41,15 @@
// TO IDENITFY THE STATE OF THE DEFAULT CONTROLLER
#define DEFAULT_CONTROLLER_STATE_UNKNOWN -1
#define DEFAULT_CONTROLLER_STATE_NORMAL 1
#define DEFAULT_CONTROLLER_STATE_STANDBY 99
#define DEFAULT_CONTROLLER_STATE_UNKNOWN -1
// > The sequence of states for a TAKE-OFF manoeuvre
#define DEFAULT_CONTROLLER_STATE_TAKEOFF_SPIN_MOTORS 11
#define DEFAULT_CONTROLLER_STATE_TAKEOFF_MOVE_UP 12
#define DEFAULT_CONTROLLER_STATE_TAKEOFF_GOTO_SETPOINT 13
#define DEFAULT_CONTROLLER_STATE_TAKEOFF_INTEGRATOR_ON 14
// > The sequence of states for a LANDING manoeuvre
#define DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN 21
......
......@@ -104,10 +104,28 @@ using namespace dfall_pkg;
// 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.
// These constants define the method used for computing
// the control actions from the state error estimates.
// The following is a short description about each mode:
//
// CONTROLLER_METHOD_RATES
// Uses the poisition, linear velocity and angle
// error estimates to compute the rates
//
// CONTROLLER_METHOD_RATE_ANGLE_NESTED
// Uses the position and linear velocity error
// estimates to compute an angle, and then uses
// this as a reference to construct an angle error
// estimate and compute from that the rates
//
#define CONTROLLER_METHOD_RATES 1
#define CONTROLLER_METHOD_RATE_ANGLE_NESTED 2 // (DEFAULT)
// 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
......@@ -160,21 +178,58 @@ float m_time_in_seconds = 0.0;
float yaml_max_setpoint_change_per_second_horizontal = 0.1;
float yaml_max_setpoint_change_per_second_vertical = 0.1;
// Max error for z
float yaml_max_setpoint_error_z = 0.4;
// Max error for yaw angle
float yaml_max_setpoint_error_yaw_degrees = 60.0;
float yaml_max_setpoint_error_yaw_radians = 60.0 * DEG2RAD;
// Max {roll,pitch} angle request
float yaml_max_roll_pitch_request_degrees = 30.0;
float yaml_max_roll_pitch_request_radians = 30.0 * DEG2RAD;
// Theshold for {roll,pitch} angle beyond
// which the motors are turned off
float yaml_threshold_roll_pitch_for_turn_off_degrees = 70.0;
float yaml_threshold_roll_pitch_for_turn_off_radians = 70.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;
// The time for: take off spin(-up) motors
float yaml_takoff_spin_motors_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
// The time for: take off spin motors
float yaml_takoff_move_up_time = 1.2;
// Minimum and maximum allowed time for: take off goto setpoint
float yaml_takoff_goto_setpoint_min_time = 1.2;
float yaml_takoff_goto_setpoint_max_time = 2.0;
// Box within which to keep the integrator on
// > Units of [meters]
// > The box consider is plus/minus this value
float yaml_takoff_integrator_on_box_horizontal = 0.25;
float yaml_takoff_integrator_on_box_vertical = 0.15;
// The time for: take off integrator-on
float yaml_takoff_integrator_on_time = 1.5;
// Height change for the landing move-down
float yaml_landing_move_down_end_height_setpoint = 0.05;
float yaml_landing_move_down_end_height_threshold = 0.10;
// The time for: landing move-down
float yaml_landing_move_down_time_max = 2.0;
// The thrust for landing spin motors
float yaml_landing_spin_motors_thrust = 10000;
// The time for: landing spin motors
float yaml_landing_spin_motors_time = 1.0;
// The setpoint to be tracked, the ordering is (x,y,z,yaw),
......@@ -244,12 +299,21 @@ bool yaml_shouldDisplayDebugInfo = false;
// 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};
// > A flag for which controller to use:
int yaml_controller_method = CONTROLLER_METHOD_RATE_ANGLE_NESTED;
// The LQR Controller parameters for z-height
std::vector<float> yaml_gainMatrixThrust_2StateVector = { 0.98, 0.25};
// The LQR Controller parameters for "CONTROLLER_METHOD_RATES"
std::vector<float> yaml_gainMatrixRollRate_3StateVector = {-6.20,-3.00, 5.20};
std::vector<float> yaml_gainMatrixPitchRate_3StateVector = { 6.20, 3.00, 5.20};
// The LQR Controller parameters for "CONTROLLER_METHOD_RATE_ANGLE_NESTED"
std::vector<float> yaml_gainMatrixRollAngle_2StateVector = {-0.20,-0.20};
std::vector<float> yaml_gainMatrixPitchAngle_2StateVector = { 0.20, 0.20};
float yaml_gainRollRate_fromAngle = 4.00;
float yaml_gainPitchRate_fromAngle = 4.00;
// The LQR Controller parameters for yaw
float yaml_gainYawRate_fromAngle = 2.30;
// VARIABLES FOR THE ESTIMATOR
......@@ -259,9 +323,10 @@ 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];
float m_current_stateInertialEstimate[9];
// > The measurement of the Crazyflie at the "current" time step,
// to avoid confusion
......@@ -273,22 +338,17 @@ float m_previous_xzy_rpy_measurement[6];
// > The full 12 state estimate maintained by the finite
// difference state estimator
float m_stateInterialEstimate_viaFiniteDifference[12];
float m_stateInterialEstimate_viaFiniteDifference[9];
// > The full 12 state estimate maintained by the point mass
// kalman filter state estimator
float m_stateInterialEstimate_viaPointMassKalmanFilter[12];
float m_stateInterialEstimate_viaPointMassKalmanFilter[9];
// 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
......@@ -301,6 +361,11 @@ ros::Publisher m_debugPublisher;
ros::Publisher m_setpointChangedPublisher;
// ROS Publisher for sending motors-off command
// to the flying agent client
ros::Publisher m_motorsOffToFlyingAgentClientPublisher;
......@@ -335,11 +400,28 @@ bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Re
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
// > For the normal state
void computeResponse_for_normal(Controller::Response &response);
// > For the standby state (also used for unknown state)
void computeResponse_for_standby(Controller::Response &response);
// > For the take-off phases
void computeResponse_for_takeoff_move_up(Controller::Response &response);
void computeResponse_for_takeoff_spin_motors(Controller::Response &response);
void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response);
void computeResponse_for_takeoff_integrator_on(Controller::Response &response);
// > For the landing phases
void computeResponse_for_landing_move_down(Controller::Response &response);
void computeResponse_for_landing_spin_motors(Controller::Response &response);
// SMOOTHING SETPOINT CHANGES
void smoothSetpointChanges( float target_setpoint[4] , float (&current_setpoint)[4] );
// > 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);
void calculateControlOutput_viaLQR_givenError(float stateErrorBody[12], Controller::Response &response);
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
......@@ -371,6 +453,9 @@ void publishCurrentSetpointAndState();
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
// PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
void publish_motors_off_to_flying_agent_client();
// LOADING OF YAML PARAMETERS
void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);
void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
......@@ -150,7 +150,7 @@ float yaml_mocap_timeout_duration = 1.0;
bool yaml_isEnabled_strictSafety = true;
// > The maximum allowed tilt angle, in degrees and radians
float yaml_maxTiltAngle_for_strictSafety_degrees = 70;
float m_maxTiltAngle_for_strictSafety_radians = 70 * DEG2RAD;
float yaml_maxTiltAngle_for_strictSafety_radians = 70 * DEG2RAD;
......@@ -192,15 +192,15 @@ int m_controller_nominally_selected;
// VARIABLES FOR THE CONTROLER SERVIVCE CLIENTS
// The default controller specified in the ClientConfig.yaml
// The default controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_defaultController;
// The Student controller specified in the ClientConfig.yaml
// The Student controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_studentController;
// The Tuning controller specified in the ClientConfig.yaml
// The Tuning controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_tuningController;
// The Picker controller specified in the ClientConfig.yaml
// The Picker controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_pickerController;
// The Template controller specified in the ClientConfig.yaml
// The Template controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_templateController;
......@@ -216,7 +216,7 @@ int m_crazyradio_status;
CrazyflieContext m_context;
// Service Client from which the "CrazyflieContext" is loaded
ros::ServiceClient centralManager;
ros::ServiceClient m_centralManager;
// Publisher for the control actions to be sent on
// to the Crazyflie (the CrazyRadio node listen to this
......@@ -232,10 +232,10 @@ ros::Publisher m_flyingStatePublisher;
//ros::Publisher commandPublisher;
// Publisher Communication with crazyRadio node. Connect and disconnect
ros::Publisher crazyRadioCommandPublisher;
ros::Publisher m_crazyRadioCommandPublisher;
// Publisher for which controller is currently being used
ros::Publisher controllerUsedPublisher;
ros::Publisher m_controllerUsedPublisher;
......@@ -358,5 +358,5 @@ void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
// FOR LOADING THE YAML PARAMETERS
void isReadyClientConfigYamlCallback(const IntWithHeader & msg);
void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
void isReadyFlyingAgentClientConfigYamlCallback(const IntWithHeader & msg);
void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
......@@ -130,8 +130,8 @@
<param name="agentID" value="$(arg agentID)" />
<rosparam
command = "load"
file = "$(find dfall_pkg)/param/ClientConfig.yaml"
ns = "ClientConfig"
file = "$(find dfall_pkg)/param/FlyingAgentClientConfig.yaml"
ns = "FlyingAgentClientConfig"
/>
<rosparam
command = "load"
......
......@@ -44,7 +44,7 @@
/>
<rosparam
command = "load"
file = "$(find dfall_pkg)/param/ClientConfig.yaml"
file = "$(find dfall_pkg)/param/FlyingAgentClientConfig.yaml"
ns = "SafeController"
/>
</node>
......
......@@ -2,23 +2,62 @@
# 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_setpoint_change_per_second_horizontal : 0.50 # [meters]
max_setpoint_change_per_second_vertical : 0.30 # [meters]
# Max error for z
max_setpoint_error_z: 0.4
# Max error for yaw angle
max_setpoint_error_yaw_degrees: 60
# Max {roll,pitch} angle request
max_roll_pitch_request_degrees: 30
# Theshold for {roll,pitch} angle beyond
# which the motors are turned off
threshold_roll_pitch_for_turn_off_degrees: 70
# 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
# The time for: take off spin motors
takoff_spin_motors_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
# The time for: take off move-up
takoff_move_up_time: 1.2
# Minimum and maximum allowed time for: take off goto setpoint
takoff_goto_setpoint_min_time: 1.0
takoff_goto_setpoint_max_time: 2.0
# Box within which to keep the integrator on
# > Units of [meters]
# > The box consider is plus/minus this value
takoff_integrator_on_box_horizontal: 0.25
takoff_integrator_on_box_vertical: 0.15
# The time for: take off integrator-on
takoff_integrator_on_time: 1.5
# Height change for the landing move-down
landing_move_down_end_height_setpoint: 0.05
landing_move_down_end_height_threshold: 0.10
# The time for: landing move-down
landing_move_down_time_max: 2.0
# The thrust for landing spin motors
landing_spin_motors_thrust: 10000
# The time for: landing spin motors
landing_spin_motors_time: 1.0
# IMPORTANT NOTE: the times above should NOT be set
# to zero because this will cause a divide by zero
# crash.
# ------------------------------------------------------
# PARAMTERS THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
......@@ -46,11 +85,25 @@ 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 controller to use, defined as:
# 1 - Rate controller
# 2 - Angle-Rate nested controller
controller_method : 1
# The LQR Controller parameters for z-height
gainMatrixThrust_2StateVector : [ 0.98, 0.25]
# The LQR Controller parameters for mode 1 (the Rate controller)
gainMatrixRollRate_3StateVector : [-6.20,-3.00, 5.20]
gainMatrixPitchRate_3StateVector : [ 6.20, 3.00, 5.20]
# The LQR Controller parameters for mode 2 (Angle-nested)
gainMatrixRollAngle_2StateVector : [-0.20,-0.20]
gainMatrixPitchAngle_2StateVector : [ 0.20, 0.20]
gainRollRate_fromAngle : 4.00
gainPitchRate_fromAngle : 4.00
# The LQR Controller parameters for yaw
gainYawRate_fromAngle : 2.30
# A flag for which estimator to use, defined as:
# 1 - Finite Different Method,
......@@ -60,22 +113,10 @@ gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0
# 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]
PMKF_Kinf_for_positions : [ 0.3277,12.9648]
\ No newline at end of file
......@@ -28,9 +28,4 @@ shouldPerfom_takeOff_with_defaultController: true
# Flag for whether the landing should be performed
# with the default controller
shouldPerfom_landing_with_defaultController: true
#battery_threshold_while_flying: 2.60 # in V
#battery_threshold_while_motors_off: 3.30 # in V
#battery_polling_period: 200 # in ms
\ No newline at end of file
shouldPerfom_landing_with_defaultController: true
\ No newline at end of file
dictionary : {
'ClientConfig' : 'ClientConfig' ,
'FlyingAgentClientConfig' : 'FlyingAgentClientConfig' ,
'SafeController' : 'SafeController'
}
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