Commit 12c2fc73 authored by maruggv's avatar maruggv
Browse files

Major changes how controllers are called, setpoint handling

parent 142c6f9c
...@@ -197,6 +197,7 @@ float m_time_seconds; ...@@ -197,6 +197,7 @@ float m_time_seconds;
float m_mass_CF_grams; float m_mass_CF_grams;
Setpoint dronexSetpoint; Setpoint dronexSetpoint;
Setpoint dronexVelocity;
int flying_state = DRONEX_STATE_GROUND; int flying_state = DRONEX_STATE_GROUND;
int integratorFlag = DRONEX_INTEGRATOR_OFF; int integratorFlag = DRONEX_INTEGRATOR_OFF;
...@@ -306,12 +307,17 @@ float gravity = 9.81; ...@@ -306,12 +307,17 @@ float gravity = 9.81;
float m_setpoint[4] = {0.0,0.0,0.0,0.0}; float m_setpoint[4] = {0.0,0.0,0.0,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.0,0.0}; float m_setpoint_for_controller[4] = {0.0,0.0,0.0,0.0};
// > The velocities for (x,y,z) position, in that order
float m_velocity[3] = {0.0,0.0,0.0,0.0};
float m_velocity_for_controller[3] = {0.0,0.0,0.0,0.0};
// > Small adjustments to the x-y setpoint // > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f; float m_xAdjustment = 0.0f;
float m_yAdjustment = 0.0f; float m_yAdjustment = 0.0f;
// Boolean for whether to limit rate of change of the setpoint // Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = true; bool m_shouldSmoothSetpointChanges = true;
bool m_shouldClipVelocity = true;
// Max setpoint change per second // Max setpoint change per second
float m_max_setpoint_change_per_second_horizontal; float m_max_setpoint_change_per_second_horizontal;
...@@ -541,17 +547,10 @@ float calculate_distance_in_xyz(std::vector<float> p1, std::vector<float> p2); ...@@ -541,17 +547,10 @@ float calculate_distance_in_xyz(std::vector<float> p1, std::vector<float> p2);
// > The function that is called to "start" all estimation and control computations // > The function that is called to "start" all estimation and control computations
bool calculateControlOutput(Controller::Request &request, Controller::Response &response); bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response); void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response);
// > The various functions that implement an LQR controller
void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforActuators( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
// > The various functions that implement an LQR controller
void calculateControlOutput_viaLQRforRates_forLanding( float stateErrorBody[12], Controller::Request &request, Controller::Response &response); void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforRates_Nested(float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
// Velocity estimator // Velocity estimator
void calculateMSVelocity(Controller::Request &request); void calculateMSVelocity(Controller::Request &request);
......
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