Copter: FlightMode - convert AUTOTUNE flight mode
This commit is contained in:
parent
018c70d224
commit
67063d6b1e
@ -709,8 +709,6 @@ private:
|
||||
void send_pid_tuning(mavlink_channel_t chan);
|
||||
void gcs_data_stream_send(void);
|
||||
void gcs_check_input(void);
|
||||
void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
|
||||
void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
|
||||
void Log_Write_Current();
|
||||
void Log_Write_Optflow();
|
||||
void Log_Write_Nav_Tuning();
|
||||
@ -776,37 +774,7 @@ private:
|
||||
void set_auto_yaw_rate(float turn_rate_cds);
|
||||
float get_auto_heading(void);
|
||||
float get_auto_yaw_rate_cds();
|
||||
bool autotune_init(bool ignore_checks);
|
||||
void autotune_stop();
|
||||
bool autotune_start(bool ignore_checks);
|
||||
void autotune_run();
|
||||
bool autotune_currently_level();
|
||||
void autotune_attitude_control();
|
||||
void autotune_backup_gains_and_initialise();
|
||||
void autotune_load_orig_gains();
|
||||
void autotune_load_tuned_gains();
|
||||
void autotune_load_intra_test_gains();
|
||||
void autotune_load_twitch_gains();
|
||||
void autotune_save_tuning_gains();
|
||||
void autotune_update_gcs(uint8_t message_id);
|
||||
bool autotune_roll_enabled();
|
||||
bool autotune_pitch_enabled();
|
||||
bool autotune_yaw_enabled();
|
||||
void autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max);
|
||||
void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
|
||||
void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
|
||||
void autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max);
|
||||
void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max);
|
||||
void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
|
||||
void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max);
|
||||
void autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
|
||||
void avoidance_adsb_update(void);
|
||||
void autotune_send_step_string();
|
||||
const char *autotune_level_issue_string() const;
|
||||
const char * autotune_type_string() const;
|
||||
void autotune_announce_state_to_gcs();
|
||||
void autotune_do_gcs_announcements();
|
||||
bool autotune_check_level(const enum AUTOTUNE_LEVEL_ISSUE issue, const float current, const float maximum) const;
|
||||
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
void afs_fs_check(void);
|
||||
@ -1055,6 +1023,10 @@ private:
|
||||
|
||||
Copter::FlightMode_AUTO flightmode_auto{*this, mission, circle_nav};
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
Copter::FlightMode_AUTOTUNE flightmode_autotune{*this};
|
||||
#endif
|
||||
|
||||
Copter::FlightMode_CIRCLE flightmode_circle{*this, circle_nav};
|
||||
|
||||
Copter::FlightMode_DRIFT flightmode_drift{*this};
|
||||
|
@ -663,3 +663,69 @@ private:
|
||||
Vector3f flip_orig_attitude; // original vehicle attitude before flip
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
class FlightMode_AUTOTUNE : public FlightMode {
|
||||
|
||||
public:
|
||||
|
||||
FlightMode_AUTOTUNE(Copter &copter) :
|
||||
Copter::FlightMode(copter)
|
||||
{ }
|
||||
|
||||
bool init(bool ignore_checks) override;
|
||||
void run() override; // should be called at 100hz or more
|
||||
|
||||
bool requires_GPS() const override { return false; }
|
||||
bool has_manual_throttle() const override { return false; }
|
||||
bool allows_arming(bool from_gcs) const override { return false; };
|
||||
bool is_autopilot() const override { return false; }
|
||||
|
||||
float get_autotune_descent_speed();
|
||||
bool autotuneing_with_GPS();
|
||||
void do_not_use_GPS();
|
||||
|
||||
void autotune_stop();
|
||||
void autotune_save_tuning_gains();
|
||||
|
||||
protected:
|
||||
|
||||
const char *name() const override { return "AUTOTUNE"; }
|
||||
const char *name4() const override { return "ATUN"; }
|
||||
|
||||
private:
|
||||
|
||||
bool autotune_start(bool ignore_checks);
|
||||
void autotune_attitude_control();
|
||||
void autotune_backup_gains_and_initialise();
|
||||
void autotune_load_orig_gains();
|
||||
void autotune_load_tuned_gains();
|
||||
void autotune_load_intra_test_gains();
|
||||
void autotune_load_twitch_gains();
|
||||
void autotune_update_gcs(uint8_t message_id);
|
||||
bool autotune_roll_enabled();
|
||||
bool autotune_pitch_enabled();
|
||||
bool autotune_yaw_enabled();
|
||||
void autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max);
|
||||
void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
|
||||
void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
|
||||
void autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max);
|
||||
void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max);
|
||||
void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
|
||||
void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max);
|
||||
void autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
|
||||
|
||||
void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
|
||||
void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
|
||||
|
||||
void autotune_send_step_string();
|
||||
const char *autotune_level_issue_string() const;
|
||||
const char * autotune_type_string() const;
|
||||
void autotune_announce_state_to_gcs();
|
||||
void autotune_do_gcs_announcements();
|
||||
bool autotune_check_level(const enum AUTOTUNE_LEVEL_ISSUE issue, const float current, const float maximum) const;
|
||||
bool autotune_currently_level();
|
||||
};
|
||||
#endif
|
||||
|
@ -21,7 +21,7 @@ struct PACKED log_AutoTune {
|
||||
};
|
||||
|
||||
// Write an Autotune data packet
|
||||
void Copter::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt)
|
||||
void Copter::FlightMode_AUTOTUNE::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt)
|
||||
{
|
||||
struct log_AutoTune pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG),
|
||||
@ -36,7 +36,7 @@ void Copter::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_targ
|
||||
new_gain_sp : new_gain_sp,
|
||||
new_ddt : new_ddt
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
_copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_AutoTuneDetails {
|
||||
@ -47,7 +47,7 @@ struct PACKED log_AutoTuneDetails {
|
||||
};
|
||||
|
||||
// Write an Autotune data packet
|
||||
void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
|
||||
void Copter::FlightMode_AUTOTUNE::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
|
||||
{
|
||||
struct log_AutoTuneDetails pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG),
|
||||
@ -55,7 +55,7 @@ void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
|
||||
angle_cd : angle_cd,
|
||||
rate_cds : rate_cds
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
_copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -175,7 +175,7 @@ static struct {
|
||||
} autotune_level_problem;
|
||||
|
||||
// autotune_init - should be called when autotune mode is selected
|
||||
bool Copter::autotune_init(bool ignore_checks)
|
||||
bool Copter::FlightMode_AUTOTUNE::init(bool ignore_checks)
|
||||
{
|
||||
bool success = true;
|
||||
|
||||
@ -220,14 +220,14 @@ bool Copter::autotune_init(bool ignore_checks)
|
||||
}
|
||||
|
||||
// only do position hold if starting autotune from LOITER or POSHOLD
|
||||
autotune_state.use_poshold = (control_mode == LOITER || control_mode == POSHOLD);
|
||||
autotune_state.use_poshold = (_copter.control_mode == LOITER || _copter.control_mode == POSHOLD);
|
||||
autotune_state.have_position = false;
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
// autotune_stop - should be called when the ch7/ch8 switch is switched OFF
|
||||
void Copter::autotune_stop()
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_stop()
|
||||
{
|
||||
// set gains to their original values
|
||||
autotune_load_orig_gains();
|
||||
@ -244,11 +244,11 @@ void Copter::autotune_stop()
|
||||
}
|
||||
|
||||
// autotune_start - Initialize autotune flight mode
|
||||
bool Copter::autotune_start(bool ignore_checks)
|
||||
bool Copter::FlightMode_AUTOTUNE::autotune_start(bool ignore_checks)
|
||||
{
|
||||
// only allow flip from Stabilize, AltHold, PosHold or Loiter modes
|
||||
if (control_mode != STABILIZE && control_mode != ALT_HOLD &&
|
||||
control_mode != LOITER && control_mode != POSHOLD) {
|
||||
if (_copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD &&
|
||||
_copter.control_mode != LOITER && _copter.control_mode != POSHOLD) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -275,7 +275,7 @@ bool Copter::autotune_start(bool ignore_checks)
|
||||
return true;
|
||||
}
|
||||
|
||||
const char *Copter::autotune_level_issue_string() const
|
||||
const char *Copter::FlightMode_AUTOTUNE::autotune_level_issue_string() const
|
||||
{
|
||||
switch (autotune_level_problem.issue) {
|
||||
case Copter::AUTOTUNE_LEVEL_ISSUE_NONE:
|
||||
@ -296,7 +296,7 @@ const char *Copter::autotune_level_issue_string() const
|
||||
return "Bug";
|
||||
}
|
||||
|
||||
void Copter::autotune_send_step_string()
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_send_step_string()
|
||||
{
|
||||
if (autotune_state.pilot_override) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Paused: Pilot Override Active");
|
||||
@ -316,7 +316,7 @@ void Copter::autotune_send_step_string()
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: unknown step");
|
||||
}
|
||||
|
||||
const char *Copter::autotune_type_string() const
|
||||
const char *Copter::FlightMode_AUTOTUNE::autotune_type_string() const
|
||||
{
|
||||
switch (autotune_state.tune_type) {
|
||||
case AUTOTUNE_TYPE_RD_UP:
|
||||
@ -333,7 +333,7 @@ const char *Copter::autotune_type_string() const
|
||||
return "Bug";
|
||||
}
|
||||
|
||||
void Copter::autotune_do_gcs_announcements()
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_do_gcs_announcements()
|
||||
{
|
||||
const uint32_t now = millis();
|
||||
if (now - autotune_announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) {
|
||||
@ -394,7 +394,7 @@ void Copter::autotune_do_gcs_announcements()
|
||||
|
||||
// autotune_run - runs the autotune flight mode
|
||||
// should be called at 100hz or more
|
||||
void Copter::autotune_run()
|
||||
void Copter::FlightMode_AUTOTUNE::run()
|
||||
{
|
||||
float target_roll, target_pitch;
|
||||
float target_yaw_rate;
|
||||
@ -420,7 +420,7 @@ void Copter::autotune_run()
|
||||
update_simple_mode();
|
||||
|
||||
// get pilot desired lean angles
|
||||
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
|
||||
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
|
||||
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||
@ -501,7 +501,7 @@ void Copter::autotune_run()
|
||||
}
|
||||
}
|
||||
|
||||
bool Copter::autotune_check_level(const Copter::AUTOTUNE_LEVEL_ISSUE issue, const float current, const float maximum) const
|
||||
bool Copter::FlightMode_AUTOTUNE::autotune_check_level(const Copter::AUTOTUNE_LEVEL_ISSUE issue, const float current, const float maximum) const
|
||||
{
|
||||
if (current > maximum) {
|
||||
autotune_level_problem.current = current;
|
||||
@ -512,7 +512,7 @@ bool Copter::autotune_check_level(const Copter::AUTOTUNE_LEVEL_ISSUE issue, cons
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Copter::autotune_currently_level()
|
||||
bool Copter::FlightMode_AUTOTUNE::autotune_currently_level()
|
||||
{
|
||||
if (!autotune_check_level(Copter::AUTOTUNE_LEVEL_ISSUE_ANGLE_ROLL,
|
||||
labs(ahrs.roll_sensor - autotune_roll_cd),
|
||||
@ -549,7 +549,7 @@ bool Copter::autotune_currently_level()
|
||||
}
|
||||
|
||||
// autotune_attitude_controller - sets attitude control targets during tuning
|
||||
void Copter::autotune_attitude_control()
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_attitude_control()
|
||||
{
|
||||
rotation_rate = 0.0f; // rotation rate in radians/second
|
||||
lean_angle = 0.0f;
|
||||
@ -733,7 +733,7 @@ void Copter::autotune_attitude_control()
|
||||
|
||||
// log this iterations lean angle and rotation rate
|
||||
Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
|
||||
DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
|
||||
_copter.DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
|
||||
break;
|
||||
|
||||
case AUTOTUNE_STEP_UPDATE_GAINS:
|
||||
@ -953,7 +953,7 @@ void Copter::autotune_attitude_control()
|
||||
|
||||
// autotune_backup_gains_and_initialise - store current gains as originals
|
||||
// called before tuning starts to backup original gains
|
||||
void Copter::autotune_backup_gains_and_initialise()
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_backup_gains_and_initialise()
|
||||
{
|
||||
// initialise state because this is our first time
|
||||
if (autotune_roll_enabled()) {
|
||||
@ -1011,7 +1011,7 @@ void Copter::autotune_backup_gains_and_initialise()
|
||||
|
||||
// autotune_load_orig_gains - set gains to their original values
|
||||
// called by autotune_stop and autotune_failed functions
|
||||
void Copter::autotune_load_orig_gains()
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_load_orig_gains()
|
||||
{
|
||||
attitude_control->bf_feedforward(orig_bf_feedforward);
|
||||
if (autotune_roll_enabled()) {
|
||||
@ -1045,7 +1045,7 @@ void Copter::autotune_load_orig_gains()
|
||||
}
|
||||
|
||||
// autotune_load_tuned_gains - load tuned gains
|
||||
void Copter::autotune_load_tuned_gains()
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_load_tuned_gains()
|
||||
{
|
||||
if (!attitude_control->get_bf_feedforward()) {
|
||||
attitude_control->bf_feedforward(true);
|
||||
@ -1084,7 +1084,7 @@ void Copter::autotune_load_tuned_gains()
|
||||
|
||||
// autotune_load_intra_test_gains - gains used between tests
|
||||
// called during testing mode's update-gains step to set gains ahead of return-to-level step
|
||||
void Copter::autotune_load_intra_test_gains()
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_load_intra_test_gains()
|
||||
{
|
||||
// we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
|
||||
// sanity check the gains
|
||||
@ -1112,7 +1112,7 @@ void Copter::autotune_load_intra_test_gains()
|
||||
|
||||
// autotune_load_twitch_gains - load the to-be-tested gains for a single axis
|
||||
// called by autotune_attitude_control() just before it beings testing a gain (i.e. just before it twitches)
|
||||
void Copter::autotune_load_twitch_gains()
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_load_twitch_gains()
|
||||
{
|
||||
switch (autotune_state.axis) {
|
||||
case AUTOTUNE_AXIS_ROLL:
|
||||
@ -1139,7 +1139,7 @@ void Copter::autotune_load_twitch_gains()
|
||||
|
||||
// autotune_save_tuning_gains - save the final tuned gains for each axis
|
||||
// save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position)
|
||||
void Copter::autotune_save_tuning_gains()
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_save_tuning_gains()
|
||||
{
|
||||
// if we successfully completed tuning
|
||||
if (autotune_state.mode == AUTOTUNE_MODE_SUCCESS) {
|
||||
@ -1227,7 +1227,7 @@ void Copter::autotune_save_tuning_gains()
|
||||
}
|
||||
|
||||
// autotune_update_gcs - send message to ground station
|
||||
void Copter::autotune_update_gcs(uint8_t message_id)
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_update_gcs(uint8_t message_id)
|
||||
{
|
||||
switch (message_id) {
|
||||
case AUTOTUNE_MESSAGE_STARTED:
|
||||
@ -1249,21 +1249,21 @@ void Copter::autotune_update_gcs(uint8_t message_id)
|
||||
}
|
||||
|
||||
// axis helper functions
|
||||
inline bool Copter::autotune_roll_enabled() {
|
||||
inline bool Copter::FlightMode_AUTOTUNE::autotune_roll_enabled() {
|
||||
return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL;
|
||||
}
|
||||
|
||||
inline bool Copter::autotune_pitch_enabled() {
|
||||
inline bool Copter::FlightMode_AUTOTUNE::autotune_pitch_enabled() {
|
||||
return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_PITCH;
|
||||
}
|
||||
|
||||
inline bool Copter::autotune_yaw_enabled() {
|
||||
inline bool Copter::FlightMode_AUTOTUNE::autotune_yaw_enabled() {
|
||||
return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW;
|
||||
}
|
||||
|
||||
// autotune_twitching_test - twitching tests
|
||||
// update min and max and test for end conditions
|
||||
void Copter::autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max)
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max)
|
||||
{
|
||||
// capture maximum measurement
|
||||
if (measurement > measurement_max) {
|
||||
@ -1303,7 +1303,7 @@ void Copter::autotune_twitching_test(float measurement, float target, float &mea
|
||||
|
||||
// autotune_updating_d_up - increase D and adjust P to optimize the D term for a little bounce back
|
||||
// optimize D term while keeping the maximum just below the target by adjusting P
|
||||
void Copter::autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
|
||||
{
|
||||
if (measurement_max > target) {
|
||||
// if maximum measurement was higher than target
|
||||
@ -1358,7 +1358,7 @@ void Copter::autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_
|
||||
|
||||
// autotune_updating_d_down - decrease D and adjust P to optimize the D term for no bounce back
|
||||
// optimize D term while keeping the maximum just below the target by adjusting P
|
||||
void Copter::autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
|
||||
{
|
||||
if (measurement_max > target) {
|
||||
// if maximum measurement was higher than target
|
||||
@ -1413,7 +1413,7 @@ void Copter::autotune_updating_d_down(float &tune_d, float tune_d_min, float tun
|
||||
|
||||
// autotune_updating_p_down - decrease P until we don't reach the target before time out
|
||||
// P is decreased to ensure we are not overshooting the target
|
||||
void Copter::autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max)
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max)
|
||||
{
|
||||
if (measurement_max < target*(1+0.5f*g.autotune_aggressiveness)) {
|
||||
if (autotune_state.ignore_next == false) {
|
||||
@ -1442,7 +1442,7 @@ void Copter::autotune_updating_p_down(float &tune_p, float tune_p_min, float tun
|
||||
|
||||
// autotune_updating_p_up - increase P to ensure the target is reached
|
||||
// P is increased until we achieve our target within a reasonable time
|
||||
void Copter::autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max)
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max)
|
||||
{
|
||||
if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) {
|
||||
// ignore the next result unless it is the same as this one
|
||||
@ -1471,7 +1471,7 @@ void Copter::autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_
|
||||
|
||||
// autotune_updating_p_up - increase P to ensure the target is reached while checking bounce back isn't increasing
|
||||
// P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold
|
||||
void Copter::autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
|
||||
{
|
||||
if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) {
|
||||
// ignore the next result unless it is the same as this one
|
||||
@ -1520,7 +1520,7 @@ void Copter::autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, floa
|
||||
}
|
||||
|
||||
// autotune_twitching_measure_acceleration - measure rate of change of measurement
|
||||
void Copter::autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max)
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max)
|
||||
{
|
||||
if (rate_measurement_max < rate_measurement) {
|
||||
rate_measurement_max = rate_measurement;
|
||||
@ -1529,7 +1529,7 @@ void Copter::autotune_twitching_measure_acceleration(float &rate_of_change, floa
|
||||
}
|
||||
|
||||
// get attitude for slow position hold in autotune mode
|
||||
void Copter::autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd)
|
||||
void Copter::FlightMode_AUTOTUNE::autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd)
|
||||
{
|
||||
roll_cd = pitch_cd = 0;
|
||||
|
||||
@ -1539,7 +1539,7 @@ void Copter::autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, floa
|
||||
}
|
||||
|
||||
// do we know where we are?
|
||||
if (!position_ok()) {
|
||||
if (!_copter.position_ok()) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -125,7 +125,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
case AUTOTUNE:
|
||||
success = autotune_init(ignore_checks);
|
||||
success = flightmode_autotune.init(ignore_checks);
|
||||
if (success) {
|
||||
flightmode = &flightmode_autotune;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
@ -221,12 +224,6 @@ void Copter::update_flight_mode()
|
||||
|
||||
switch (control_mode) {
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
case AUTOTUNE:
|
||||
autotune_run();
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if POSHOLD_ENABLED == ENABLED
|
||||
case POSHOLD:
|
||||
poshold_run();
|
||||
@ -264,7 +261,7 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
|
||||
{
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
if (old_control_mode == AUTOTUNE) {
|
||||
autotune_stop();
|
||||
flightmode_autotune.autotune_stop();
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -381,9 +378,6 @@ void Copter::notify_flight_mode()
|
||||
|
||||
// set flight mode string
|
||||
switch (control_mode) {
|
||||
case AUTOTUNE:
|
||||
notify.set_flight_mode_str("ATUN");
|
||||
break;
|
||||
case POSHOLD:
|
||||
notify.set_flight_mode_str("PHLD");
|
||||
break;
|
||||
|
@ -246,7 +246,7 @@ void Copter::init_disarm_motors()
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
// save auto tuned parameters
|
||||
autotune_save_tuning_gains();
|
||||
flightmode_autotune.autotune_save_tuning_gains();
|
||||
#endif
|
||||
|
||||
// we are not in the air
|
||||
|
Loading…
Reference in New Issue
Block a user