Line data Source code
1 : #include "Sub.h"
2 :
3 : /*
4 : constructor for Mode object
5 : */
6 913 : Mode::Mode(void) :
7 913 : g(sub.g),
8 913 : g2(sub.g2),
9 913 : ahrs(sub.ahrs),
10 913 : motors(sub.motors),
11 913 : channel_roll(sub.channel_roll),
12 913 : channel_pitch(sub.channel_pitch),
13 913 : channel_throttle(sub.channel_throttle),
14 913 : channel_yaw(sub.channel_yaw),
15 913 : channel_forward(sub.channel_forward),
16 913 : channel_lateral(sub.channel_lateral),
17 913 : position_control(&sub.pos_control),
18 913 : attitude_control(&sub.attitude_control),
19 913 : G_Dt(sub.G_Dt)
20 913 : { };
21 :
22 : // return the static controller object corresponding to supplied mode
23 291 : Mode *Sub::mode_from_mode_num(const Mode::Number mode)
24 : {
25 291 : Mode *ret = nullptr;
26 :
27 291 : switch (mode) {
28 107 : case Mode::Number::MANUAL:
29 107 : ret = &mode_manual;
30 107 : break;
31 7 : case Mode::Number::STABILIZE:
32 7 : ret = &mode_stabilize;
33 7 : break;
34 1 : case Mode::Number::ACRO:
35 1 : ret = &mode_acro;
36 1 : break;
37 12 : case Mode::Number::ALT_HOLD:
38 12 : ret = &mode_althold;
39 12 : break;
40 4 : case Mode::Number::SURFTRAK:
41 4 : ret = &mode_surftrak;
42 4 : break;
43 22 : case Mode::Number::POSHOLD:
44 22 : ret = &mode_poshold;
45 22 : break;
46 55 : case Mode::Number::AUTO:
47 55 : ret = &mode_auto;
48 55 : break;
49 22 : case Mode::Number::GUIDED:
50 22 : ret = &mode_guided;
51 22 : break;
52 55 : case Mode::Number::CIRCLE:
53 55 : ret = &mode_circle;
54 55 : break;
55 6 : case Mode::Number::SURFACE:
56 6 : ret = &mode_surface;
57 6 : break;
58 0 : case Mode::Number::MOTOR_DETECT:
59 0 : ret = &mode_motordetect;
60 0 : break;
61 0 : default:
62 0 : break;
63 : }
64 :
65 291 : return ret;
66 : }
67 :
68 :
69 : // set_mode - change flight mode and perform any necessary initialisation
70 : // optional force parameter used to force the flight mode change (used only first time mode is set)
71 : // returns true if mode was successfully set
72 : // Some modes can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
73 337 : bool Sub::set_mode(Mode::Number mode, ModeReason reason)
74 : {
75 :
76 : // return immediately if we are already in the desired mode
77 337 : if (mode == control_mode) {
78 46 : control_mode_reason = reason;
79 46 : return true;
80 : }
81 :
82 291 : Mode *new_flightmode = mode_from_mode_num((Mode::Number)mode);
83 291 : if (new_flightmode == nullptr) {
84 0 : notify_no_such_mode((uint8_t)mode);
85 0 : return false;
86 : }
87 :
88 445 : if (new_flightmode->requires_GPS() &&
89 154 : !sub.position_ok()) {
90 125 : gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name());
91 125 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
92 125 : return false;
93 : }
94 :
95 : // check for valid altitude if old mode did not require it but new one does
96 : // we only want to stop changing modes if it could make things worse
97 293 : if (!flightmode->requires_altitude() &&
98 293 : new_flightmode->requires_altitude() &&
99 33 : !sub.control_check_barometer()) { // maybe use ekf_alt_ok() instead?
100 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name());
101 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
102 0 : return false;
103 : }
104 :
105 166 : if (!new_flightmode->init(false)) {
106 0 : gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name());
107 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
108 0 : return false;
109 : }
110 :
111 : // perform any cleanup required by previous flight mode
112 166 : exit_mode(flightmode, new_flightmode);
113 :
114 : // store previous flight mode
115 166 : prev_control_mode = control_mode;
116 :
117 : // update flight mode
118 166 : flightmode = new_flightmode;
119 166 : control_mode = mode;
120 166 : control_mode_reason = reason;
121 : #if HAL_LOGGING_ENABLED
122 166 : logger.Write_Mode((uint8_t)control_mode, reason);
123 : #endif
124 166 : gcs().send_message(MSG_HEARTBEAT);
125 :
126 : // update notify object
127 166 : notify_flight_mode();
128 :
129 : // return success
130 166 : return true;
131 : }
132 :
133 : // exit_mode - high level call to organise cleanup as a flight mode is exited
134 0 : void Sub::exit_mode(Mode::Number old_control_mode, Mode::Number new_control_mode)
135 : {
136 : // stop mission when we leave auto mode
137 0 : if (old_control_mode == Mode::Number::AUTO) {
138 0 : if (mission.state() == AP_Mission::MISSION_RUNNING) {
139 0 : mission.stop();
140 : }
141 : #if HAL_MOUNT_ENABLED
142 0 : camera_mount.set_mode_to_default();
143 : #endif // HAL_MOUNT_ENABLED
144 : }
145 0 : motors.set_max_throttle(1.0f);
146 0 : }
147 :
148 241 : bool Sub::set_mode(const uint8_t new_mode, const ModeReason reason)
149 : {
150 : static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
151 241 : return sub.set_mode(static_cast<Mode::Number>(new_mode), reason);
152 : }
153 :
154 : // update_flight_mode - calls the appropriate attitude controllers based on flight mode
155 : // called at 100hz or more
156 2180683 : void Sub::update_flight_mode()
157 : {
158 2180683 : flightmode->run();
159 2180683 : }
160 :
161 : // exit_mode - high level call to organise cleanup as a flight mode is exited
162 166 : void Sub::exit_mode(Mode *&old_flightmode, Mode *&new_flightmode){
163 : #if HAL_MOUNT_ENABLED
164 166 : camera_mount.set_mode_to_default();
165 : #endif // HAL_MOUNT_ENABLED
166 166 : motors.set_max_throttle(1.0f);
167 166 : }
168 :
169 : // notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device
170 166 : void Sub::notify_flight_mode()
171 : {
172 166 : AP_Notify::flags.autopilot_mode = flightmode->is_autopilot();
173 166 : AP_Notify::flags.flight_mode = (uint8_t)control_mode;
174 166 : notify.set_flight_mode_str(flightmode->name4());
175 166 : }
176 :
177 :
178 : // get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
179 : // returns desired angle rates in centi-degrees-per-second
180 360 : void Mode::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
181 : {
182 : float rate_limit;
183 360 : Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
184 :
185 : // apply circular limit to pitch and roll inputs
186 360 : float total_in = norm(pitch_in, roll_in);
187 :
188 360 : if (total_in > ROLL_PITCH_INPUT_MAX) {
189 0 : float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in;
190 0 : roll_in *= ratio;
191 0 : pitch_in *= ratio;
192 : }
193 :
194 : // calculate roll, pitch rate requests
195 360 : if (g.acro_expo <= 0) {
196 0 : rate_bf_request.x = roll_in * g.acro_rp_p;
197 0 : rate_bf_request.y = pitch_in * g.acro_rp_p;
198 : } else {
199 : // expo variables
200 : float rp_in, rp_in3, rp_out;
201 :
202 : // range check expo
203 360 : if (g.acro_expo > 1.0f) {
204 0 : g.acro_expo.set(1.0f);
205 : }
206 :
207 : // roll expo
208 360 : rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
209 360 : rp_in3 = rp_in*rp_in*rp_in;
210 360 : rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
211 360 : rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
212 :
213 : // pitch expo
214 360 : rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX;
215 360 : rp_in3 = rp_in*rp_in*rp_in;
216 360 : rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
217 360 : rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
218 : }
219 :
220 : // calculate yaw rate request
221 360 : rate_bf_request.z = yaw_in * g.acro_yaw_p;
222 :
223 : // calculate earth frame rate corrections to pull the vehicle back to level while in ACRO mode
224 :
225 360 : if (g.acro_trainer != ACRO_TRAINER_DISABLED) {
226 : // Calculate trainer mode earth frame rate command for roll
227 360 : int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
228 360 : rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
229 :
230 : // Calculate trainer mode earth frame rate command for pitch
231 360 : int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
232 360 : rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
233 :
234 : // Calculate trainer mode earth frame rate command for yaw
235 360 : rate_ef_level.z = 0;
236 :
237 : // Calculate angle limiting earth frame rate commands
238 360 : const float angle_max_cd = attitude_control->lean_angle_max_cd();
239 360 : if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
240 360 : if (roll_angle > angle_max_cd) {
241 0 : rate_ef_level.x -= g.acro_balance_roll*(roll_angle-angle_max_cd);
242 360 : } else if (roll_angle < -angle_max_cd) {
243 0 : rate_ef_level.x -= g.acro_balance_roll*(roll_angle+angle_max_cd);
244 : }
245 :
246 360 : if (pitch_angle > angle_max_cd) {
247 0 : rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-angle_max_cd);
248 360 : } else if (pitch_angle < -angle_max_cd) {
249 0 : rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+angle_max_cd);
250 : }
251 : }
252 :
253 : // convert earth-frame level rates to body-frame level rates
254 360 : attitude_control->euler_derivative_to_body(attitude_control->get_attitude_target_quat(), rate_ef_level, rate_bf_level);
255 :
256 : // combine earth frame rate corrections with rate requests
257 360 : if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
258 360 : rate_bf_request.x += rate_bf_level.x;
259 360 : rate_bf_request.y += rate_bf_level.y;
260 360 : rate_bf_request.z += rate_bf_level.z;
261 : } else {
262 0 : float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();
263 :
264 : // Scale leveling rates by stick input
265 0 : rate_bf_level = rate_bf_level*acro_level_mix;
266 :
267 : // Calculate rate limit to prevent change of rate through inverted
268 0 : rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x));
269 0 : rate_bf_request.x += rate_bf_level.x;
270 0 : rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit);
271 :
272 : // Calculate rate limit to prevent change of rate through inverted
273 0 : rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y));
274 0 : rate_bf_request.y += rate_bf_level.y;
275 0 : rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit);
276 :
277 : // Calculate rate limit to prevent change of rate through inverted
278 0 : rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z));
279 0 : rate_bf_request.z += rate_bf_level.z;
280 0 : rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit);
281 : }
282 : }
283 :
284 : // hand back rate request
285 360 : roll_out = rate_bf_request.x;
286 360 : pitch_out = rate_bf_request.y;
287 360 : yaw_out = rate_bf_request.z;
288 360 : }
289 :
290 :
291 2 : bool Mode::set_mode(Mode::Number mode, ModeReason reason)
292 : {
293 2 : return sub.set_mode(mode, reason);
294 : }
295 :
296 4 : GCS_Sub &Mode::gcs()
297 : {
298 4 : return sub.gcs();
299 : }
|
