Switched controller to use frequency in calculations.

rosplane/include/controller_successive_loop.hpp

@@ -117,10 +117,9 @@ class controller_successive_loop : public controller_state_machine
    * @param phi_ff The roll angle feedforward term. This allows for faster convergence.   // TODO add reference to book?
    * @param r The yaw rate taken from the gyro.
    * @param params The parameters for the control algorithm, including the control gains.
-   * @param Ts The sampling period.
    * @return The commanded roll angle, to achieve the course angle.
    */
-  float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s &params, float Ts);
+  float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s &params);
 
   /**
    * The difference between the commanded course angle and the current course angle.
@@ -138,10 +137,9 @@ class controller_successive_loop : public controller_state_machine
    * @param phi The current roll angle.
    * @param p The roll rate taken from the gyro.
    * @param params The parameters for the control algorithm, including the control gains and max deflection.
-   * @param Ts The sampling period.
    * @return The aileron deflection in radians required to achieve the commanded roll angle.
    */
-  float roll_hold(float phi_c, float phi, float p, const struct params_s &params, float Ts);
+  float roll_hold(float phi_c, float phi, float p, const struct params_s &params);
 
   /**
    * The difference between the commanded roll angle and the current roll angle.
@@ -159,10 +157,9 @@ class controller_successive_loop : public controller_state_machine
    * @param theta The current pitch angle.
    * @param q The pitch rate taken from the gyro.
    * @param params The parameters for the control algorithm, including the control gains and max deflection.
-   * @param Ts The sampling period
    * @return The elevator deflection in radians required to achieve the commanded pitch.
    */
-  float pitch_hold(float theta_c, float theta, float q, const struct params_s &params, float Ts);
+  float pitch_hold(float theta_c, float theta, float q, const struct params_s &params);
 
   /**
    * The difference between the commanded pitch angle and the current pitch angle.
@@ -179,10 +176,9 @@ class controller_successive_loop : public controller_state_machine
    * @param Va_c The commanded airspeed.
    * @param Va The current airspeed.
    * @param params The parameters for the control algorithm, including control gains.
-   * @param Ts The sampling period.
    * @return The required throttle between 0 (no throttle) and 1 (full throttle).
    */
-  float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s &params, float Ts);
+  float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s &params);
 
   /**
    * The difference between the commanded airspeed and the current airspeed.
@@ -204,10 +200,9 @@ class controller_successive_loop : public controller_state_machine
    * @param h_c The commanded altitude.
    * @param h The current altitude.
    * @param params The parameters for the control algorithm, including control gains.
-   * @param Ts The sampling period
    * @return The commanded pitch angle to maintain and achieve the commanded altitude.
    */
-  float altitude_hold_control(float h_c, float h, const struct params_s &params, float Ts);
+  float altitude_hold_control(float h_c, float h, const struct params_s &params);
 
   /**
    * The difference between the commanded altitude and the current altitude.

rosplane/include/controller_total_energy.hpp

@@ -70,7 +70,7 @@ namespace rosplane
    * @param Ts The sampling period in seconds.
    * @return The throttle value saturated between 0 and the parameter of max throttle.
    */
-  float total_energy_throttle(float va_c, float va, float h_c, float h, const struct params_s &params, float Ts);
+  float total_energy_throttle(float va_c, float va, float h_c, float h, const struct params_s &params);
 
   /**
    * This uses the error in the balance of energy to find the necessary elevator deflection to acheive that energy.
@@ -82,7 +82,7 @@ namespace rosplane
    * @param Ts The sampling period in seconds.
    * @return The pitch command value saturated between min and max pitch.
    */
-  float total_energy_pitch(float va_c, float va, float h_c, float h, const struct params_s &params, float Ts);
+  float total_energy_pitch(float va_c, float va, float h_c, float h, const struct params_s &params);
 
   /**
    * This calculates and updates the kinetic energy reference and error, the potential energy error.

rosplane/src/controller_base.cpp

@@ -82,7 +82,6 @@ void controller_base::actuator_controls_publish()
   input.h_c = controller_commands_.h_c;
   input.chi_c = controller_commands_.chi_c;
   input.phi_ff = controller_commands_.phi_ff;
-  input.Ts = 0.01f;
 
 
   struct output_s output;

rosplane/src/controller_successive_loop.cpp

@@ -71,14 +71,14 @@ void controller_successive_loop::alt_hold_lateral_control(const struct params_s
   // Set rudder command to zero, can use cooridinated_turn_hold if implemented.
   // Find commanded roll angle in order to achieve commanded course.
   // Find aileron deflection required to acheive required roll angle.
-  output.delta_r = 0; //cooridinated_turn_hold(input.beta, params, input.Ts)
-  output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params, input.Ts);
+  output.delta_r = 0; //cooridinated_turn_hold(input.beta, params)
+  output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params);
 
   if (params.roll_tuning_debug_override){
     output.phi_c = tuning_debug_override_msg_.phi_c;
   }
 
-  output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts);
+  output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params);
 }
 
 void controller_successive_loop::alt_hold_longitudinal_control(const struct params_s &params, const struct input_s &input, struct output_s &output)
@@ -87,21 +87,21 @@ void controller_successive_loop::alt_hold_longitudinal_control(const struct para
   double adjusted_hc = adjust_h_c(input.h_c, input.h, params.alt_hz);
 
   // Control airspeed with throttle loop and altitude with commanded pitch and drive aircraft to commanded pitch.
-  output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts);
-  output.theta_c = altitude_hold_control(adjusted_hc, input.h, params, input.Ts);
+  output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params);
+  output.theta_c = altitude_hold_control(adjusted_hc, input.h, params);
 
   if (params.pitch_tuning_debug_override){
     output.theta_c = tuning_debug_override_msg_.theta_c;
   }
 
-  output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
+  output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params);
 }
 
 void controller_successive_loop::climb_lateral_control(const struct params_s &params, const struct input_s &input, struct output_s &output)
 {
   // Maintain straight flight while gaining altitude.
   output.phi_c = 0;
-  output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts);
+  output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params);
   output.delta_r = 0;
 }
 
@@ -111,37 +111,39 @@ void controller_successive_loop::climb_longitudinal_control(const struct params_
   double adjusted_hc = adjust_h_c(input.h_c, input.h, params.alt_hz);
 
   // Find the control efforts for throttle and find the commanded pitch angle.
-  output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts);
-  output.theta_c = altitude_hold_control(adjusted_hc, input.h, params, input.Ts);
-  output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
+  output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params);
+  output.theta_c = altitude_hold_control(adjusted_hc, input.h, params);
+  output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params);
 }
 
 void controller_successive_loop::take_off_lateral_control(const struct params_s &params, const struct input_s &input, struct output_s &output)
 {
   // In the take-off zone maintain level straight flight by commanding a roll angle of 0 and rudder of 0.
   output.delta_r = 0;
   output.phi_c = 0;
-  output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts);
+  output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params);
 }
 
 void controller_successive_loop::take_off_longitudinal_control(const struct params_s &params, const struct input_s &input, struct output_s &output)
 {
   // Set throttle to not overshoot altitude.
-  output.delta_t = sat(airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts), params.max_takeoff_throttle, 0);
+  output.delta_t = sat(airspeed_with_throttle_hold(input.Va_c, input.va, params), params.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.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
+  output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params);
 }
 
 /// All the following control loops follow this basic outline.
 /*
     float controller_successive_loop::pid_control(float command_val, float actual_val, float rate, // Not all loops use rate.
-                                          const params_s &params, float Ts)
+                                          const params_s &params)
     {
       // Find the error between the commanded and actual value.
       float error = commanded_val - actual_val;
 
+      float Ts = 1.0/params.frequency;
+
       // Integrate the error of the state by using the trapezoid method with the stored value for the previous error.
       state_integrator_ = state_integrator_ + (Ts/2.0)*(error + state_error_);
 
@@ -171,15 +173,15 @@ void controller_successive_loop::take_off_longitudinal_control(const struct para
     }
 */
 
-float controller_successive_loop::course_hold(float chi_c, float chi, float phi_ff, float r, const params_s &params, float Ts)
+float controller_successive_loop::course_hold(float chi_c, float chi, float phi_ff, float r, const params_s &params)
 {
 
   double wrapped_chi_c = wrap_within_180(chi, chi_c);
 
 
   float error = wrapped_chi_c - chi;
 
-  // RCLCPP_INFO_STREAM(this->get_logger(), "chi error: " << error);
+  float Ts = 1.0/params.frequency;
 
   c_integrator_ = c_integrator_ + (Ts/2.0)*(error + c_error_);
 
@@ -198,10 +200,12 @@ float controller_successive_loop::course_hold(float chi_c, float chi, float phi_
   return phi_c;
 }
 
-float controller_successive_loop::roll_hold(float phi_c, float phi, float p, const params_s &params, float Ts)
+float controller_successive_loop::roll_hold(float phi_c, float phi, float p, const params_s &params)
 {
   float error = phi_c - phi;
 
+  float Ts = 1.0/params.frequency;
+
   r_integrator = r_integrator + (Ts/2.0)*(error + r_error_);
 
   float up = params.r_kp*error;
@@ -219,10 +223,12 @@ float controller_successive_loop::roll_hold(float phi_c, float phi, float p, con
   return delta_a;
 }
 
-float controller_successive_loop::pitch_hold(float theta_c, float theta, float q, const params_s &params, float Ts)
+float controller_successive_loop::pitch_hold(float theta_c, float theta, float q, const params_s &params)
 {
   float error = theta_c - theta;
 
+  float Ts = 1.0/params.frequency;
+
   p_integrator_ = p_integrator_ + (Ts/2.0)*(error + p_error_);
 
   float up = params.p_kp*error;
@@ -243,10 +249,12 @@ float controller_successive_loop::pitch_hold(float theta_c, float theta, float q
   return -delta_e; // TODO explain subtraction.
 }
 
-float controller_successive_loop::airspeed_with_throttle_hold(float Va_c, float Va, const params_s &params, float Ts)
+float controller_successive_loop::airspeed_with_throttle_hold(float Va_c, float Va, const params_s &params)
 {
   float error = Va_c - Va;
 
+  float Ts = 1.0/params.frequency;
+
   at_integrator_ = at_integrator_ + (Ts/2.0)*(error + at_error_);
   at_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*at_differentiator_ + (2.0 /
                        (2.0*params.tau + Ts))*(error - at_error_);
@@ -266,10 +274,12 @@ float controller_successive_loop::airspeed_with_throttle_hold(float Va_c, float
   return delta_t;
 }
 
-float controller_successive_loop::altitude_hold_control(float h_c, float h, const params_s &params, float Ts)
+float controller_successive_loop::altitude_hold_control(float h_c, float h, const params_s &params)
 {
   float error = h_c - h;
 
+  float Ts = 1.0/params.frequency;
+
   if (-params.alt_hz + .01 < error && error < params.alt_hz - .01) {
     a_integrator_ = a_integrator_ + (Ts / 2.0) * (error + a_error_);
   }

rosplane/src/controller_total_energy.cpp

@@ -13,11 +13,11 @@ controller_total_energy::controller_total_energy() : controller_successive_loop(
 void controller_total_energy::take_off_longitudinal_control(const struct params_s &params, const struct input_s &input, struct output_s &output)
 {
   // Set throttle to not overshoot altitude.
-  output.delta_t = sat(total_energy_throttle(input.Va_c, input.va, input.h_c, input.h, params, input.Ts), params.max_takeoff_throttle, 0);
+  output.delta_t = sat(total_energy_throttle(input.Va_c, input.va, input.h_c, input.h, params), params.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.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
+  output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params);
 }
 
 void controller_total_energy::take_off_exit()
@@ -36,9 +36,9 @@ void controller_total_energy::climb_longitudinal_control(const struct params_s &
   double adjusted_hc = adjust_h_c(input.h_c, input.h, params.alt_hz/2.0);
 
   // Find the control efforts for throttle and find the commanded pitch angle using total energy.
-  output.delta_t = total_energy_throttle(input.Va_c, input.va, adjusted_hc, input.h, params, input.Ts);
-  output.theta_c = total_energy_pitch(input.Va_c, input.va, adjusted_hc, input.h, params, input.Ts);
-  output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
+  output.delta_t = total_energy_throttle(input.Va_c, input.va, adjusted_hc, input.h, params);
+  output.theta_c = total_energy_pitch(input.Va_c, input.va, adjusted_hc, input.h, params);
+  output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params);
 }
 
 void controller_total_energy::climb_exit()
@@ -58,9 +58,9 @@ void controller_total_energy::alt_hold_longitudinal_control(const struct params_
   double adjusted_hc = adjust_h_c(input.h_c, input.h, params.alt_hz);
 
   // Calculate the control effort to maintain airspeed and the required pitch angle to maintain altitude.
-  output.delta_t = total_energy_throttle(input.Va_c, input.va, adjusted_hc, input.h, params, input.Ts);
-  output.theta_c = total_energy_pitch(input.Va_c, input.va, adjusted_hc, input.h, params, input.Ts); // TODO remove capital from Va_c
-  output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
+  output.delta_t = total_energy_throttle(input.Va_c, input.va, adjusted_hc, input.h, params);
+  output.theta_c = total_energy_pitch(input.Va_c, input.va, adjusted_hc, input.h, params); // TODO remove capital from Va_c
+  output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params);
 }
 
 void controller_total_energy::altitude_hold_exit()
@@ -74,14 +74,16 @@ void controller_total_energy::altitude_hold_exit()
 }
 
 float controller_total_energy::total_energy_throttle(float va_c, float va, float h_c, float h,
-                                                     const struct params_s &params, float Ts)
+                                                     const struct params_s &params)
 {
   // Update energies based off of most recent data.
   update_energies(va_c, va, h_c, h, params);
 
   // Calculate total energy error, and normalize relative to the desired kinetic energy.
   float E_error = (K_error + U_error) / K_ref;
 
+  float Ts = 1.0/params.frequency;
+
   // Integrate error.
   E_integrator_ = E_integrator_ + (Ts/2.0)*(E_error + E_error_prev_);
 
@@ -96,7 +98,7 @@ float controller_total_energy::total_energy_throttle(float va_c, float va, float
 }
 
 float controller_total_energy::total_energy_pitch(float va_c, float va, float h_c, float h,
-                                                  const struct params_s &params, float Ts)
+                                                  const struct params_s &params)
 {
   // Update energies based off of most recent data.
   update_energies(va_c, va, h_c, h, params);
@@ -105,6 +107,8 @@ float controller_total_energy::total_energy_pitch(float va_c, float va, float h_
 
   float L_error = (U_error - K_error) / K_ref;
 
+  float Ts = 1.0/params.frequency;
+
   // Integrate error.
   L_integrator_ = L_integrator_ + (Ts/2.0)*(L_error + L_error_prev_);