2022-04-20 00:42:03 -03:00
|
|
|
#include "AC_CommandModel.h"
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
|
2023-10-11 04:41:49 -03:00
|
|
|
// The Command Model class holds parameters that shape the pilot desired angular rate input. This class can
|
2022-05-21 23:50:38 -03:00
|
|
|
// be expanded to hold the methods that shape the pilot desired input.
|
|
|
|
|
2022-04-20 00:42:03 -03:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
|
|
// table of user settable parameters
|
|
|
|
const AP_Param::GroupInfo AC_CommandModel::var_info[] = {
|
|
|
|
|
|
|
|
// @Param: RATE
|
|
|
|
// @DisplayName: Maximum Controlled Rate
|
|
|
|
// @Description: Sets the maximum rate commanded.
|
|
|
|
// @Units: deg/s
|
|
|
|
// @Range: 1 360
|
|
|
|
// @User: Standard
|
2023-01-03 13:22:22 -04:00
|
|
|
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("RATE", 1, AC_CommandModel, rate, default_rate),
|
2022-04-20 00:42:03 -03:00
|
|
|
|
|
|
|
// @Param: EXPO
|
|
|
|
// @DisplayName: Controlled Expo
|
|
|
|
// @Description: Controlled expo to allow faster rotation when stick at edges
|
|
|
|
// @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
|
|
|
|
// @Range: -0.5 1.0
|
|
|
|
// @User: Advanced
|
2023-01-03 13:22:22 -04:00
|
|
|
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("EXPO", 2, AC_CommandModel, expo, default_expo),
|
2022-04-20 00:42:03 -03:00
|
|
|
|
|
|
|
// @Param: RATE_TC
|
|
|
|
// @DisplayName: Rate control input time constant
|
|
|
|
// @Description: Rate control input time constant. Low numbers lead to sharper response, higher numbers to softer response
|
|
|
|
// @Units: s
|
|
|
|
// @Range: 0 1
|
|
|
|
// @Increment: 0.01
|
|
|
|
// @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp
|
|
|
|
// @User: Standard
|
2023-01-03 13:22:22 -04:00
|
|
|
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("RATE_TC", 3, AC_CommandModel, rate_tc, default_rate_tc),
|
2022-04-20 00:42:03 -03:00
|
|
|
|
|
|
|
AP_GROUPEND
|
|
|
|
};
|
|
|
|
|
2022-06-15 00:10:11 -03:00
|
|
|
// Constructor
|
2023-01-03 13:22:22 -04:00
|
|
|
AC_CommandModel::AC_CommandModel(float initial_rate, float initial_expo, float initial_tc) :
|
|
|
|
default_rate(initial_rate),
|
|
|
|
default_expo(initial_expo),
|
|
|
|
default_rate_tc(initial_tc)
|
2022-06-15 00:10:11 -03:00
|
|
|
{
|
|
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
|
|
}
|
|
|
|
|