Browse Source

AC_AttitudeControl: add comments

apm_2208
Bill Geyer 3 years ago committed by Randy Mackay
parent
commit
fd24b3912f
  1. 4
      libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
  2. 1
      libraries/AC_AttitudeControl/AC_AttitudeControl.h
  3. 3
      libraries/AC_AttitudeControl/AC_CommandModel.cpp
  4. 8
      libraries/AC_AttitudeControl/AC_CommandModel.h

4
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@ -878,11 +878,13 @@ float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desi @@ -878,11 +878,13 @@ float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desi
}
}
// calculates the accleration correction from an rate error. The angular acceleration is limited.
// calculates the accleration correction from an rate error. The angular acceleration and deceleration is limited.
float AC_AttitudeControl::input_shaping_rate(float error_rate, float input_tc, float accel_max, float target_ang_vel, float dt)
{
// Calculate the acceleration to smoothly achieve rate. Jerk is not limited.
float desired_ang_accel = sqrt_controller(error_rate, 1.0f / MAX(input_tc, 0.01f), 0.0f, dt);
// limit acceleration or deceleration
if (is_positive(accel_max)) {
desired_ang_accel = constrain_float(desired_ang_accel, -accel_max, accel_max);
}

1
libraries/AC_AttitudeControl/AC_AttitudeControl.h

@ -309,6 +309,7 @@ public: @@ -309,6 +309,7 @@ public:
// limits the acceleration and deceleration of a velocity request
static float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt);
// calculates the accleration correction from an rate error. The angular acceleration and deceleration is limited.
static float input_shaping_rate(float error_rate, float input_tc, float accel_max, float target_ang_vel, float dt);
// calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.

3
libraries/AC_AttitudeControl/AC_CommandModel.cpp

@ -1,6 +1,9 @@ @@ -1,6 +1,9 @@
#include "AC_CommandModel.h"
#include <AP_HAL/AP_HAL.h>
// The Commmand Model class holds parameters that shape the pilot desired angular rate input. This class can
// be expanded to hold the methods that shape the pilot desired input.
extern const AP_HAL::HAL& hal;
// table of user settable parameters

8
libraries/AC_AttitudeControl/AC_CommandModel.h

@ -16,19 +16,21 @@ public: @@ -16,19 +16,21 @@ public:
AP_Param::setup_object_defaults(this, var_info);
}
// Accessors for parameters
float get_rate_tc() { return rate_tc; }
float get_rate() { return rate; }
float get_expo() { return expo; }
// Set the max rate
void set_rate(float input) { rate = input; }
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
AP_Float rate_tc;
AP_Float rate;
AP_Float expo;
AP_Float rate_tc; // rate time constant
AP_Float rate; // maximum rate
AP_Float expo; // expo shaping
};

Loading…
Cancel
Save