Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: add support for command model class

This commit is contained in:
Bill Geyer 2022-06-14 23:08:53 -04:00 committed by Randy Mackay
parent fd24b3912f
commit 4fde394395
8 changed files with 154 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -45,6 +45,7 @@
#include <AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h> // 6DoF Attitude control library
#include <AC_AttitudeControl/AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
#include <AC_AttitudeControl/AC_PosControl.h> // Position control library
#include <AC_AttitudeControl/AC_CommandModel.h> // Command model library
#include <AP_Motors/AP_Motors.h> // AP Motors library
#include <AP_Stats/AP_Stats.h> // statistics library
#include <Filter/Filter.h> // Filter library

154
ArduCopter/Parameters.cpp
View File

@ -443,6 +443,8 @@ const AP_Param::Info Copter::var_info[] = {
GSCALAR(acro_balance_pitch, "ACRO_BAL_PITCH", ACRO_BALANCE_PITCH),
#endif
// ACRO_RP_EXPO moved to Command Model class
#if MODE_ACRO_ENABLED == ENABLED
// @Param: ACRO_TRAINER
// @DisplayName: Acro Trainer
@ -450,14 +452,6 @@ const AP_Param::Info Copter::var_info[] = {
// @Values: 0:Disabled,1:Leveling,2:Leveling and Limited
// @User: Advanced
GSCALAR(acro_trainer, "ACRO_TRAINER", (uint8_t)ModeAcro::Trainer::LIMITED),
// @Param: ACRO_RP_EXPO
// @DisplayName: Acro Roll/Pitch Expo
// @Description: Acro roll/pitch 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 0.95
// @User: Advanced
GSCALAR(acro_rp_expo, "ACRO_RP_EXPO", ACRO_RP_EXPO_DEFAULT),
#endif
// variables not in the g class which contain EEPROM saved variables
@ -807,13 +801,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(proximity, "PRX", 8, ParametersG2, AP_Proximity),
#endif
// @Param: ACRO_Y_EXPO
// @DisplayName: Acro Yaw Expo
// @Description: Acro yaw 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: -1.0 0.95
// @User: Advanced
AP_GROUPINFO("ACRO_Y_EXPO", 9, ParametersG2, acro_y_expo, ACRO_Y_EXPO_DEFAULT),
// ACRO_Y_EXPO (9) moved to Command Model Class
#if MODE_ACRO_ENABLED == ENABLED
// @Param: ACRO_THR_MID
@ -1056,41 +1044,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0),
#endif
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
// @Param: ACRO_RP_RATE
// @DisplayName: Acro Roll and Pitch Rate
// @Description: Acro mode maximum roll and pitch rate. Higher values mean faster rate of rotation
// @Units: deg/s
// @Range: 1 1080
// @User: Standard
AP_GROUPINFO("ACRO_RP_RATE", 47, ParametersG2, acro_rp_rate, ACRO_RP_RATE_DEFAULT),
#endif
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
// @Param: ACRO_Y_RATE
// @DisplayName: Acro Yaw Rate
// @Description: Acro mode maximum yaw rate. Higher value means faster rate of rotation
// @Units: deg/s
// @Range: 1 360
// @User: Standard
AP_GROUPINFO("ACRO_Y_RATE", 48, ParametersG2, acro_y_rate, ACRO_Y_RATE_DEFAULT),
#endif
// @Param: PILOT_Y_RATE
// @DisplayName: Pilot controlled yaw rate
// @Description: Pilot controlled yaw rate max. Used in all pilot controlled modes except Acro
// @Units: deg/s
// @Range: 1 360
// @User: Standard
AP_GROUPINFO("PILOT_Y_RATE", 49, ParametersG2, pilot_y_rate, PILOT_Y_RATE_DEFAULT),
// @Param: PILOT_Y_EXPO
// @DisplayName: Pilot controlled yaw expo
// @Description: Pilot controlled yaw 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
AP_GROUPINFO("PILOT_Y_EXPO", 50, ParametersG2, pilot_y_expo, PILOT_Y_EXPO_DEFAULT),
// ACRO_PR_RATE (47), ACRO_Y_RATE (48), PILOT_Y_RATE (49) and PILOT_Y_EXPO (50) moved to command model class
// @Param: SURFTRAK_MODE
// @DisplayName: Surface Tracking Mode
@ -1113,6 +1067,82 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard
AP_GROUPINFO("FS_DR_TIMEOUT", 53, ParametersG2, failsafe_dr_timeout, 30),
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
// @Param: ACRO_RP_RATE
// @DisplayName: Acro Roll and Pitch Rate
// @Description: Acro mode maximum roll and pitch rate. Higher values mean faster rate of rotation
// @Units: deg/s
// @Range: 1 1080
// @User: Standard
// @Param: ACRO_RP_EXPO
// @DisplayName: Acro Roll/Pitch Expo
// @Description: Acro roll/pitch 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 0.95
// @User: Advanced
// @Param: ACRO_RP_RATE_TC
// @DisplayName: Acro roll/pitch rate control input time constant
// @Description: Acro roll and pitch 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
AP_SUBGROUPINFO(command_model_acro_rp, "ACRO_RP_", 54, ParametersG2, AC_CommandModel),
#endif
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
// @Param: ACRO_Y_RATE
// @DisplayName: Acro Yaw Rate
// @Description: Acro mode maximum yaw rate. Higher value means faster rate of rotation
// @Units: deg/s
// @Range: 1 360
// @User: Standard
// @Param: ACRO_Y_EXPO
// @DisplayName: Acro Yaw Expo
// @Description: Acro yaw 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: -1.0 0.95
// @User: Advanced
// @Param: ACRO_Y_RATE_TC
// @DisplayName: Acro yaw rate control input time constant
// @Description: Acro yaw 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
AP_SUBGROUPINFO(command_model_acro_y, "ACRO_Y_", 55, ParametersG2, AC_CommandModel),
#endif
// @Param: PILOT_Y_RATE
// @DisplayName: Pilot controlled yaw rate
// @Description: Pilot controlled yaw rate max. Used in all pilot controlled modes except Acro
// @Units: deg/s
// @Range: 1 360
// @User: Standard
// @Param: PILOT_Y_EXPO
// @DisplayName: Pilot controlled yaw expo
// @Description: Pilot controlled yaw 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
// @Param: PILOT_Y_RATE_TC
// @DisplayName: Pilot yaw rate control input time constant
// @Description: Pilot yaw 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
AP_SUBGROUPINFO(command_model_pilot, "PILOT_Y_", 56, ParametersG2, AC_CommandModel),
AP_GROUPEND
};
@ -1157,6 +1187,16 @@ ParametersG2::ParametersG2(void)
#if MODE_ZIGZAG_ENABLED == ENABLED
,mode_zigzag_ptr(&copter.mode_zigzag)
#endif
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
,command_model_acro_rp(ACRO_RP_RATE_DEFAULT, ACRO_RP_EXPO_DEFAULT, 0.05)
#endif
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
,command_model_acro_y(ACRO_Y_RATE_DEFAULT, ACRO_Y_EXPO_DEFAULT, 0.05)
#endif
,command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.05)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -1453,6 +1493,20 @@ void Copter::convert_pid_parameters(void)
AP_Param::convert_old_parameter(&info, 45.0);
}
// convert rate and expo command model parameters for Copter-4.3
// PARAMETER_CONVERSION - Added: June-2022
const AP_Param::ConversionInfo cmd_mdl_conversion_info[] = {
{ Parameters::k_param_g2, 47, AP_PARAM_FLOAT, "ACRO_RP_RATE" },
{ Parameters::k_param_acro_rp_expo, 0, AP_PARAM_FLOAT, "ACRO_RP_EXPO" },
{ Parameters::k_param_g2, 48, AP_PARAM_FLOAT, "ACRO_Y_RATE" },
{ Parameters::k_param_g2, 9, AP_PARAM_FLOAT, "ACRO_Y_EXPO" },
{ Parameters::k_param_g2, 49, AP_PARAM_FLOAT, "PILOT_Y_RATE" },
{ Parameters::k_param_g2, 50, AP_PARAM_FLOAT, "PILOT_Y_EXPO" },
};
for (const auto &info : cmd_mdl_conversion_info) {
AP_Param::convert_old_parameter(&info, 1.0);
}
// make any SRV_Channel upgrades needed
SRV_Channels::upgrade_parameters();
}

23
ArduCopter/Parameters.h
View File

@ -128,7 +128,7 @@ public:
k_param_rangefinder, // rangefinder object
k_param_fs_ekf_thresh,
k_param_terrain,
k_param_acro_rp_expo,
k_param_acro_rp_expo, // deprecated - remove
k_param_throttle_deadzone,
k_param_optflow,
k_param_dcmcheck_thresh, // deprecated - remove
@ -540,7 +540,7 @@ public:
AP_Int32 dev_options;
// acro exponent parameters
AP_Float acro_y_expo;
AP_Float acro_y_expo; // remove
#if MODE_ACRO_ENABLED == ENABLED
AP_Float acro_thr_mid;
#endif
@ -627,6 +627,17 @@ public:
void *mode_zigzag_ptr;
#endif
// command model parameters
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
AC_CommandModel command_model_acro_rp;
#endif
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
AC_CommandModel command_model_acro_y;
#endif
AC_CommandModel command_model_pilot;
#if MODE_ACRO_ENABLED == ENABLED
AP_Int8 acro_options;
#endif
@ -657,15 +668,15 @@ public:
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
// Acro parameters
AP_Float acro_rp_rate;
AP_Float acro_rp_rate; // remove
#endif
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
AP_Float acro_y_rate;
AP_Float acro_y_rate; // remove
#endif
AP_Float pilot_y_rate;
AP_Float pilot_y_expo;
AP_Float pilot_y_rate; // remove
AP_Float pilot_y_expo; // remove
AP_Int8 surftrak_mode;
AP_Int8 failsafe_dr_enable;
AP_Int16 failsafe_dr_timeout;

17
ArduCopter/mode.cpp
View File

@ -207,6 +207,10 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
// return immediately if we are already in the desired mode
if (mode == flightmode->mode_number()) {
control_mode_reason = reason;
// set yaw rate time constant during autopilot startup
if (reason == ModeReason::INITIALISED && mode == Mode::Number::STABILIZE) {
attitude_control->set_yaw_rate_tc(g2.command_model_pilot.get_rate_tc());
}
// make happy noise
if (copter.ap.initialised && (reason != last_reason)) {
AP_Notify::events.user_mode_change = 1;
@ -319,6 +323,17 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
camera.set_is_auto_mode(flightmode->mode_number() == Mode::Number::AUTO);
#endif
// set rate shaping time constants
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
attitude_control->set_roll_pitch_rate_tc(g2.command_model_acro_rp.get_rate_tc());
#endif
attitude_control->set_yaw_rate_tc(g2.command_model_pilot.get_rate_tc());
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
if (mode== Mode::Number::ACRO || mode== Mode::Number::DRIFT) {
attitude_control->set_yaw_rate_tc(g2.command_model_acro_y.get_rate_tc());
}
#endif
// update notify object
notify_flight_mode();
@ -962,7 +977,7 @@ float Mode::get_pilot_desired_yaw_rate(float yaw_in)
}
// convert pilot input to the desired yaw rate
return g2.pilot_y_rate * 100.0 * input_expo(yaw_in, g2.pilot_y_expo);
return g2.command_model_pilot.get_rate() * 100.0 * input_expo(yaw_in, g2.command_model_pilot.get_expo());
}
// pass-through functions to reduce code churn on conversion;

14
ArduCopter/mode_acro.cpp
View File

@ -113,13 +113,13 @@ void ModeAcro::get_pilot_desired_angle_rates(float roll_in, float pitch_in, floa
// calculate roll, pitch rate requests
// roll expo
rate_bf_request_cd.x = g2.acro_rp_rate * 100.0 * input_expo(roll_in, g.acro_rp_expo);
rate_bf_request_cd.x = g2.command_model_acro_rp.get_rate() * 100.0 * input_expo(roll_in, g2.command_model_acro_rp.get_expo());
// pitch expo
rate_bf_request_cd.y = g2.acro_rp_rate * 100.0 * input_expo(pitch_in, g.acro_rp_expo);
rate_bf_request_cd.y = g2.command_model_acro_rp.get_rate() * 100.0 * input_expo(pitch_in, g2.command_model_acro_rp.get_expo());
// yaw expo
rate_bf_request_cd.z = g2.acro_y_rate * 100.0 * input_expo(yaw_in, g2.acro_y_expo);
rate_bf_request_cd.z = g2.command_model_acro_y.get_rate() * 100.0 * input_expo(yaw_in, g2.command_model_acro_y.get_expo());
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
@ -143,15 +143,15 @@ void ModeAcro::get_pilot_desired_angle_rates(float roll_in, float pitch_in, floa
if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
const float angle_max = copter.aparm.angle_max;
if (roll_angle > angle_max){
rate_ef_level_cd.x += sqrt_controller(angle_max - roll_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
rate_ef_level_cd.x += sqrt_controller(angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
}else if (roll_angle < -angle_max) {
rate_ef_level_cd.x += sqrt_controller(-angle_max - roll_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
rate_ef_level_cd.x += sqrt_controller(-angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
}
if (pitch_angle > angle_max){
rate_ef_level_cd.y += sqrt_controller(angle_max - pitch_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
rate_ef_level_cd.y += sqrt_controller(angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
}else if (pitch_angle < -angle_max) {
rate_ef_level_cd.y += sqrt_controller(-angle_max - pitch_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
rate_ef_level_cd.y += sqrt_controller(-angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
}
}

2
ArduCopter/mode_drift.cpp
View File

@ -54,7 +54,7 @@ void ModeDrift::run()
// gain scheduling for yaw
float pitch_vel2 = MIN(fabsf(pitch_vel), 2000);
float target_yaw_rate = target_roll * (1.0f - (pitch_vel2 / 5000.0f)) * g2.acro_y_rate / 45.0;
float target_yaw_rate = target_roll * (1.0f - (pitch_vel2 / 5000.0f)) * g2.command_model_acro_y.get_rate() / 45.0;
roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);

12
ArduCopter/mode_sport.cpp
View File

@ -34,8 +34,8 @@ void ModeSport::run()
// get pilot's desired roll and pitch rates
// calculate rate requests
float target_roll_rate = channel_roll->get_control_in() * g2.acro_rp_rate * 100.0 / ROLL_PITCH_YAW_INPUT_MAX;
float target_pitch_rate = channel_pitch->get_control_in() * g2.acro_rp_rate * 100.0 / ROLL_PITCH_YAW_INPUT_MAX;
float target_roll_rate = channel_roll->get_control_in() * g2.command_model_acro_rp.get_rate() * 100.0 / ROLL_PITCH_YAW_INPUT_MAX;
float target_pitch_rate = channel_pitch->get_control_in() * g2.command_model_acro_rp.get_rate() * 100.0 / ROLL_PITCH_YAW_INPUT_MAX;
// get attitude targets
const Vector3f att_target = attitude_control->get_att_target_euler_cd();
@ -50,15 +50,15 @@ void ModeSport::run()
const float angle_max = copter.aparm.angle_max;
if (roll_angle > angle_max){
target_roll_rate += sqrt_controller(angle_max - roll_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
target_roll_rate += sqrt_controller(angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
}else if (roll_angle < -angle_max) {
target_roll_rate += sqrt_controller(-angle_max - roll_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
target_roll_rate += sqrt_controller(-angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
}
if (pitch_angle > angle_max){
target_pitch_rate += sqrt_controller(angle_max - pitch_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
target_pitch_rate += sqrt_controller(angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
}else if (pitch_angle < -angle_max) {
target_pitch_rate += sqrt_controller(-angle_max - pitch_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
target_pitch_rate += sqrt_controller(-angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
}
// get pilot's desired yaw rate

4
ArduCopter/tuning.cpp
View File

@ -107,14 +107,14 @@ void Copter::tuning()
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
// Acro roll pitch rates
case TUNING_ACRO_RP_RATE:
g2.acro_rp_rate = tuning_value;
g2.command_model_acro_rp.set_rate(tuning_value);
break;
#endif
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
// Acro yaw rate
case TUNING_ACRO_YAW_RATE:
g2.acro_y_rate = tuning_value;
g2.command_model_acro_y.set_rate(tuning_value);
break;
#endif