mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AC_AttitudeControl: add comments
This commit is contained in:
parent
1cd537895e
commit
fd24b3912f
@ -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)
|
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);
|
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)) {
|
if (is_positive(accel_max)) {
|
||||||
desired_ang_accel = constrain_float(desired_ang_accel, -accel_max, accel_max);
|
desired_ang_accel = constrain_float(desired_ang_accel, -accel_max, accel_max);
|
||||||
}
|
}
|
||||||
|
@ -309,6 +309,7 @@ public:
|
|||||||
// limits the acceleration and deceleration of a velocity request
|
// 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);
|
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);
|
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.
|
// calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.
|
||||||
|
@ -1,6 +1,9 @@
|
|||||||
#include "AC_CommandModel.h"
|
#include "AC_CommandModel.h"
|
||||||
#include <AP_HAL/AP_HAL.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;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
// table of user settable parameters
|
// table of user settable parameters
|
||||||
|
@ -16,19 +16,21 @@ public:
|
|||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Accessors for parameters
|
||||||
float get_rate_tc() { return rate_tc; }
|
float get_rate_tc() { return rate_tc; }
|
||||||
float get_rate() { return rate; }
|
float get_rate() { return rate; }
|
||||||
float get_expo() { return expo; }
|
float get_expo() { return expo; }
|
||||||
|
|
||||||
|
// Set the max rate
|
||||||
void set_rate(float input) { rate = input; }
|
void set_rate(float input) { rate = input; }
|
||||||
|
|
||||||
// var_info for holding Parameter information
|
// var_info for holding Parameter information
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
AP_Float rate_tc;
|
AP_Float rate_tc; // rate time constant
|
||||||
AP_Float rate;
|
AP_Float rate; // maximum rate
|
||||||
AP_Float expo;
|
AP_Float expo; // expo shaping
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user