Commit dda9d29e authored by mastefan's avatar mastefan
Browse files

Bugfixes and velocity of mothership estimation

parent 63ab8d92
......@@ -250,6 +250,11 @@ float m_max_setpoint_change_per_second_yaw_radians;
float m_vicon_frequency;
float prev_DroneX_pos[3] = {0,0,0};
float current_DroneX_pos[3];
float drone_X_vel[3];
// THE FOLLOWING PARAMETERS ARE USED
// FOR THE LOW-LEVEL CONTROLLER
......@@ -444,6 +449,10 @@ void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12]
void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
// Velocity estimator
void calculateDroneXVelocity(Controller::Request &request);
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
......
......@@ -191,8 +191,8 @@ std::vector<float> m_dropoff_coordinates_xy_for_H(2);
float m_picker_string_length;
// > The setpoints for (x,y,z) position and yaw angle, in that order
float m_setpoint[4] = {0.0,0.0,0.4,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
float m_setpoint[4] = {0.0,0.0,0,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.0,0.0};
// > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f;
......@@ -215,6 +215,7 @@ float m_vicon_frequency;
// THE FOLLOWING PARAMETERS ARE USED
// FOR THE LOW-LEVEL CONTROLLER
......
......@@ -537,6 +537,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
calculateDroneXVelocity(request);
setpointCallback(dronexSetpoint);
/*dronexSetpoint.x = request.otherCrazyflies[0].x;
......@@ -623,6 +626,28 @@ void motorsOFF(Controller::Response &response){
// Estimate mothership velocity
void calculateDroneXVelocity(Controller::Request &request){
prev_DroneX_pos[0] = current_DroneX_pos[0];
prev_DroneX_pos[1] = current_DroneX_pos[1];
prev_DroneX_pos[2] = current_DroneX_pos[2];
current_DroneX_pos[0] = request.otherCrazyflies[0].x;
current_DroneX_pos[1] = request.otherCrazyflies[0].y;
current_DroneX_pos[2] = request.otherCrazyflies[0].z;
drone_X_vel[0] = (current_DroneX_pos[0] - prev_DroneX_pos[0])*m_vicon_frequency;
drone_X_vel[1] = (current_DroneX_pos[1] - prev_DroneX_pos[1])*m_vicon_frequency;
drone_X_vel[2] = (current_DroneX_pos[2] - prev_DroneX_pos[2])*m_vicon_frequency;
ROS_INFO_STREAM("Velocity: vx " << drone_X_vel[0] << ", vy " << drone_X_vel[1] << ", vz " << drone_X_vel[2]);
}
// ------------------------------------------------------------------------------
// EEEEE SSSS TTTTT III M M A TTTTT III OOO N N
// E S T I MM MM A A T I O O NN N
......
......@@ -185,10 +185,6 @@ void perControlCycleOperations()
// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
void buttonPressed_gotoStart()
......
Supports Markdown
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