CoCalc -- mode

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/ArduCopter/mode_acro.cpp
Views: ¹⁷⁹⁸
1
#include "Copter.h"
2

3
#include "mode.h"
4

5
#if MODE_ACRO_ENABLED
6

7
/*
8
 * Init and run calls for acro flight mode
9
 */
10
void ModeAcro::run()
11
{
12
    // convert the input to the desired body frame rate
13
    float target_roll, target_pitch, target_yaw;
14
    get_pilot_desired_angle_rates(channel_roll->norm_input_dz(), channel_pitch->norm_input_dz(), channel_yaw->norm_input_dz(), target_roll, target_pitch, target_yaw);
15

16
    if (!motors->armed()) {
17
        // Motors should be Stopped
18
        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
19
    } else if (copter.ap.throttle_zero
20
               || (copter.air_mode == AirMode::AIRMODE_ENABLED && motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN)) {
21
        // throttle_zero is never true in air mode, but the motors should be allowed to go through ground idle
22
        // in order to facilitate the spoolup block
23

24
        // Attempting to Land or motors not yet spinning
25
        // if airmode is enabled only an actual landing will spool down the motors
26
        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
27
    } else {
28
        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
29
    }
30

31
    float pilot_desired_throttle = get_pilot_desired_throttle();
32

33
    switch (motors->get_spool_state()) {
34
    case AP_Motors::SpoolState::SHUT_DOWN:
35
        // Motors Stopped
36
        attitude_control->reset_target_and_rate(true);
37
        attitude_control->reset_rate_controller_I_terms();
38
        pilot_desired_throttle = 0.0f;
39
        break;
40

41
    case AP_Motors::SpoolState::GROUND_IDLE:
42
        // Landed
43
        attitude_control->reset_target_and_rate();
44
        attitude_control->reset_rate_controller_I_terms_smoothly();
45
        pilot_desired_throttle = 0.0f;
46
        break;
47

48
    case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
49
        // clear landing flag above zero throttle
50
        if (!motors->limit.throttle_lower) {
51
            set_land_complete(false);
52
        }
53
        break;
54

55
    case AP_Motors::SpoolState::SPOOLING_UP:
56
    case AP_Motors::SpoolState::SPOOLING_DOWN:
57
        // do nothing
58
        break;
59
    }
60

61
    // run attitude controller
62
    if (g2.acro_options.get() & uint8_t(AcroOptions::RATE_LOOP_ONLY)) {
63
        attitude_control->input_rate_bf_roll_pitch_yaw_2(target_roll, target_pitch, target_yaw);
64
    } else {
65
        attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
66
    }
67

68
    // output pilot's throttle without angle boost
69
    attitude_control->set_throttle_out(pilot_desired_throttle, false, copter.g.throttle_filt);
70
}
71

72
bool ModeAcro::init(bool ignore_checks)
73
{
74
    if (g2.acro_options.get() & uint8_t(AcroOptions::AIR_MODE)) {
75
        disable_air_mode_reset = false;
76
        copter.air_mode = AirMode::AIRMODE_ENABLED;
77
    }
78

79
    return true;
80
}
81

82
void ModeAcro::exit()
83
{
84
    if (!disable_air_mode_reset && (g2.acro_options.get() & uint8_t(AcroOptions::AIR_MODE))) {
85
        copter.air_mode = AirMode::AIRMODE_DISABLED;
86
    }
87
    disable_air_mode_reset = false;
88
}
89

90
void ModeAcro::air_mode_aux_changed()
91
{
92
    disable_air_mode_reset = true;
93
}
94

95
float ModeAcro::throttle_hover() const
96
{
97
    if (g2.acro_thr_mid > 0) {
98
        return g2.acro_thr_mid;
99
    }
100
    return Mode::throttle_hover();
101
}
102

103
// get_pilot_desired_angle_rates - transform pilot's normalised roll pitch and yaw input into a desired lean angle rates
104
// inputs are -1 to 1 and the function returns desired angle rates in centi-degrees-per-second
105
void ModeAcro::get_pilot_desired_angle_rates(float roll_in, float pitch_in, float yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
106
{
107
    float rate_limit;
108
    Vector3f rate_ef_level_cd, rate_bf_level_cd, rate_bf_request_cd;
109

110
    // apply circular limit to pitch and roll inputs
111
    float total_in = norm(pitch_in, roll_in);
112

113
    if (total_in > 1.0) {
114
        float ratio = 1.0 / total_in;
115
        roll_in *= ratio;
116
        pitch_in *= ratio;
117
    }
118

119
    // calculate roll, pitch rate requests
120
    
121
    // roll expo
122
    rate_bf_request_cd.x = g2.command_model_acro_rp.get_rate() * 100.0 * input_expo(roll_in, g2.command_model_acro_rp.get_expo());
123

124
    // pitch expo
125
    rate_bf_request_cd.y = g2.command_model_acro_rp.get_rate() * 100.0 * input_expo(pitch_in, g2.command_model_acro_rp.get_expo());
126

127
    // yaw expo
128
    rate_bf_request_cd.z = g2.command_model_acro_y.get_rate() * 100.0 * input_expo(yaw_in, g2.command_model_acro_y.get_expo());
129

130
    // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
131

132
    if (g.acro_trainer != (uint8_t)Trainer::OFF) {
133

134
        // get attitude targets
135
        const Vector3f att_target = attitude_control->get_att_target_euler_cd();
136

137
        // Calculate trainer mode earth frame rate command for roll
138
        int32_t roll_angle = wrap_180_cd(att_target.x);
139
        rate_ef_level_cd.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
140

141
        // Calculate trainer mode earth frame rate command for pitch
142
        int32_t pitch_angle = wrap_180_cd(att_target.y);
143
        rate_ef_level_cd.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
144

145
        // Calculate trainer mode earth frame rate command for yaw
146
        rate_ef_level_cd.z = 0;
147

148
        // Calculate angle limiting earth frame rate commands
149
        if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
150
            const float angle_max = copter.aparm.angle_max;
151
            if (roll_angle > angle_max){
152
                rate_ef_level_cd.x += sqrt_controller(angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
153
            }else if (roll_angle < -angle_max) {
154
                rate_ef_level_cd.x += sqrt_controller(-angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
155
            }
156

157
            if (pitch_angle > angle_max){
158
                rate_ef_level_cd.y += sqrt_controller(angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
159
            }else if (pitch_angle < -angle_max) {
160
                rate_ef_level_cd.y += sqrt_controller(-angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
161
            }
162
        }
163

164
        // convert earth-frame level rates to body-frame level rates
165
        attitude_control->euler_rate_to_ang_vel(attitude_control->get_attitude_target_quat(), rate_ef_level_cd, rate_bf_level_cd);
166

167
        // combine earth frame rate corrections with rate requests
168
        if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
169
            rate_bf_request_cd.x += rate_bf_level_cd.x;
170
            rate_bf_request_cd.y += rate_bf_level_cd.y;
171
            rate_bf_request_cd.z += rate_bf_level_cd.z;
172
        }else{
173
            float acro_level_mix = constrain_float(1-float(MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0), 0, 1)*ahrs.cos_pitch();
174

175
            // Scale levelling rates by stick input
176
            rate_bf_level_cd = rate_bf_level_cd * acro_level_mix;
177

178
            // Calculate rate limit to prevent change of rate through inverted
179
            rate_limit = fabsf(fabsf(rate_bf_request_cd.x)-fabsf(rate_bf_level_cd.x));
180
            rate_bf_request_cd.x += rate_bf_level_cd.x;
181
            rate_bf_request_cd.x = constrain_float(rate_bf_request_cd.x, -rate_limit, rate_limit);
182

183
            // Calculate rate limit to prevent change of rate through inverted
184
            rate_limit = fabsf(fabsf(rate_bf_request_cd.y)-fabsf(rate_bf_level_cd.y));
185
            rate_bf_request_cd.y += rate_bf_level_cd.y;
186
            rate_bf_request_cd.y = constrain_float(rate_bf_request_cd.y, -rate_limit, rate_limit);
187

188
            // Calculate rate limit to prevent change of rate through inverted
189
            rate_limit = fabsf(fabsf(rate_bf_request_cd.z)-fabsf(rate_bf_level_cd.z));
190
            rate_bf_request_cd.z += rate_bf_level_cd.z;
191
            rate_bf_request_cd.z = constrain_float(rate_bf_request_cd.z, -rate_limit, rate_limit);
192
        }
193
    }
194

195
    // hand back rate request
196
    roll_out = rate_bf_request_cd.x;
197
    pitch_out = rate_bf_request_cd.y;
198
    yaw_out = rate_bf_request_cd.z;
199
}
200
#endif
201

202
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.
