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

AC_AutoTune: remove check_level function and LevelIssue

This commit is contained in:
Iampete1 2022-02-03 21:05:41 +00:00 committed by Randy Mackay
parent 12210f9066
commit 08fcbedaee
2 changed files with 7 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -112,27 +112,6 @@ bool AC_AutoTune::init_position_controller(void)
return true;
}
const char *AC_AutoTune::level_issue_string() const
{
switch (level_problem.issue) {
case LevelIssue::NONE:
return "None";
case LevelIssue::ANGLE_ROLL:
return "Angle(R)";
case LevelIssue::ANGLE_PITCH:
return "Angle(P)";
case LevelIssue::ANGLE_YAW:
return "Angle(Y)";
case LevelIssue::RATE_ROLL:
return "Rate(R)";
case LevelIssue::RATE_PITCH:
return "Rate(P)";
case LevelIssue::RATE_YAW:
return "Rate(Y)";
}
return "Bug";
}
void AC_AutoTune::send_step_string()
{
if (pilot_override) {
@ -141,7 +120,7 @@ void AC_AutoTune::send_step_string()
}
switch (step) {
case WAITING_FOR_LEVEL:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Leveling (%s %4.1f > %4.1f)", level_issue_string(), (double)(level_problem.current*0.01f), (double)(level_problem.maximum*0.01f));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Leveling");
return;
case UPDATE_GAINS:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains");
@ -258,18 +237,6 @@ void AC_AutoTune::run()
}
// check if current is greater than maximum and update level_problem structure
bool AC_AutoTune::check_level(const LevelIssue issue, const float current, const float maximum)
{
if (current > maximum) {
level_problem.current = current;
level_problem.maximum = maximum;
level_problem.issue = issue;
return false;
}
return true;
}
// return true if vehicle is close to level
bool AC_AutoTune::currently_level()
{
@ -289,35 +256,23 @@ bool AC_AutoTune::currently_level()
level_fail_warning_time_ms = now_ms;
}
if (!check_level(LevelIssue::ANGLE_ROLL,
fabsf(ahrs_view->roll_sensor - roll_cd),
threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {
if (fabsf(ahrs_view->roll_sensor - roll_cd) > threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD) {
return false;
}
if (!check_level(LevelIssue::ANGLE_PITCH,
fabsf(ahrs_view->pitch_sensor - pitch_cd),
threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {
if (fabsf(ahrs_view->pitch_sensor - pitch_cd) > threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD) {
return false;
}
if (!check_level(LevelIssue::ANGLE_YAW,
fabsf(wrap_180_cd(ahrs_view->yaw_sensor - desired_yaw_cd)),
threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {
if (fabsf(wrap_180_cd(ahrs_view->yaw_sensor - desired_yaw_cd)) > threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD) {
return false;
}
if (!check_level(LevelIssue::RATE_ROLL,
(ToDeg(ahrs_view->get_gyro().x) * 100.0f),
threshold_mul*AUTOTUNE_LEVEL_RATE_RP_CD)) {
if ((ToDeg(ahrs_view->get_gyro().x) * 100.0f) > threshold_mul*AUTOTUNE_LEVEL_RATE_RP_CD) {
return false;
}
if (!check_level(LevelIssue::RATE_PITCH,
(ToDeg(ahrs_view->get_gyro().y) * 100.0f),
threshold_mul*AUTOTUNE_LEVEL_RATE_RP_CD)) {
if ((ToDeg(ahrs_view->get_gyro().y) * 100.0f) > threshold_mul*AUTOTUNE_LEVEL_RATE_RP_CD) {
return false;
}
if (!check_level(LevelIssue::RATE_YAW,
(ToDeg(ahrs_view->get_gyro().z) * 100.0f),
threshold_mul*AUTOTUNE_LEVEL_RATE_Y_CD)) {
if ((ToDeg(ahrs_view->get_gyro().z) * 100.0f) > threshold_mul*AUTOTUNE_LEVEL_RATE_Y_CD) {
return false;
}
return true;

22
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -292,22 +292,6 @@ private:
// directly updates attitude controller with targets
void control_attitude();
// convert latest level issue to string for reporting
const char *level_issue_string() const;
enum struct LevelIssue {
NONE,
ANGLE_ROLL,
ANGLE_PITCH,
ANGLE_YAW,
RATE_ROLL,
RATE_PITCH,
RATE_YAW,
};
// check if current is greater than maximum and update level_problem structure
bool check_level(const enum LevelIssue issue, const float current, const float maximum);
// returns true if vehicle is close to level
bool currently_level();
@ -333,10 +317,4 @@ private:
// time in ms of last pilot override warning
uint32_t last_pilot_override_warning;
struct {
LevelIssue issue{LevelIssue::NONE};
float maximum;
float current;
} level_problem;
};