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

added temporal PID to remove offset, triggered by yaw setpoint. Added logging...

added temporal PID to remove offset, triggered by yaw setpoint. Added logging message for inputs also
parent b508726d
......@@ -148,6 +148,7 @@ add_message_files(
DebugMsg.msg
CustomButton.msg
LogMsg.msg
LogMsgLong.msg
)
## Generate services in the 'srv' folder
......
......@@ -58,8 +58,11 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/LogMsgLong.h"
#include "d_fall_pps/CustomButton.h"
#include <rosbag/bag.h>
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
......
......@@ -9,4 +9,4 @@ float64 pitch
float64 yaw
float64 x_sp
float64 y_sp
float64 z_sp
\ No newline at end of file
float64 z_sp
float64 x
float64 y
float64 z
float64 x_dot
float64 y_dot
float64 z_dot
float64 roll
float64 pitch
float64 yaw
float64 x_sp
float64 y_sp
float64 z_sp
float64 roll_sp
float64 pitch_sp
float64 yaw_sp
float64 thrust
\ No newline at end of file
......@@ -162,6 +162,12 @@
//
//
// This function WILL NEED TO BE edited for successful completion of the PPS exercise
rosbag::Bag bag;
LogMsgLong log_temp;
ros::Time t_temp;
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
......@@ -177,7 +183,22 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// to be used for subsequent controller copmutations
performEstimatorUpdate_forStateInterial(request);
t_temp = ros::Time::now();
log_temp.x = current_stateInertialEstimate[0];
log_temp.y = current_stateInertialEstimate[1];
log_temp.z = current_stateInertialEstimate[2];
log_temp.x_dot = current_stateInertialEstimate[3];
log_temp.y_dot = current_stateInertialEstimate[4];
log_temp.z_dot = current_stateInertialEstimate[5];
log_temp.roll = current_stateInertialEstimate[6];
log_temp.pitch = current_stateInertialEstimate[7];
log_temp.yaw = current_stateInertialEstimate[8];
log_temp.x_sp = setpoint[0];
log_temp.y_sp = setpoint[1];
log_temp.z_sp = setpoint[2];
// CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
// THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
// > Define a local array to fill in with the body frame error
......@@ -881,6 +902,14 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
if(lqr_angleRateNested_prev_pitchAngle < -clipping_box_angle)
lqr_angleRateNested_prev_pitchAngle = -clipping_box_angle;
log_temp.roll_sp = lqr_angleRateNested_prev_rollAngle;
log_temp.pitch_sp = lqr_angleRateNested_prev_pitchAngle;
log_temp.yaw_sp = lqr_angleRateNested_prev_yawAngle;
log_temp.thrust = lqr_angleRateNested_prev_thrustAdjustment + gravity_force;
bag.write("log_topic", t_temp, log_temp);
//ROS_INFO("Inner called");
// PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
......@@ -1925,12 +1954,23 @@ int main(int argc, char* argv[]) {
// and the message received is passed as an input argument to the callback function.
ros::Subscriber customCommandReceivedSubscriber = PPSClient_nodeHandle.subscribe("StudentCustomButton", 1, customCommandReceivedCallback);
// Open a ROS "bag" to store data for post-analysis
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
ROS_INFO("Here is the place where we will save the file: ====================================");
ROS_INFO_STREAM(package_path);
std::string record_file = package_path + "Logging_data_DEMO.bag";
bag.open(record_file, rosbag::bagmode::Write);
// Print out some information to the user.
ROS_INFO("[DEMO CONTROLLER] Service ready :-)");
// Enter an endless while loop to keep the node alive.
ros::spin();
bag.close();
ROS_INFO("bag closed");
// Return zero if the "ross::spin" is cancelled.
return 0;
}
......@@ -60,7 +60,7 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/LogMsg.h"
#include "d_fall_pps/LogMsgLong.h"
#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
......@@ -202,7 +202,7 @@ VectorXtype x0;
std::mutex MPC_mutex;
int N = 2;
int N = 10;
// ROS Publisher for debugging variables
......@@ -521,8 +521,8 @@ void do_MPC(int* publish_rate)
MPC_mutex.unlock();
// Do the MPC step:
U_0 = mympc_varying_another_ADP(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
// U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
// U_0 = mympc_varying_another_ADP(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
VectorUtype u = U_0.tail(4); // this is the input to apply next
// prediction of next N states:
......@@ -591,7 +591,7 @@ void do_MPC(int* publish_rate)
rosbag::Bag bag;
LogMsg log_temp;
LogMsgLong log_temp;
ros::Time t_temp;
const double z_offset = 0.09;
......@@ -599,9 +599,9 @@ double sum_integrator_x = 0.0;
double sum_integrator_y = 0.0;
double sum_integrator_z = 0.0;
double Ki_x = 0.5;
double Ki_y = 0.5;
double Ki_z = 0.1;
double Ki_x = 0.1;
double Ki_y = 0.1;
double Ki_z = 0.05;
bool integrator_enabled = false;
......@@ -633,7 +633,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
log_temp.y_sp = setpoint[1];
log_temp.z_sp = setpoint[2];
bag.write("log_topic", t_temp, log_temp);
// ROS_INFO("wrote in bag");
......@@ -671,6 +671,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
local_input[3] = raw_input[3]+offset_input_thrust;
MPC_mutex.unlock();
log_temp.roll_sp = local_input[0];
log_temp.pitch_sp = local_input[1];
log_temp.yaw_sp = local_input[2];
log_temp.thrust = local_input[3];
bag.write("log_topic", t_temp, log_temp);
// 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);
......
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