AC_AttitudeControl: add comments

This commit is contained in:
Bill Geyer 2022-05-21 22:50:38 -04:00 committed by Randy Mackay
parent 1cd537895e
commit fd24b3912f
4 changed files with 12 additions and 4 deletions

View File

@ -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);
}

View File

@ -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.

View File

@ -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

View File

@ -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
};