Skip to content

Commit

Permalink
added bugfixes and removed old parameters
Browse files Browse the repository at this point in the history
  • Loading branch information
@JMoore5353
JMoore5353 committed Mar 29, 2024
1 parent 96e7b49 commit 1f4b10e
Show file tree
Hide file tree
Showing 4 changed files with 49 additions and 142 deletions.
104 changes: 8 additions & 96 deletions rosplane/include/controller_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,56 +83,6 @@ class controller_base : public rclcpp::Node
alt_zones current_zone; /**< The current altitude zone for the control */
};

/**
* This struct contains all of the parameters used by the controller.
*/
struct params_s
{
double alt_hz; /**< altitude hold zone */
double alt_toz; /**< altitude takeoff zone */
double tau; /**< servo response time constant */
double c_kp; /**< course hold proportional gain */
double c_kd; /**< course hold derivative gain */
double c_ki; /**< course hold integral gain */
double r_kp; /**< roll hold proportional gain */
double r_kd; /**< roll hold derivative gain */
double r_ki; /**< roll hold integral gain */
double p_kp; /**< pitch hold proportional gain */
double p_kd; /**< pitch hold derivative gain */
double p_ki; /**< pitch hold integral gain */
double p_ff; /**< pitch feedforward */
double a_t_kp; /**< airspeed with throttle hold proportional gain */
double a_t_kd; /**< airspeed with throttle hold derivative gain */
double a_t_ki; /**< airspeed with throttle hold integral gain */
double a_kp; /**< altitude hold proportional gain */
double a_kd; /**< altitude hold derivative gain */
double a_ki; /**< altitude hold integral gain */
double l_kp; /**< energy balance proportional gain */
double l_kd; /**< energy balance derivative gain */
double l_ki; /**< energy balance integral gain */
double e_kp; /**< total energy proportional gain */
double e_kd; /**< total energy derivative gain */
double e_ki; /**< total energy integral gain */
double trim_e; /**< trim value for elevator */
double trim_a; /**< trim value for aileron */
double trim_r; /**< trim value for rudder */
double trim_t; /**< trim value for throttle */
double max_e; /**< maximum value for elevator */
double max_a; /**< maximum value for aileron */
double max_r; /**< maximum value for rudder */
double max_t; /**< maximum value for throttle */
double pwm_rad_e; /**< conversion to pwm from radians for output of control loops */
double pwm_rad_a; /**< conversion to pwm from radians for output of control loops */
double pwm_rad_r; /**< conversion to pwm from radians for output of control loops */
double max_takeoff_throttle; /**< maximum throttle allowed at takeoff */
double mass; /**< mass of the aircraft */
double gravity; /**< gravity in m/s^2 */
bool pitch_tuning_debug_override;
bool roll_tuning_debug_override;
double max_roll;
int frequency;
};

/**
* Returns a std::variant that holds the value of the given parameter
*/
Expand All @@ -143,7 +93,7 @@ class controller_base : public rclcpp::Node

void declare_param(std::string param_name, double value);
void declare_param(std::string param_name, bool value);
void declare_int(std::string param_name, int64_t value);
void declare_int(std::string param_name, int64_t value);
void declare_param(std::string param_name, std::string value);

/**
Expand Down Expand Up @@ -200,51 +150,13 @@ class controller_base : public rclcpp::Node
rclcpp::TimerBase::SharedPtr timer_;

/** Parameters for use in control loops.*/
std::map<std::string, std::variant<double, bool, int64_t, std::string>> params1_;
// /** Parameters for use in control loops.*/
struct params_s params_ = {/* alt_hz */ 10.0,
/* alt_toz */ 5.0,
/* tau */ 50.0,
/* c_kp */ 2.37,
/* c_kd */ 0.0,
/* c_ki */ 0.4,
/* r_kp */ .06,
/* r_kd */ 0.04,
/* r_ki */ 0.0,
/* p_kp */ -.15,
/* p_kd */ -.05,
/* p_ki */ 0.0,
/* p_ff */ 0.0,
/* a_t_kp */ .05,
/* a_t_kd */ 0.0,
/* a_t_ki */ .005,
/* a_kp */ 0.015,
/* a_kd */ 0.0,
/* a_ki */ 0.003,
/* l_kp */ 1.0,
/* l_kd */ 0.0,
/* l_ki */ 0.05,
/* e_kp */ 5.0,
/* e_kd */ 0.0,
/* e_ki */ .9,
/* trim_e */ 0.02,
/* trim_a */ 0.0,
/* trim_r */ 0.0,
/* trim_t */ 0.5,
/* max_e */ 0.61,
/* max_a */ 0.15,
/* max_r */ 0.523,
/* max_t */ 1.0,
/* pwm_rad_e */ 1.0,
/* pwm_rad_a */ 1.0,
/* pwm_rad_r */ 1.0,
/* max_takeoff_throttle */ .55,
/* mass */ 2.28,
/* gravity */ 9.8,
/* pitch_tuning_debug_override*/ false,
/* roll_tuning_debug_override*/ false,
/* max_roll*/ 25.0,
/* frequency of contorl loop */ 100};
/** Note that these parameters are not used:
*
* double p_ff; //< pitch feedforward
* double trim_r; //< trim value for rudder
* double max_r; //< maximum value for rudder
*/
std::map<std::string, std::variant<double, bool, int64_t, std::string>> params_; // Can I cast ROS int to int?

/**
* The stored value for the most up to date commands for the controller.
Expand Down
58 changes: 22 additions & 36 deletions rosplane/src/controller_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,44 +165,56 @@ void controller_base::actuator_controls_publish()

void controller_base::declare_param(std::string param_name, double value)
{

// Insert the parameter into the parameter struct
params1_[param_name] = value;

// Declare each of the parameters, making it visible to the ROS2 param system.
this->declare_parameter(param_name, value);
}

void controller_base::declare_param(std::string param_name, bool value)
{

// Insert the parameter into the parameter struct
params1_[param_name] = value;

// Declare each of the parameters, making it visible to the ROS2 param system.
this->declare_parameter(param_name, value);
}

void controller_base::declare_int(std::string param_name, int64_t value)
{

// Insert the parameter into the parameter struct
params1_[param_name] = value;

// Declare each of the parameters, making it visible to the ROS2 param system.
this->declare_parameter(param_name, value);
}

void controller_base::declare_param(std::string param_name, std::string value)
{

// Insert the parameter into the parameter struct
params1_[param_name] = value;

// Declare each of the parameters, making it visible to the ROS2 param system.
this->declare_parameter(param_name, value);
}

double controller_base::get_double(std::string param_name)
{
return std::get<double>(params1_[param_name]);
}

bool controller_base::get_bool(std::string param_name)
{
return std::get<bool>(params1_[param_name]);
}

int64_t controller_base::get_int(std::string param_name)
{
return std::get<int64_t>(params1_[param_name]);
}

std::string controller_base::get_string(std::string param_name)
{
return std::get<std::string>(params1_[param_name]);
}

void controller_base::set_parameters()
{

Expand All @@ -222,7 +234,6 @@ void controller_base::set_parameters()
else
RCLCPP_ERROR_STREAM(this->get_logger(), "Unable to set parameter: " + key + ". Error casting parameter as double, int, string, or bool!");
}

}

rcl_interfaces::msg::SetParametersResult
Expand All @@ -247,6 +258,8 @@ controller_base::parametersCallback(const std::vector<rclcpp::Parameter> & param
params1_[param.get_name()] = param.as_double();
else if (param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL)
params1_[param.get_name()] = param.as_bool();
else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
params1_[param.get_name()] = param.as_int();
else if (param.get_type() == rclcpp::ParameterType::PARAMETER_STRING)
params1_[param.get_name()] = param.as_string();
else
Expand All @@ -257,33 +270,6 @@ controller_base::parametersCallback(const std::vector<rclcpp::Parameter> & param
return result;
}

double controller_base::get_double(std::string param_name)
{
// try
// {
return std::get<double>(params1_[param_name]);
// }
// catch (const std::bad_variant_access& e)
// {
// RCLCPP_ERROR_STREAM(this->get_logger(), "Error when accessing " + param_name + " parameter: " + e.what());
// }
}

bool controller_base::get_bool(std::string param_name)
{
return std::get<bool>(params1_[param_name]);
}

int64_t controller_base::get_int(std::string param_name)
{
return std::get<int64_t>(params1_[param_name]);
}

std::string controller_base::get_string(std::string param_name)
{
return std::get<std::string>(params1_[param_name]);
}

void controller_base::set_timer()
{

Expand Down
17 changes: 11 additions & 6 deletions rosplane/src/controller_successive_loop.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,13 +149,14 @@ void controller_successive_loop::take_off_longitudinal_control(const struct inpu
{
// For readability, declare parameters here that will be used in this function
double max_takeoff_throttle = get_double("max_takeoff_throttle");
double cmd_takeoff_pitch = get_double("cmd_takeoff_pitch");

// Set throttle to not overshoot altitude.
output.delta_t =
sat(airspeed_with_throttle_hold(input.Va_c, input.va), max_takeoff_throttle, 0);

// Command a shallow pitch angle to gain altitude.
output.theta_c = 5.0 * 3.14 / 180.0; // TODO add to params.
output.theta_c = cmd_takeoff_pitch * M_PI / 180.0;
output.delta_e = pitch_hold(output.theta_c, input.theta, input.q);
}

Expand Down Expand Up @@ -238,6 +239,8 @@ float controller_successive_loop::roll_hold(float phi_c, float phi, float p)
double r_ki = get_double("r_ki");
double r_kd = get_double("r_kd");
double max_a = get_double("max_a");
double trim_a = get_double("trim_a");
double pwm_rad_a = get_double("pwm_rad_a"); // Declared in controller base

float error = phi_c - phi;

Expand All @@ -249,9 +252,9 @@ float controller_successive_loop::roll_hold(float phi_c, float phi, float p)
float ui = r_ki * r_integrator;
float ud = r_kd * p;

float delta_a = sat(up + ui - ud, max_a, -max_a);
float delta_a = sat(trim_a / pwm_rad_a + up + ui - ud, max_a, -max_a);
if (fabs(r_ki) >= 0.00001) {
float delta_a_unsat = up + ui - ud;
float delta_a_unsat = trim_a / pwm_rad_a + up + ui - ud;
r_integrator = r_integrator + (Ts / r_ki) * (delta_a - delta_a_unsat);
}

Expand Down Expand Up @@ -282,7 +285,6 @@ float controller_successive_loop::pitch_hold(float theta_c, float theta, float q

float delta_e = sat(trim_e / pwm_rad_e + up + ui - ud, max_e, -max_e);

// TODO : Why doesn't roll_hold have the trim a on it?
if (fabs(p_ki) >= 0.00001) {
float delta_e_unsat = trim_e / pwm_rad_e + up + ui - ud;
p_integrator_ = p_integrator_ + (Ts / p_ki) * (delta_e - delta_e_unsat);
Expand Down Expand Up @@ -335,6 +337,7 @@ float controller_successive_loop::altitude_hold_control(float h_c, float h)
double a_kp = get_double("a_kp");
double a_ki = get_double("a_ki");
double a_kd = get_double("a_kd");
double max_pitch = get_double("max_pitch");

float error = h_c - h;

Expand All @@ -353,7 +356,7 @@ float controller_successive_loop::altitude_hold_control(float h_c, float h)
float ui = a_ki * a_integrator_;
float ud = a_kd * a_differentiator_;

float theta_c = sat(up + ui + ud, 20.0 * 3.14 / 180.0, -20.0 * 3.14 / 180.0);
float theta_c = sat(up + ui + ud, max_pitch * M_PI / 180.0, -max_pitch * M_PI / 180.0);
if (fabs(a_ki) >= 0.00001) {
float theta_c_unsat = up + ui + ud;
a_integrator_ = a_integrator_ + (Ts / a_ki) * (theta_c - theta_c_unsat);
Expand Down Expand Up @@ -403,22 +406,24 @@ float controller_successive_loop::adjust_h_c(float h_c, float h, float max_diff)
void controller_successive_loop::declare_parameters()
{
// Declare param with ROS2 and set the default value.
std::cout << "THIS IS WORKING " << std::endl;
declare_param("max_takeoff_throttle", 0.55);
declare_param("c_kp", 2.37);
declare_param("c_ki", .4);
declare_param("c_kd", .0);
declare_param("max_roll", 25.0);
declare_param("cmd_takeoff_pitch", 5.0);

declare_param("r_kp", .06);
declare_param("r_ki", .0);
declare_param("r_kd", .04);
declare_param("max_a", .15);
declare_param("trim_a", 0.0);

declare_param("p_kp", -.15);
declare_param("p_ki", .0);
declare_param("p_kd", -.05);
declare_param("max_e", .15);
declare_param("max_pitch", 20.0);
declare_param("trim_e", 0.02);

declare_param("tau", 50.0);
Expand Down
12 changes: 8 additions & 4 deletions rosplane/src/controller_total_energy.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,13 +21,14 @@ void controller_total_energy::take_off_longitudinal_control(const struct input_s
{
// For readability, declare parameters here that will be used in this function
double max_takeoff_throttle = get_double("max_takeoff_throttle");
double cmd_takeoff_pitch = get_double("cmd_takeoff_pitch"); // Declared in controller_successive_loop

// Set throttle to not overshoot altitude.
output.delta_t = sat(total_energy_throttle(input.Va_c, input.va, input.h_c, input.h),
max_takeoff_throttle, 0);

// Command a shallow pitch angle to gain altitude.
output.theta_c = 5.0 * 3.14 / 180.0; //TODO add to params.
output.theta_c = cmd_takeoff_pitch * 3.14 / 180.0;
output.delta_e = pitch_hold(output.theta_c, input.theta, input.q);
}

Expand Down Expand Up @@ -128,6 +129,7 @@ float controller_total_energy::total_energy_pitch(float va_c, float va, float h_
double l_kp = get_double("l_kp");
double l_ki = get_double("l_ki");
double l_kd = get_double("l_kd");
double max_roll = get_double("max_roll"); // Declared in controller_successive_loop

// Update energies based off of most recent data.
update_energies(va_c, va, h_c, h);
Expand All @@ -144,15 +146,16 @@ float controller_total_energy::total_energy_pitch(float va_c, float va, float h_
L_error_prev_ = L_error;

// Return saturated pitch command.
return sat(l_kp * L_error + l_ki * L_integrator_, 25.0 * M_PI / 180.0,
-25.0 * M_PI / 180.0); // TODO remove hard coded bounds from all of rosplane!!!!
return sat(l_kp * L_error + l_ki * L_integrator_, max_roll * M_PI / 180.0,
-max_roll * M_PI / 180.0);
}

void controller_total_energy::update_energies(float va_c, float va, float h_c, float h)
{
// For readability, declare parameters here that will be used in this function
double mass = get_double("mass");
double gravity = get_double("gravity");
double max_energy = get_double("max_energy");

// Calculate the error in kinetic energy.
K_error = 0.5 * mass * (pow(va_c, 2) - pow(va, 2));
Expand All @@ -161,7 +164,7 @@ void controller_total_energy::update_energies(float va_c, float va, float h_c, f
K_ref = 0.5 * mass * pow(va_c, 2);

// Calculate the error in the potential energy.
U_error = mass * gravity * sat(h_c - h, 5, -5); // TODO add limits to param
U_error = mass * gravity * sat(h_c - h, max_energy, -max_energy);
}

void controller_total_energy::declare_parameters()
Expand All @@ -177,5 +180,6 @@ void controller_total_energy::declare_parameters()

declare_param("mass", 2.28);
declare_param("gravity", 9.8);
declare_param("max_energy", 5.0);
}
} // namespace rosplane

0 comments on commit 1f4b10e

Please sign in to comment.