mirror of https://github.com/ArduPilot/ardupilot
106 lines
2.5 KiB
C++
106 lines
2.5 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#ifndef __AP_AUTOTUNE_H__
|
|
#define __AP_AUTOTUNE_H__
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <DataFlash/DataFlash.h>
|
|
|
|
class AP_AutoTune {
|
|
public:
|
|
struct ATGains {
|
|
AP_Float tau;
|
|
AP_Float P;
|
|
AP_Float I;
|
|
AP_Float D;
|
|
AP_Float FF;
|
|
AP_Int16 rmax;
|
|
AP_Int16 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;
|
|
int16_t servo;
|
|
float demanded;
|
|
float achieved;
|
|
float P;
|
|
};
|
|
|
|
|
|
// constructor
|
|
AP_AutoTune(ATGains &_gains, ATType type, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash);
|
|
|
|
// 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
|
|
// typically at 50Hz
|
|
void update(float desired_rate, float achieved_rate, float servo_out);
|
|
|
|
// are we running?
|
|
bool running:1;
|
|
|
|
private:
|
|
// the current gains
|
|
ATGains ¤t;
|
|
|
|
// what type of autotune is this
|
|
ATType type;
|
|
|
|
const AP_Vehicle::FixedWing &aparm;
|
|
|
|
DataFlash_Class &dataflash;
|
|
|
|
// did we saturate surfaces?
|
|
bool saturated_surfaces:1;
|
|
|
|
// 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 = 0;
|
|
|
|
// the demanded/achieved state
|
|
enum ATState {DEMAND_UNSATURATED,
|
|
DEMAND_UNDER_POS,
|
|
DEMAND_OVER_POS,
|
|
DEMAND_UNDER_NEG,
|
|
DEMAND_OVER_NEG} state = DEMAND_UNSATURATED;
|
|
|
|
// when we entered the current state
|
|
uint32_t state_enter_ms = 0;
|
|
|
|
void check_save(void);
|
|
void check_state_exit(uint32_t state_time_ms);
|
|
void save_gains(const ATGains &v);
|
|
|
|
void write_log_headers(void);
|
|
void write_log(float servo, float demanded, float achieved);
|
|
|
|
void log_param_change(float v, const prog_char_t *suffix);
|
|
void save_float_if_changed(AP_Float &v, float value, const prog_char_t *suffix);
|
|
void save_int16_if_changed(AP_Int16 &v, int16_t value, const prog_char_t *suffix);
|
|
};
|
|
|
|
#endif // __AP_AUTOTUNE_H__
|
|
|