CoCalc -- AC_AttitudeControl

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
Views: ¹⁷⁹⁸

1
#pragma once
2

3
/// @file    AC_AttitudeControl_Multi.h
4
/// @brief   ArduCopter attitude control library
5

6
#include "AC_AttitudeControl.h"
7
#include <AP_Motors/AP_MotorsMulticopter.h>
8

9
// default rate controller PID gains
10
#ifndef AC_ATC_MULTI_RATE_RP_P
11
  # define AC_ATC_MULTI_RATE_RP_P           0.135f
12
#endif
13
#ifndef AC_ATC_MULTI_RATE_RP_I
14
  # define AC_ATC_MULTI_RATE_RP_I           0.135f
15
#endif
16
#ifndef AC_ATC_MULTI_RATE_RP_D
17
  # define AC_ATC_MULTI_RATE_RP_D           0.0036f
18
#endif
19
#ifndef AC_ATC_MULTI_RATE_RP_IMAX
20
 # define AC_ATC_MULTI_RATE_RP_IMAX         0.5f
21
#endif
22
#ifndef AC_ATC_MULTI_RATE_RPY_FILT_HZ
23
 # define AC_ATC_MULTI_RATE_RPY_FILT_HZ      20.0f
24
#endif
25
#ifndef AC_ATC_MULTI_RATE_YAW_P
26
 # define AC_ATC_MULTI_RATE_YAW_P           0.180f
27
#endif
28
#ifndef AC_ATC_MULTI_RATE_YAW_I
29
 # define AC_ATC_MULTI_RATE_YAW_I           0.018f
30
#endif
31
#ifndef AC_ATC_MULTI_RATE_YAW_D
32
 # define AC_ATC_MULTI_RATE_YAW_D           0.0f
33
#endif
34
#ifndef AC_ATC_MULTI_RATE_YAW_IMAX
35
 # define AC_ATC_MULTI_RATE_YAW_IMAX        0.5f
36
#endif
37
#ifndef AC_ATC_MULTI_RATE_YAW_FILT_HZ
38
 # define AC_ATC_MULTI_RATE_YAW_FILT_HZ     2.5f
39
#endif
40

41

42
class AC_AttitudeControl_Multi : public AC_AttitudeControl {
43
public:
44
	AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors);
45

46
	// empty destructor to suppress compiler warning
47
	virtual ~AC_AttitudeControl_Multi() {}
48

49
    // pid accessors
50
    AC_PID& get_rate_roll_pid() override { return _pid_rate_roll; }
51
    AC_PID& get_rate_pitch_pid() override { return _pid_rate_pitch; }
52
    AC_PID& get_rate_yaw_pid() override { return _pid_rate_yaw; }
53
    const AC_PID& get_rate_roll_pid() const override { return _pid_rate_roll; }
54
    const AC_PID& get_rate_pitch_pid() const override { return _pid_rate_pitch; }
55
    const AC_PID& get_rate_yaw_pid() const override { return _pid_rate_yaw; }
56

57
    // Update Alt_Hold angle maximum
58
    void update_althold_lean_angle_max(float throttle_in) override;
59

60
    // Set output throttle
61
    void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;
62

63
    // calculate total body frame throttle required to produce the given earth frame throttle
64
    float get_throttle_boosted(float throttle_in);
65

66
    // set desired throttle vs attitude mixing (actual mix is slewed towards this value over 1~2 seconds)
67
    //  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
68
    //  has no effect when throttle is above hover throttle
69
    void set_throttle_mix_min() override { _throttle_rpy_mix_desired = _thr_mix_min; }
70
    void set_throttle_mix_man() override { _throttle_rpy_mix_desired = _thr_mix_man; }
71
    void set_throttle_mix_max(float ratio) override;
72
    void set_throttle_mix_value(float value) override { _throttle_rpy_mix_desired = _throttle_rpy_mix = value; }
73
    float get_throttle_mix(void) const override { return _throttle_rpy_mix; }
74

75
    // are we producing min throttle?
76
    bool is_throttle_mix_min() const override { return (_throttle_rpy_mix < 1.25f * _thr_mix_min); }
77

78
    // run lowest level body-frame rate controller and send outputs to the motors
79
    void rate_controller_run_dt(const Vector3f& gyro, float dt) override;
80
    void rate_controller_target_reset() override;
81
    void rate_controller_run() override;
82

83
    // sanity check parameters.  should be called once before take-off
84
    void parameter_sanity_check() override;
85

86
    // set the PID notch sample rates
87
    void set_notch_sample_rate(float sample_rate) override;
88

89
    // user settable parameters
90
    static const struct AP_Param::GroupInfo var_info[];
91

92
protected:
93

94
    // boost angle_p/pd each cycle on high throttle slew
95
    void update_throttle_gain_boost();
96

97
    // update_throttle_rpy_mix - updates thr_low_comp value towards the target
98
    void update_throttle_rpy_mix();
99

100
    // get maximum value throttle can be raised to based on throttle vs attitude prioritisation
101
    float get_throttle_avg_max(float throttle_in);
102

103
    AP_MotorsMulticopter& _motors_multi;
104
    AC_PID                _pid_rate_roll {
105
        AC_PID::Defaults{
106
            .p         = AC_ATC_MULTI_RATE_RP_P,
107
            .i         = AC_ATC_MULTI_RATE_RP_I,
108
            .d         = AC_ATC_MULTI_RATE_RP_D,
109
            .ff        = 0.0f,
110
            .imax      = AC_ATC_MULTI_RATE_RP_IMAX,
111
            .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
112
            .filt_E_hz = 0.0f,
113
            .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
114
            .srmax     = 0,
115
            .srtau     = 1.0
116
        }
117
    };
118
    AC_PID                _pid_rate_pitch{
119
        AC_PID::Defaults{
120
            .p         = AC_ATC_MULTI_RATE_RP_P,
121
            .i         = AC_ATC_MULTI_RATE_RP_I,
122
            .d         = AC_ATC_MULTI_RATE_RP_D,
123
            .ff        = 0.0f,
124
            .imax      = AC_ATC_MULTI_RATE_RP_IMAX,
125
            .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
126
            .filt_E_hz = 0.0f,
127
            .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
128
            .srmax     = 0,
129
            .srtau     = 1.0
130
        }
131
    };
132

133
    AC_PID                _pid_rate_yaw{
134
        AC_PID::Defaults{
135
            .p         = AC_ATC_MULTI_RATE_YAW_P,
136
            .i         = AC_ATC_MULTI_RATE_YAW_I,
137
            .d         = AC_ATC_MULTI_RATE_YAW_D,
138
            .ff        = 0.0f,
139
            .imax      = AC_ATC_MULTI_RATE_YAW_IMAX,
140
            .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
141
            .filt_E_hz = AC_ATC_MULTI_RATE_YAW_FILT_HZ,
142
            .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
143
            .srmax     = 0,
144
            .srtau     = 1.0
145
        }
146
    };
147

148
    AP_Float              _thr_mix_man;     // throttle vs attitude control prioritisation used when using manual throttle (higher values mean we prioritise attitude control over throttle)
149
    AP_Float              _thr_mix_min;     // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
150
    AP_Float              _thr_mix_max;     // throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
151

152
    // angle_p/pd boost multiplier
153
    AP_Float              _throttle_gain_boost;
154
};
155

156

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.