ardupilot/libraries/APM_Control/AP_AutoTune.h

137 lines
3.1 KiB
C
Raw Normal View History

#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_Logger/LogStructure.h>
#include <AP_Param/AP_Param.h>
#include <AP_Vehicle/AP_Vehicle.h>
2021-02-27 05:56:41 -04:00
#include <AC_PID/AC_PID.h>
class AP_AutoTune {
public:
struct ATGains {
AP_Float tau;
AP_Int16 rmax_pos;
AP_Int16 rmax_neg;
float FF, P, I, D, IMAX;
};
enum ATType {
AUTOTUNE_ROLL = 0,
AUTOTUNE_PITCH = 1
};
struct PACKED log_ATRP {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t type;
uint8_t state;
float actuator;
float desired_rate;
float actual_rate;
float FF_single;
float FF;
float P;
float I;
float D;
uint8_t action;
float rmax;
float tau;
};
// constructor
2021-02-27 05:56:41 -04:00
AP_AutoTune(ATGains &_gains, ATType type, const AP_Vehicle::FixedWing &parms, AC_PID &rpid);
// called when autotune mode is entered
void start(void);
// called to stop autotune and restore gains when user leaves
// autotune
void stop(void);
// update called whenever autotune mode is active. This is
// called at the main loop rate
void update(AP_Logger::PID_Info &pid_info, float scaler, float angle_err_deg);
// are we running?
bool running;
private:
// the current gains
ATGains &current;
2021-02-27 05:56:41 -04:00
AC_PID &rpid;
// what type of autotune is this
ATType type;
const AP_Vehicle::FixedWing &aparm;
// values to restore if we leave autotune mode
ATGains restore;
// values we last saved
ATGains last_save;
// values to save on the next save event
ATGains next_save;
// time when we last saved
uint32_t last_save_ms;
2021-04-12 05:48:36 -03:00
// last logging time
uint32_t last_log_ms;
// the demanded/achieved state
enum class ATState {IDLE,
DEMAND_POS,
DEMAND_NEG};
ATState state;
// the demanded/achieved state
enum class Action {NONE,
LOW_RATE,
SHORT,
RAISE_PD,
LOWER_PD,
IDLE_LOWER_PD};
Action action;
// when we entered the current state
uint32_t state_enter_ms;
void check_save(void);
void check_state_exit(uint32_t state_time_ms);
void save_gains(const ATGains &v);
void save_float_if_changed(AP_Float &v, float value);
void save_int16_if_changed(AP_Int16 &v, int16_t value);
void state_change(ATState newstate);
// get gains with PID components
ATGains get_gains(const ATGains &current);
void set_gains(const ATGains &v);
// update rmax and tau towards target
void update_rmax();
// 5 point mode filter for FF estimate
ModeFilterFloat_Size5 ff_filter;
LowPassFilterFloat actuator_filter;
LowPassFilterFloat rate_filter;
LowPassFilterFloat target_filter;
float max_actuator;
float min_actuator;
float max_rate;
float min_rate;
float max_target;
float min_target;
float max_P;
float max_D;
float min_Dmod;
float max_Dmod;
float max_SRate;
2021-04-01 22:54:34 -03:00
float FF_single;
};