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 f2f00c7f authored by Tobias's avatar Tobias
Browse files

minor changes, distributed feedforward thrust based on z-error integration after take-off

parent d9ebb43f
......@@ -208,12 +208,9 @@ Eigen::Matrix<float,24,57> outputMatrix;
Eigen::Matrix<float,24,33> outputMatrix_centralized;
// augmented equilibirium state vector of the copter-load system
Eigen::Matrix<float,33,1> state_eq;
Eigen::Matrix<float,33,1> estimator_state_prev;
// augmented equilibirium state vector of the copter-load system
Eigen::Matrix<float,57,1> augmented_state_eq;
// initialize estimated state
Eigen::Matrix<float,57,1> overlapping_estimator_state_prev;
// gains
......
......@@ -243,17 +243,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Distributed Controller runs at 100Hz --> downsampling by factor 2
if (outer_loop_counter % 2 == 0)
{
// set equilibrium load position to current load set point
augmented_state_eq(7,0) = load_setpoint[0];
augmented_state_eq(8,0) = load_setpoint[1];
augmented_state_eq(9,0) = load_setpoint[2];
augmented_state_eq(26,0) = load_setpoint[0];
augmented_state_eq(27,0) = load_setpoint[1];
augmented_state_eq(28,0) = load_setpoint[2];
augmented_state_eq(45,0) = load_setpoint[0];
augmented_state_eq(46,0) = load_setpoint[1];
augmented_state_eq(47,0) = load_setpoint[2];
// update measurement vector using vicon Data
get_y_from_CFData( request );
......@@ -296,17 +285,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Distributed Controller runs at 100Hz --> downsampling by factor 2
if (outer_loop_counter % 2 == 0)
{
// set equilibrium load position to current load set point
augmented_state_eq(7,0) = load_setpoint[0];
augmented_state_eq(8,0) = load_setpoint[1];
augmented_state_eq(9,0) = load_setpoint[2];
augmented_state_eq(26,0) = load_setpoint[0];
augmented_state_eq(27,0) = load_setpoint[1];
augmented_state_eq(28,0) = load_setpoint[2];
augmented_state_eq(45,0) = load_setpoint[0];
augmented_state_eq(46,0) = load_setpoint[1];
augmented_state_eq(47,0) = load_setpoint[2];
// update measurement vector using vicon Data
get_y_from_CFData( request );
......@@ -415,11 +393,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Distributed Controller runs at 100Hz --> downsampling by factor 2
if (outer_loop_counter % 2 == 0)
{
// set equilibrium load position to current load set point
state_eq(21,0) = load_setpoint[0];
state_eq(22,0) = load_setpoint[1];
state_eq(23,0) = load_setpoint[2];
// update measurement vector using vicon Data
get_y_from_CFData( request );
......@@ -1076,9 +1049,8 @@ void ControllerStateReceivedCallback(const CustomButton& commandReceived)
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 2 received in controller. ");
ROS_INFO("[STUDENT CONTROLLER] Controller State: enable distributed Controller. ");
augmented_state_eq(6,0) = PI/2; // cable angle theta1_eq = pi/2
augmented_state_eq(25,0) = PI/2; // cable angle theta2_eq = pi/2
augmented_state_eq(44,0) = PI/2; // cable angle theta3_eq = pi/2
gravity_force_copterloadsystem = gravity_force + load_mass * 9.81/(1000*4*3);
controller_state = custom_button_index;
outer_loop_counter = 0;
break;
......@@ -1089,6 +1061,7 @@ void ControllerStateReceivedCallback(const CustomButton& commandReceived)
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 3 received in controller. ");
ROS_INFO("[STUDENT CONTROLLER] Controller State: prepare Landing, lower the load. ");
gravity_force_copterloadsystem = gravity_force + load_mass * 9.81/(1000*4*3);
controller_state = custom_button_index;
outer_loop_counter = 0;
break;
......@@ -1110,6 +1083,7 @@ void ControllerStateReceivedCallback(const CustomButton& commandReceived)
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 5 received in controller. ");
ROS_INFO("[STUDENT CONTROLLER] Controller State: centralized controller for copter-load system ");
gravity_force_copterloadsystem = gravity_force + load_mass * 9.81/(1000*4*3);
controller_state = custom_button_index;
outer_loop_counter = 0;
break;
......@@ -1251,7 +1225,6 @@ void processFetchedParameters()
// Compute the feed-forward force that we need to counteract gravity (i.e., mg)
// > in units of [Newtons]
gravity_force = cf_mass * 9.81/(1000*4);
gravity_force_copterloadsystem = gravity_force + load_mass * 9.81/(1000*4*3);
takeOffDistance_increment = (takeOffDistance - takeOffinitial) / ( control_frequency * durationTakeOff );
// DEBUGGING: Print out one of the computed quantities
......@@ -1513,9 +1486,9 @@ int main(int argc, char* argv[]) {
// Initialize some state vector of the output feedback controller
augmented_state_eq.setZero();
//augmented_state_eq.setZero();
overlapping_estimator_state_prev.setZero();
state_eq.setZero();
//state_eq.setZero();
estimator_state_prev.setZero();
// set measurement vector to zero
y_to_controller.setZero();
......
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