Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 21 additions & 21 deletions libraries/AC_AutoTune/AC_AutoTune.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,13 @@

#define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
# define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level (Plane uses more relaxed 5deg)
# define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch (Plane uses more relaxed 10deg/sec)
# define AUTOTUNE_LEVEL_ANGLE_RAD radians(5.0) // angle which qualifies as level (Plane uses more relaxed 5deg)
# define AUTOTUNE_LEVEL_RATE_RP_RADS radians(10.0) // rate which qualifies as level for roll and pitch (Plane uses more relaxed 10deg/sec)
#else
# define AUTOTUNE_LEVEL_ANGLE_CD 250 // angle which qualifies as level
# define AUTOTUNE_LEVEL_RATE_RP_CD 500 // rate which qualifies as level for roll and pitch
# define AUTOTUNE_LEVEL_ANGLE_RAD radians(2.5) // angle which qualifies as level
# define AUTOTUNE_LEVEL_RATE_RP_RADS radians(5.0) // rate which qualifies as level for roll and pitch
#endif
#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw
#define AUTOTUNE_LEVEL_RATE_Y_RADS radians(7.5) // rate which qualifies as level for yaw
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 250 // time we require the aircraft to be level before starting next test
#define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level
#define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users
Expand Down Expand Up @@ -342,22 +342,22 @@ bool AC_AutoTune::currently_level()
// relax threshold if we pass AUTOTUNE_LEVEL_TIMEOUT_MS
const float threshold_mul = constrain_float((float)(now_ms - level_start_time_ms) / (float)AUTOTUNE_LEVEL_TIMEOUT_MS, 0.0, 2.0);

if (fabsf(ahrs_view->get_roll_rad() - desired_roll_rad) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_ANGLE_CD)) {
if (fabsf(ahrs_view->get_roll_rad() - desired_roll_rad) > threshold_mul * AUTOTUNE_LEVEL_ANGLE_RAD) {
return false;
}
if (fabsf(ahrs_view->get_pitch_rad() - desired_pitch_rad) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_ANGLE_CD)) {
if (fabsf(ahrs_view->get_pitch_rad() - desired_pitch_rad) > threshold_mul * AUTOTUNE_LEVEL_ANGLE_RAD) {
return false;
}
if (fabsf(wrap_PI(ahrs_view->get_yaw_rad() - desired_yaw_rad)) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_ANGLE_CD)) {
if (fabsf(wrap_PI(ahrs_view->get_yaw_rad() - desired_yaw_rad)) > threshold_mul * AUTOTUNE_LEVEL_ANGLE_RAD) {
return false;
}
if (fabsf(ahrs_view->get_gyro().x) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_RATE_RP_CD)) {
if (fabsf(ahrs_view->get_gyro().x) > threshold_mul * AUTOTUNE_LEVEL_RATE_RP_RADS) {
return false;
}
if (fabsf(ahrs_view->get_gyro().y) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_RATE_RP_CD)) {
if (fabsf(ahrs_view->get_gyro().y) > threshold_mul * AUTOTUNE_LEVEL_RATE_RP_RADS) {
return false;
}
if (fabsf(ahrs_view->get_gyro().z) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_RATE_Y_CD)) {
if (fabsf(ahrs_view->get_gyro().z) > threshold_mul * AUTOTUNE_LEVEL_RATE_Y_RADS) {
return false;
}
return true;
Expand All @@ -367,8 +367,8 @@ bool AC_AutoTune::currently_level()
// Updates attitude controller targets and processes test results to adjust PID gains.
void AC_AutoTune::control_attitude()
{
rotation_rate = 0.0f;
lean_angle = 0.0f;
rotation_rate_rads = 0.0f;
lean_angle_rad = 0.0f;
const float direction_sign = positive_direction ? 1.0f : -1.0f;
const uint32_t now_ms = AP_HAL::millis();

Expand All @@ -394,17 +394,17 @@ void AC_AutoTune::control_attitude()
// Record starting angular position and rate
switch (axis) {
case AxisType::ROLL:
start_rate = degrees(ahrs_view->get_gyro().x) * 100.0f;
start_angle = ahrs_view->roll_sensor;
start_rate_rads = ahrs_view->get_gyro().x;
start_angle_rad = ahrs_view->get_roll_rad();
break;
case AxisType::PITCH:
start_rate = degrees(ahrs_view->get_gyro().y) * 100.0f;
start_angle = ahrs_view->pitch_sensor;
start_rate_rads = ahrs_view->get_gyro().y;
start_angle_rad = ahrs_view->get_pitch_rad();
break;
case AxisType::YAW:
case AxisType::YAW_D:
start_rate = degrees(ahrs_view->get_gyro().z) * 100.0f;
start_angle = ahrs_view->yaw_sensor;
start_rate_rads = ahrs_view->get_gyro().z;
start_angle_rad = ahrs_view->get_yaw_rad();
break;
}

Expand All @@ -421,8 +421,8 @@ void AC_AutoTune::control_attitude()
test_run(axis, direction_sign);

// Detect failure due to reverse response or excessive lean angle
if (lean_angle <= -angle_lim_neg_rpy_cd() ||
attitude_control->lean_angle_deg() * 100 > angle_lim_max_rp_cd()) {
if (lean_angle_rad <= -angle_lim_neg_rpy_rad() ||
attitude_control->lean_angle_rad() > angle_lim_max_rp_rad()) {
step = Step::ABORT;
}

Expand Down
22 changes: 11 additions & 11 deletions libraries/AC_AutoTune/AC_AutoTune.h
Original file line number Diff line number Diff line change
Expand Up @@ -276,9 +276,9 @@ class AC_AutoTune
uint32_t step_timeout_ms; // time limit of current autotune process
uint32_t level_start_time_ms; // start time of waiting for level
int8_t success_counter; // counter for tuning gains
float start_angle; // start angle
float start_rate; // start rate - parent and multi
float test_accel_max_cdss; // maximum acceleration variable
float start_angle_rad; // start angle in radians
float start_rate_rads; // start rate in radians/second - parent and multi
float test_accel_max_radss; // maximum acceleration variable in radians/second/second
float step_scaler; // scaler to reduce maximum target step - parent and multi

LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second
Expand All @@ -296,8 +296,8 @@ class AC_AutoTune
float tune_roll_rff, tune_pitch_rff, tune_yaw_rd, tune_yaw_rff;

uint32_t last_announce_ms;
float lean_angle;
float rotation_rate;
float lean_angle_rad;
float rotation_rate_rads;
float desired_roll_rad, desired_pitch_rad, desired_yaw_rad; // desired attitude target setpoints and test origins

// heli specific variables
Expand All @@ -309,22 +309,22 @@ class AC_AutoTune
virtual bool position_ok();

// methods subclasses must implement to specify max/min test angles:
virtual float target_angle_max_rp_cd() const = 0;
virtual float target_angle_max_rp_rad() const = 0;

// methods subclasses must implement to specify max/min test angles:
virtual float target_angle_max_y_cd() const = 0;
virtual float target_angle_max_y_rad() const = 0;

// methods subclasses must implement to specify max/min test angles:
virtual float target_angle_min_rp_cd() const = 0;
virtual float target_angle_min_rp_rad() const = 0;

// methods subclasses must implement to specify max/min test angles:
virtual float target_angle_min_y_cd() const = 0;
virtual float target_angle_min_y_rad() const = 0;

// methods subclasses must implement to specify max/min test angles:
virtual float angle_lim_max_rp_cd() const = 0;
virtual float angle_lim_max_rp_rad() const = 0;

// methods subclasses must implement to specify max/min test angles:
virtual float angle_lim_neg_rpy_cd() const = 0;
virtual float angle_lim_neg_rpy_rad() const = 0;

// initialise position controller
bool init_position_controller();
Expand Down
66 changes: 33 additions & 33 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,12 +61,12 @@
#define AUTOTUNE_SEQ_BITMASK_TUNE_CHECK 16

// angle limits preserved from previous behaviour as Multi changed:
#define AUTOTUNE_ANGLE_TARGET_MAX_RP_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_TARGET_MIN_RP_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_TARGET_MAX_Y_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_TARGET_MIN_Y_CD 500 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_MAX_RP_CD 3000 // maximum allowable angle in degrees during testing
#define AUTOTUNE_ANGLE_NEG_RPY_CD 1000 // maximum allowable angle in degrees during testing
#define AUTOTUNE_ANGLE_TARGET_MAX_RP_RAD radians(20.0) // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_TARGET_MIN_RP_RAD radians(10.0) // minimum target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_TARGET_MAX_Y_RAD radians(30.0) // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_TARGET_MIN_Y_RAD radians(5.0) // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_MAX_RP_RAD radians(30.0) // maximum allowable angle during testing
#define AUTOTUNE_ANGLE_NEG_RPY_RAD radians(10.0) // maximum allowable angle against the test direction

const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = {

Expand Down Expand Up @@ -174,7 +174,7 @@ void AC_AutoTune_Heli::test_init()
} else {
start_freq = next_test_freq;
stop_freq = start_freq;
test_accel_max_cdss = 0.0f;
test_accel_max_radss = 0.0f;
}
filter_freq = start_freq;

Expand Down Expand Up @@ -225,7 +225,7 @@ void AC_AutoTune_Heli::test_init()
} else {
start_freq = next_test_freq;
stop_freq = start_freq;
test_accel_max_cdss = 0.0f;
test_accel_max_radss = 0.0f;
}
filter_freq = start_freq;
attitude_control->bf_feedforward(false);
Expand All @@ -241,7 +241,7 @@ void AC_AutoTune_Heli::test_init()
// initialize start frequency
start_freq = min_sweep_freq;
stop_freq = max_sweep_freq;
test_accel_max_cdss = 0.0f;
test_accel_max_radss = 0.0f;
filter_freq = start_freq;

// variables needed to initialize frequency response object and test method
Expand Down Expand Up @@ -382,7 +382,7 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() {
} else if (tune_type == TuneType::RATE_FF_UP) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_ff=%f", (double)(tune_rff));
} else if (tune_type == TuneType::ANGLE_P_UP) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(degrees(tune_accel_radss)), (double)(test_accel_max_cdss * 0.01f));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(degrees(tune_accel_radss)), (double)(degrees(test_accel_max_radss)));
}
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep_tgt.maxgain.freq), (double)(sweep_tgt.maxgain.gain));
Expand Down Expand Up @@ -771,7 +771,7 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float am
rotation_rate_filt.reset(0);
command_filt.reset(0);
target_rate_filt.reset(0);
rotation_rate = 0.0f;
rotation_rate_rads = 0.0f;
command_out = 0.0f;
filt_target_rate = 0.0f;

Expand Down Expand Up @@ -904,7 +904,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
}

// looks at gain and phase of input rate to output rate
rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading.get()),
rotation_rate_rads = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading.get()),
AP::scheduler().get_loop_period_s());
filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading.get()),
AP::scheduler().get_loop_period_s());
Expand All @@ -917,8 +917,8 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
float dwell_phase_tgt = 0.0f;
// wait for dwell to start before determining gain and phase
if ((float)(now_ms - dwell_start_time_ms) > pre_calc_cycles * cycle_time_ms || (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP && settle_time == 0)) {
freqresp_mtr.update(command_out,command_out,rotation_rate, dwell_freq);
freqresp_tgt.update(command_out,filt_target_rate,rotation_rate, dwell_freq);
freqresp_mtr.update(command_out,command_out,rotation_rate_rads, dwell_freq);
freqresp_tgt.update(command_out,filt_target_rate,rotation_rate_rads, dwell_freq);

if (freqresp_mtr.is_cycle_complete()) {
if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
Expand Down Expand Up @@ -950,7 +950,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
curr_test_tgt.freq = freqresp_tgt.get_freq();
curr_test_tgt.gain = freqresp_tgt.get_gain();
curr_test_tgt.phase = freqresp_tgt.get_phase();
if (test_calc_type == DRB) {test_accel_max_cdss = freqresp_tgt.get_accel_max();}
if (test_calc_type == DRB) {test_accel_max_radss = cd_to_rad(freqresp_tgt.get_accel_max());}
}
// reset cycle_complete to allow indication of next cycle
freqresp_tgt.reset_cycle_complete();
Expand All @@ -961,7 +961,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
} else {
dwell_gain_tgt = freqresp_tgt.get_gain();
dwell_phase_tgt = freqresp_tgt.get_phase();
if (test_calc_type == DRB) {test_accel_max_cdss = freqresp_tgt.get_accel_max();}
if (test_calc_type == DRB) {test_accel_max_radss = cd_to_rad(freqresp_tgt.get_accel_max());}
cycle_complete_tgt = true;
}
}
Expand Down Expand Up @@ -1453,34 +1453,34 @@ void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_fre

}

float AC_AutoTune_Heli::target_angle_max_rp_cd() const
float AC_AutoTune_Heli::target_angle_max_rp_rad() const
{
return AUTOTUNE_ANGLE_TARGET_MAX_RP_CD;
return AUTOTUNE_ANGLE_TARGET_MAX_RP_RAD;
}

float AC_AutoTune_Heli::target_angle_max_y_cd() const
float AC_AutoTune_Heli::target_angle_max_y_rad() const
{
return AUTOTUNE_ANGLE_TARGET_MAX_Y_CD;
return AUTOTUNE_ANGLE_TARGET_MAX_Y_RAD;
}

float AC_AutoTune_Heli::target_angle_min_rp_cd() const
float AC_AutoTune_Heli::target_angle_min_rp_rad() const
{
return AUTOTUNE_ANGLE_TARGET_MIN_RP_CD;
return AUTOTUNE_ANGLE_TARGET_MIN_RP_RAD;
}

float AC_AutoTune_Heli::target_angle_min_y_cd() const
float AC_AutoTune_Heli::target_angle_min_y_rad() const
{
return AUTOTUNE_ANGLE_TARGET_MIN_Y_CD;
return AUTOTUNE_ANGLE_TARGET_MIN_Y_RAD;
}

float AC_AutoTune_Heli::angle_lim_max_rp_cd() const
float AC_AutoTune_Heli::angle_lim_max_rp_rad() const
{
return AUTOTUNE_ANGLE_MAX_RP_CD;
return AUTOTUNE_ANGLE_MAX_RP_RAD;
}

float AC_AutoTune_Heli::angle_lim_neg_rpy_cd() const
float AC_AutoTune_Heli::angle_lim_neg_rpy_rad() const
{
return AUTOTUNE_ANGLE_NEG_RPY_CD;
return AUTOTUNE_ANGLE_NEG_RPY_RAD;
}

// freq_search_for_phase: general search strategy for specified phase. interpolation done once specified phase has been bounded.
Expand Down Expand Up @@ -1530,14 +1530,14 @@ void AC_AutoTune_Heli::Log_AutoTune()

switch (axis) {
case AxisType::ROLL:
Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max_cdss);
Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, rad_to_cd(test_accel_max_radss));
break;
case AxisType::PITCH:
Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max_cdss);
Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, rad_to_cd(test_accel_max_radss));
break;
case AxisType::YAW:
case AxisType::YAW_D:
Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_cdss);
Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, rad_to_cd(test_accel_max_radss));
break;
}
}
Expand All @@ -1546,9 +1546,9 @@ void AC_AutoTune_Heli::Log_AutoTune()
void AC_AutoTune_Heli::Log_AutoTuneDetails()
{
if (tune_type == TuneType::ANGLE_P_UP || tune_type == TuneType::TUNE_CHECK) {
Log_Write_AutoTuneDetails(command_out, 0.0f, 0.0f, filt_target_rate, rotation_rate);
Log_Write_AutoTuneDetails(command_out, 0.0f, 0.0f, filt_target_rate, rotation_rate_rads);
} else {
Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate, 0.0f, 0.0f);
Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate_rads, 0.0f, 0.0f);
}
}

Expand Down
12 changes: 6 additions & 6 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.h
Original file line number Diff line number Diff line change
Expand Up @@ -163,17 +163,17 @@ class AC_AutoTune_Heli : public AC_AutoTune
TARGET = 1,
};

float target_angle_max_rp_cd() const override;
float target_angle_max_rp_rad() const override;

float target_angle_max_y_cd() const override;
float target_angle_max_y_rad() const override;

float target_angle_min_rp_cd() const override;
float target_angle_min_rp_rad() const override;

float target_angle_min_y_cd() const override;
float target_angle_min_y_rad() const override;

float angle_lim_max_rp_cd() const override;
float angle_lim_max_rp_rad() const override;

float angle_lim_neg_rpy_cd() const override;
float angle_lim_neg_rpy_rad() const override;

// initialize dwell test or angle dwell test variables
void dwell_test_init(float start_frq, float stop_frq, float amplitude, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type);
Expand Down
Loading
Loading