Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : * ArduCopter (also known as APM, APM:Copter or just Copter)
18 : * Wiki: copter.ardupilot.club
19 : * Creator: Jason Short
20 : * Lead Developer: Randy Mackay
21 : * Lead Tester: Marco Robustini
22 : * Based on code and ideas from the Arducopter team: Leonard Hall, Andrew Tridgell, Robert Lefebvre, Pat Hickey, Michael Oborne, Jani Hirvinen,
23 : Olivier Adler, Kevin Hester, Arthur Benemann, Jonathan Challinger, John Arne Birkeland,
24 : Jean-Louis Naudin, Mike Smith, and more
25 : * Thanks to: Chris Anderson, Jordi Munoz, Jason Short, Doug Weibel, Jose Julio
26 : *
27 : * Special Thanks to contributors (in alphabetical order by first name):
28 : *
29 : * Adam M Rivera :Auto Compass Declination
30 : * Amilcar Lucas :Camera mount library
31 : * Andrew Tridgell :General development, Mavlink Support
32 : * Andy Piper :Harmonic notch, In-flight FFT, Bi-directional DShot, various drivers
33 : * Angel Fernandez :Alpha testing
34 : * AndreasAntonopoulous:GeoFence
35 : * Arthur Benemann :DroidPlanner GCS
36 : * Benjamin Pelletier :Libraries
37 : * Bill King :Single Copter
38 : * Christof Schmid :Alpha testing
39 : * Craig Elder :Release Management, Support
40 : * Dani Saez :V Octo Support
41 : * Doug Weibel :DCM, Libraries, Control law advice
42 : * Emile Castelnuovo :VRBrain port, bug fixes
43 : * Gregory Fletcher :Camera mount orientation math
44 : * Guntars :Arming safety suggestion
45 : * HappyKillmore :Mavlink GCS
46 : * Hein Hollander :Octo Support, Heli Testing
47 : * Igor van Airde :Control Law optimization
48 : * Jack Dunkle :Alpha testing
49 : * James Goppert :Mavlink Support
50 : * Jani Hiriven :Testing feedback
51 : * Jean-Louis Naudin :Auto Landing
52 : * John Arne Birkeland :PPM Encoder
53 : * Jose Julio :Stabilization Control laws, MPU6k driver
54 : * Julien Dubois :PosHold flight mode
55 : * Julian Oes :Pixhawk
56 : * Jonathan Challinger :Inertial Navigation, CompassMot, Spin-When-Armed
57 : * Kevin Hester :Andropilot GCS
58 : * Max Levine :Tri Support, Graphics
59 : * Leonard Hall :Flight Dynamics, Throttle, Loiter and Navigation Controllers
60 : * Marco Robustini :Lead tester
61 : * Michael Oborne :Mission Planner GCS
62 : * Mike Smith :Pixhawk driver, coding support
63 : * Olivier Adler :PPM Encoder, piezo buzzer
64 : * Pat Hickey :Hardware Abstraction Layer (HAL)
65 : * Robert Lefebvre :Heli Support, Copter LEDs
66 : * Roberto Navoni :Library testing, Porting to VRBrain
67 : * Sandro Benigno :Camera support, MinimOSD
68 : * Sandro Tognana :PosHold flight mode
69 : * Sebastian Quilter :SmartRTL
70 : * ..and many more.
71 : *
72 : * Code commit statistics can be found here: https://github.com/ArduPilot/ardupilot/graphs/contributors
73 : * Wiki: https://copter.ardupilot.club/
74 : *
75 : */
76 :
77 : #include "Copter.h"
78 : #include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
79 :
80 : #define FORCE_VERSION_H_INCLUDE
81 : #include "version.h"
82 : #undef FORCE_VERSION_H_INCLUDE
83 :
84 : const AP_HAL::HAL& hal = AP_HAL::get_HAL();
85 :
86 : #define SCHED_TASK(func, rate_hz, _max_time_micros, _prio) SCHED_TASK_CLASS(Copter, &copter, func, rate_hz, _max_time_micros, _prio)
87 : #define FAST_TASK(func) FAST_TASK_CLASS(Copter, &copter, func)
88 :
89 : /*
90 : scheduler table - all tasks should be listed here.
91 :
92 : All entries in this table must be ordered by priority.
93 :
94 : This table is interleaved with the table in AP_Vehicle to determine
95 : the order in which tasks are run. Convenience methods SCHED_TASK
96 : and SCHED_TASK_CLASS are provided to build entries in this structure:
97 :
98 : SCHED_TASK arguments:
99 : - name of static function to call
100 : - rate (in Hertz) at which the function should be called
101 : - expected time (in MicroSeconds) that the function should take to run
102 : - priority (0 through 255, lower number meaning higher priority)
103 :
104 : SCHED_TASK_CLASS arguments:
105 : - class name of method to be called
106 : - instance on which to call the method
107 : - method to call on that instance
108 : - rate (in Hertz) at which the method should be called
109 : - expected time (in MicroSeconds) that the method should take to run
110 : - priority (0 through 255, lower number meaning higher priority)
111 :
112 : */
113 : const AP_Scheduler::Task Copter::scheduler_tasks[] = {
114 : // update INS immediately to get current gyro data populated
115 : FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),
116 : // run low level rate controllers that only require IMU data
117 : FAST_TASK(run_rate_controller_main),
118 : #if AC_CUSTOMCONTROL_MULTI_ENABLED
119 : FAST_TASK(run_custom_controller),
120 : #endif
121 : #if FRAME_CONFIG == HELI_FRAME
122 : FAST_TASK(heli_update_autorotation),
123 : #endif //HELI_FRAME
124 : // send outputs to the motors library immediately
125 : FAST_TASK(motors_output_main),
126 : // run EKF state estimator (expensive)
127 : FAST_TASK(read_AHRS),
128 : #if FRAME_CONFIG == HELI_FRAME
129 : FAST_TASK(update_heli_control_dynamics),
130 : #endif //HELI_FRAME
131 : // Inertial Nav
132 : FAST_TASK(read_inertia),
133 : // check if ekf has reset target heading or position
134 : FAST_TASK(check_ekf_reset),
135 : // run the attitude controllers
136 : FAST_TASK(update_flight_mode),
137 : // update home from EKF if necessary
138 : FAST_TASK(update_home_from_EKF),
139 : // check if we've landed or crashed
140 : FAST_TASK(update_land_and_crash_detectors),
141 : // surface tracking update
142 : FAST_TASK(update_rangefinder_terrain_offset),
143 : #if HAL_MOUNT_ENABLED
144 : // camera mount's fast update
145 : FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),
146 : #endif
147 : #if HAL_LOGGING_ENABLED
148 : FAST_TASK(Log_Video_Stabilisation),
149 : #endif
150 :
151 : SCHED_TASK(rc_loop, 250, 130, 3),
152 : SCHED_TASK(throttle_loop, 50, 75, 6),
153 : #if AP_FENCE_ENABLED
154 : SCHED_TASK(fence_check, 25, 100, 7),
155 : #endif
156 : SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200, 9),
157 : #if AP_OPTICALFLOW_ENABLED
158 : SCHED_TASK_CLASS(AP_OpticalFlow, &copter.optflow, update, 200, 160, 12),
159 : #endif
160 : SCHED_TASK(update_batt_compass, 10, 120, 15),
161 : SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all, 10, 50, 18),
162 : #if TOY_MODE_ENABLED
163 : SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50, 24),
164 : #endif
165 : SCHED_TASK(auto_disarm_check, 10, 50, 27),
166 : #if AP_COPTER_AHRS_AUTO_TRIM_ENABLED
167 : SCHED_TASK_CLASS(RC_Channels_Copter, &copter.g2.rc_channels, auto_trim_run, 10, 75, 30),
168 : #endif
169 : #if AP_RANGEFINDER_ENABLED
170 : SCHED_TASK(read_rangefinder, 20, 100, 33),
171 : #endif
172 : #if HAL_PROXIMITY_ENABLED
173 : SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 200, 36),
174 : #endif
175 : SCHED_TASK(update_altitude, 10, 100, 42),
176 : SCHED_TASK(run_nav_updates, 50, 100, 45),
177 : SCHED_TASK(update_throttle_hover,100, 90, 48),
178 : #if MODE_SMARTRTL_ENABLED
179 : SCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100, 51),
180 : #endif
181 : #if HAL_SPRAYER_ENABLED
182 : SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90, 54),
183 : #endif
184 : SCHED_TASK(three_hz_loop, 3, 75, 57),
185 : #if AP_SERVORELAYEVENTS_ENABLED
186 : SCHED_TASK_CLASS(AP_ServoRelayEvents, &copter.ServoRelayEvents, update_events, 50, 75, 60),
187 : #endif
188 : #if AC_PRECLAND_ENABLED
189 : SCHED_TASK(update_precland, 400, 50, 69),
190 : #endif
191 : #if FRAME_CONFIG == HELI_FRAME
192 : SCHED_TASK(check_dynamic_flight, 50, 75, 72),
193 : #endif
194 : #if HAL_LOGGING_ENABLED
195 : SCHED_TASK(loop_rate_logging, LOOP_RATE, 50, 75),
196 : #endif
197 : SCHED_TASK(one_hz_loop, 1, 100, 81),
198 : SCHED_TASK(ekf_check, 10, 75, 84),
199 : SCHED_TASK(check_vibration, 10, 50, 87),
200 : SCHED_TASK(gpsglitch_check, 10, 50, 90),
201 : SCHED_TASK(takeoff_check, 50, 50, 91),
202 : #if AP_LANDINGGEAR_ENABLED
203 : SCHED_TASK(landinggear_update, 10, 75, 93),
204 : #endif
205 : SCHED_TASK(standby_update, 100, 75, 96),
206 : SCHED_TASK(lost_vehicle_check, 10, 50, 99),
207 : SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180, 102),
208 : SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550, 105),
209 : #if HAL_MOUNT_ENABLED
210 : SCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75, 108),
211 : #endif
212 : #if AP_CAMERA_ENABLED
213 : SCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75, 111),
214 : #endif
215 : #if HAL_LOGGING_ENABLED
216 : SCHED_TASK(ten_hz_logging_loop, 10, 350, 114),
217 : SCHED_TASK(twentyfive_hz_logging, 25, 110, 117),
218 : SCHED_TASK_CLASS(AP_Logger, &copter.logger, periodic_tasks, 400, 300, 120),
219 : #endif
220 : SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50, 123),
221 :
222 : #if HAL_LOGGING_ENABLED
223 : SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75, 126),
224 : #endif
225 : #if AP_TEMPCALIBRATION_ENABLED
226 : SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100, 135),
227 : #endif
228 : #if HAL_ADSB_ENABLED || AP_ADSB_AVOIDANCE_ENABLED
229 : SCHED_TASK(avoidance_adsb_update, 10, 100, 138),
230 : #endif // HAL_ADSB_ENABLED || AP_ADSB_AVOIDANCE_ENABLED
231 : #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
232 : SCHED_TASK(afs_fs_check, 10, 100, 141),
233 : #endif
234 : #if AP_TERRAIN_AVAILABLE
235 : SCHED_TASK(terrain_update, 10, 100, 144),
236 : #endif
237 : #if AP_WINCH_ENABLED
238 : SCHED_TASK_CLASS(AP_Winch, &copter.g2.winch, update, 50, 50, 150),
239 : #endif
240 : #ifdef USERHOOK_FASTLOOP
241 : SCHED_TASK(userhook_FastLoop, 100, 75, 153),
242 : #endif
243 : #ifdef USERHOOK_50HZLOOP
244 : SCHED_TASK(userhook_50Hz, 50, 75, 156),
245 : #endif
246 : #ifdef USERHOOK_MEDIUMLOOP
247 : SCHED_TASK(userhook_MediumLoop, 10, 75, 159),
248 : #endif
249 : #ifdef USERHOOK_SLOWLOOP
250 : SCHED_TASK(userhook_SlowLoop, 3.3, 75, 162),
251 : #endif
252 : #ifdef USERHOOK_SUPERSLOWLOOP
253 : SCHED_TASK(userhook_SuperSlowLoop, 1, 75, 165),
254 : #endif
255 : #if HAL_BUTTON_ENABLED
256 : SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168),
257 : #endif
258 : #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
259 : // don't delete this, there is an equivalent (virtual) in AP_Vehicle for the non-rate loop case
260 : SCHED_TASK(update_dynamic_notch_at_specified_rate_main, LOOP_RATE, 200, 215),
261 : #endif
262 : };
263 :
264 1001 : void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
265 : uint8_t &task_count,
266 : uint32_t &log_bit)
267 : {
268 1001 : tasks = &scheduler_tasks[0];
269 1001 : task_count = ARRAY_SIZE(scheduler_tasks);
270 1001 : log_bit = MASK_LOG_PM;
271 1001 : }
272 :
273 : constexpr int8_t Copter::_failsafe_priorities[7];
274 :
275 :
276 : #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
277 : #if MODE_GUIDED_ENABLED
278 : // set target location (for use by external control and scripting)
279 0 : bool Copter::set_target_location(const Location& target_loc)
280 : {
281 : // exit if vehicle is not in Guided mode or Auto-Guided mode
282 0 : if (!flightmode->in_guided_mode()) {
283 0 : return false;
284 : }
285 :
286 0 : return mode_guided.set_destination(target_loc);
287 : }
288 :
289 : // start takeoff to given altitude (for use by scripting)
290 1 : bool Copter::start_takeoff(const float alt_m)
291 : {
292 : // exit if vehicle is not in Guided mode or Auto-Guided mode
293 1 : if (!flightmode->in_guided_mode()) {
294 0 : return false;
295 : }
296 :
297 1 : if (mode_guided.do_user_takeoff_start_m(alt_m)) {
298 1 : copter.set_auto_armed(true);
299 1 : return true;
300 : }
301 0 : return false;
302 : }
303 : #endif //MODE_GUIDED_ENABLED
304 : #endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
305 :
306 : #if AP_SCRIPTING_ENABLED
307 : #if MODE_GUIDED_ENABLED
308 : // set target position (for use by scripting)
309 0 : bool Copter::set_target_pos_NED(const Vector3f& target_pos_ned_m, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool is_terrain_alt)
310 : {
311 : // exit if vehicle is not in Guided mode or Auto-Guided mode
312 0 : if (!flightmode->in_guided_mode()) {
313 0 : return false;
314 : }
315 :
316 0 : return mode_guided.set_pos_NED_m(target_pos_ned_m.topostype(), use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), yaw_relative, is_terrain_alt);
317 : }
318 :
319 : // set target position and velocity (for use by scripting)
320 0 : bool Copter::set_target_posvel_NED(const Vector3f& target_pos_ned_m, const Vector3f& target_vel_ned_ms)
321 : {
322 : // exit if vehicle is not in Guided mode or Auto-Guided mode
323 0 : if (!flightmode->in_guided_mode()) {
324 0 : return false;
325 : }
326 :
327 0 : return mode_guided.set_pos_vel_accel_NED_m(target_pos_ned_m.topostype(), target_vel_ned_ms, Vector3f());
328 : }
329 :
330 : // set target position, velocity and acceleration (for use by scripting)
331 0 : bool Copter::set_target_posvelaccel_NED(const Vector3f& target_pos_ned_m, const Vector3f& target_vel_ned_ms, const Vector3f& target_accel_ned_mss, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative)
332 : {
333 : // exit if vehicle is not in Guided mode or Auto-Guided mode
334 0 : if (!flightmode->in_guided_mode()) {
335 0 : return false;
336 : }
337 :
338 0 : return mode_guided.set_pos_vel_accel_NED_m(target_pos_ned_m.topostype(), target_vel_ned_ms, target_accel_ned_mss, use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), yaw_relative);
339 : }
340 :
341 47 : bool Copter::set_target_velocity_NED(const Vector3f& target_vel_ned_ms, bool align_yaw_to_target)
342 : {
343 : // exit if vehicle is not in Guided mode or Auto-Guided mode
344 47 : if (!flightmode->in_guided_mode()) {
345 0 : return false;
346 : }
347 :
348 : // optionally line up the copter with the velocity vector
349 47 : float yaw_rads = 0.0f;
350 47 : if (align_yaw_to_target) {
351 0 : const float speed_sq = target_vel_ned_ms.xy().length_squared();
352 0 : if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED_MS * YAW_LOOK_AHEAD_MIN_SPEED_MS))) {
353 0 : yaw_rads = atan2f(target_vel_ned_ms.y, target_vel_ned_ms.x);
354 : }
355 : }
356 :
357 47 : mode_guided.set_vel_accel_NED_m(target_vel_ned_ms, Vector3f(), align_yaw_to_target, yaw_rads);
358 47 : return true;
359 : }
360 :
361 : // set target velocity and acceleration (for use by scripting)
362 0 : bool Copter::set_target_velaccel_NED(const Vector3f& target_vel_ned_ms, const Vector3f& target_accel_ned_mss, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw)
363 : {
364 : // exit if vehicle is not in Guided mode or Auto-Guided mode
365 0 : if (!flightmode->in_guided_mode()) {
366 0 : return false;
367 : }
368 :
369 0 : mode_guided.set_vel_accel_NED_m(target_vel_ned_ms, target_accel_ned_mss, use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), relative_yaw);
370 0 : return true;
371 : }
372 :
373 : // set target roll pitch and yaw angles with throttle (for use by scripting)
374 0 : bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs)
375 : {
376 : // exit if vehicle is not in Guided mode or Auto-Guided mode
377 0 : if (!flightmode->in_guided_mode()) {
378 0 : return false;
379 : }
380 :
381 0 : Quaternion q;
382 0 : q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
383 :
384 0 : mode_guided.set_angle(q, Vector3f{}, climb_rate_ms, false);
385 0 : return true;
386 : }
387 :
388 : // set target roll pitch and yaw rates with throttle (for use by scripting)
389 245 : bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle)
390 : {
391 : // exit if vehicle is not in Guided mode or Auto-Guided mode
392 245 : if (!flightmode->in_guided_mode()) {
393 0 : return false;
394 : }
395 :
396 : // Zero quaternion indicates rate control
397 245 : Quaternion q;
398 245 : q.zero();
399 :
400 : // Convert from degrees per second to radians per second
401 245 : Vector3f ang_vel_body { roll_rate_dps, pitch_rate_dps, yaw_rate_dps };
402 245 : ang_vel_body *= DEG_TO_RAD;
403 :
404 : // Pass to guided mode
405 245 : mode_guided.set_angle(q, ang_vel_body, throttle, true);
406 245 : return true;
407 : }
408 :
409 : // set target roll pitch and yaw angles and roll pitch and yaw rates with throttle (for use by scripting)
410 0 : bool Copter::set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_degs, float pitch_rate_degs, float yaw_rate_degs, float throttle)
411 : {
412 : // exit if vehicle is not in Guided mode or Auto-Guided mode
413 0 : if (!flightmode->in_guided_mode()) {
414 0 : return false;
415 : }
416 :
417 0 : Quaternion q;
418 0 : q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
419 :
420 : // Convert from degrees per second to radians per second
421 0 : Vector3f ang_vel_body_degs { roll_rate_degs, pitch_rate_degs, yaw_rate_degs };
422 0 : ang_vel_body_degs *= DEG_TO_RAD;
423 :
424 0 : mode_guided.set_angle(q, ang_vel_body_degs, throttle, true);
425 0 : return true;
426 : }
427 :
428 : // Register a custom mode with given number and names
429 1 : AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
430 : {
431 1 : const Mode::Number number = (Mode::Number)num;
432 :
433 : // See if this mode has been registered already, if it has return the state for it
434 : // This allows scripting restarts
435 1 : for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
436 1 : if (mode_guided_custom[i] == nullptr) {
437 1 : break;
438 : }
439 0 : if ((mode_guided_custom[i]->mode_number() == number) &&
440 0 : (strcmp(mode_guided_custom[i]->name(), full_name) == 0) &&
441 0 : (strncmp(mode_guided_custom[i]->name4(), short_name, 4) == 0)) {
442 0 : return &mode_guided_custom[i]->state;
443 : }
444 : }
445 :
446 : // Number already registered to existing mode
447 1 : if (mode_from_mode_num(number) != nullptr) {
448 0 : return nullptr;
449 : }
450 :
451 : // Find free slot
452 1 : for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
453 1 : if (mode_guided_custom[i] == nullptr) {
454 : // Duplicate strings so were not pointing to unknown memory
455 1 : const char* full_name_copy = strdup(full_name);
456 1 : const char* short_name_copy = strndup(short_name, 4);
457 1 : if ((full_name_copy != nullptr) && (short_name_copy != nullptr)) {
458 1 : mode_guided_custom[i] = NEW_NOTHROW ModeGuidedCustom(number, full_name_copy, short_name_copy);
459 : }
460 1 : if (mode_guided_custom[i] == nullptr) {
461 : // Allocation failure
462 0 : free((void*)full_name_copy);
463 0 : free((void*)short_name_copy);
464 0 : return nullptr;
465 : }
466 :
467 : // Registration successful, notify the GCS that it should re-request the available modes
468 1 : gcs().available_modes_changed();
469 :
470 1 : return &mode_guided_custom[i]->state;
471 : }
472 : }
473 :
474 : // No free slots
475 0 : return nullptr;
476 : }
477 : #endif // MODE_GUIDED_ENABLED
478 :
479 : #if MODE_CIRCLE_ENABLED
480 : // circle mode controls
481 412 : bool Copter::get_circle_radius(float &radius_m)
482 : {
483 412 : radius_m = circle_nav->get_radius_m();
484 412 : return true;
485 : }
486 :
487 412 : bool Copter::set_circle_rate(float rate_degs)
488 : {
489 412 : circle_nav->set_rate_degs(rate_degs);
490 412 : return true;
491 : }
492 : #endif
493 :
494 : // set desired speed (m/s). Used for scripting.
495 0 : bool Copter::set_desired_speed(float speed_ms)
496 : {
497 0 : return flightmode->set_speed_NE_ms(speed_ms);
498 : }
499 :
500 : #if MODE_AUTO_ENABLED
501 : // returns true if mode supports NAV_SCRIPT_TIME mission commands
502 0 : bool Copter::nav_scripting_enable(uint8_t mode)
503 : {
504 0 : return mode == (uint8_t)mode_auto.mode_number();
505 : }
506 :
507 : // lua scripts use this to retrieve the contents of the active command
508 0 : bool Copter::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4)
509 : {
510 0 : if (flightmode != &mode_auto) {
511 0 : return false;
512 : }
513 :
514 0 : return mode_auto.nav_script_time(id, cmd, arg1, arg2, arg3, arg4);
515 : }
516 :
517 : // lua scripts use this to indicate when they have complete the command
518 0 : void Copter::nav_script_time_done(uint16_t id)
519 : {
520 0 : if (flightmode != &mode_auto) {
521 0 : return;
522 : }
523 :
524 0 : return mode_auto.nav_script_time_done(id);
525 : }
526 : #endif
527 :
528 : // returns true if the EKF failsafe has triggered. Only used by Lua scripts
529 0 : bool Copter::has_ekf_failsafed() const
530 : {
531 0 : return failsafe.ekf;
532 : }
533 :
534 : // get target location (for use by scripting)
535 0 : bool Copter::get_target_location(Location& target_loc)
536 : {
537 0 : return flightmode->get_wp(target_loc);
538 : }
539 :
540 : /*
541 : update_target_location() acts as a wrapper for set_target_location
542 : */
543 0 : bool Copter::update_target_location(const Location &old_loc, const Location &new_loc)
544 : {
545 : /*
546 : by checking the caller has provided the correct old target
547 : location we prevent a race condition where the user changes mode
548 : or commands a different target in the controlling lua script
549 : */
550 0 : Location next_WP_loc;
551 0 : flightmode->get_wp(next_WP_loc);
552 0 : if (!old_loc.same_loc_as(next_WP_loc) ||
553 0 : old_loc.get_alt_frame() != new_loc.get_alt_frame()) {
554 0 : return false;
555 : }
556 :
557 0 : return set_target_location(new_loc);
558 : }
559 :
560 : #endif // AP_SCRIPTING_ENABLED
561 :
562 : // returns true if vehicle is landing.
563 2705 : bool Copter::is_landing() const
564 : {
565 2705 : return flightmode->is_landing();
566 : }
567 :
568 : // returns true if vehicle is taking off.
569 2705 : bool Copter::is_taking_off() const
570 : {
571 2705 : return flightmode->is_taking_off();
572 : }
573 :
574 478720 : bool Copter::current_mode_requires_mission() const
575 : {
576 : #if MODE_AUTO_ENABLED
577 478720 : return flightmode == &mode_auto;
578 : #else
579 : return false;
580 : #endif
581 : }
582 :
583 : // rc_loops - reads user input from transmitter/receiver
584 : // called at 100hz
585 30412448 : void Copter::rc_loop()
586 : {
587 : // Read radio and 3-position switch on radio
588 : // -----------------------------------------
589 30412448 : read_radio();
590 30412448 : rc().read_mode_switch();
591 30412448 : }
592 :
593 : // throttle_loop - should be run at 50 hz
594 : // ---------------------------
595 3800955 : void Copter::throttle_loop()
596 : {
597 : // update throttle_low_comp value (controls priority of throttle vs attitude control)
598 3800955 : update_throttle_mix();
599 :
600 : // check auto_armed status
601 3800955 : update_auto_armed();
602 :
603 : #if FRAME_CONFIG == HELI_FRAME
604 : // update rotor speed
605 404223 : heli_update_rotor_speed_targets();
606 :
607 : // update trad heli swash plate movement
608 404223 : heli_update_landing_swash();
609 : #endif
610 :
611 : // compensate for ground effect (if enabled)
612 : #if AP_GROUNDEFFECT_ENABLED
613 3800955 : update_ground_effect_detector();
614 : #endif
615 3800955 : update_ekf_terrain_height_stable();
616 3800955 : }
617 :
618 : // update_batt_compass - read battery and compass
619 : // should be called at 10hz
620 759512 : void Copter::update_batt_compass(void)
621 : {
622 : // read battery before compass because it may be used for motor interference compensation
623 759512 : battery.read();
624 :
625 759512 : if(AP::compass().available()) {
626 : // update compass with throttle value - used for compassmot
627 759512 : compass.set_throttle(motors->get_throttle());
628 759512 : compass.set_voltage(battery.voltage());
629 759512 : compass.read();
630 : }
631 759512 : }
632 :
633 : #if HAL_LOGGING_ENABLED
634 : // Full rate logging of attitude, rate and pid loops
635 : // should be run at loop rate
636 30412448 : void Copter::loop_rate_logging()
637 : {
638 30412448 : if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
639 408999 : Log_Write_Attitude();
640 408999 : if (!using_rate_thread) {
641 306864 : Log_Write_Rate();
642 306864 : Log_Write_PIDS(); // only logs if PIDS bitmask is set
643 : }
644 : }
645 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
646 30412448 : if (should_log(MASK_LOG_FTN_FAST) && !using_rate_thread) {
647 0 : AP::ins().write_notch_log_messages();
648 : }
649 : #endif
650 30412448 : if (should_log(MASK_LOG_IMU_FAST)) {
651 0 : AP::ins().Write_IMU();
652 : }
653 :
654 30412448 : motors->Log_Write_SPOL();
655 30412448 : }
656 :
657 : // ten_hz_logging_loop
658 : // should be run at 10hz
659 759507 : void Copter::ten_hz_logging_loop()
660 : {
661 : // always write AHRS attitude at 10Hz
662 759507 : ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG);
663 : // log attitude controller data if we're not already logging at the higher rate
664 759507 : if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
665 718578 : Log_Write_Attitude();
666 718578 : if (!using_rate_thread) {
667 718578 : Log_Write_Rate();
668 : }
669 : }
670 759507 : if (!should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
671 : // log at 10Hz if PIDS bitmask is selected, even if no ATT bitmask is selected; logs at looprate if ATT_FAST and PIDS bitmask set
672 749286 : if (!using_rate_thread) {
673 748962 : Log_Write_PIDS();
674 : }
675 : }
676 : // log EKF attitude data always at 10Hz unless ATTITUDE_FAST, then do it in the 25Hz loop
677 759507 : if (!should_log(MASK_LOG_ATTITUDE_FAST)) {
678 749286 : Log_Write_EKF_POS();
679 : }
680 678719 : if ((FRAME_CONFIG == HELI_FRAME) || should_log(MASK_LOG_MOTBATT)) {
681 : // always write motors log if we are a heli
682 700287 : motors->Log_Write();
683 : }
684 759507 : if (should_log(MASK_LOG_RCIN)) {
685 698661 : logger.Write_RCIN();
686 : #if AP_RSSI_ENABLED
687 698661 : if (rssi.enabled()) {
688 0 : logger.Write_RSSI();
689 : }
690 : #endif
691 : }
692 759507 : if (should_log(MASK_LOG_RCOUT)) {
693 698661 : logger.Write_RCOUT();
694 : }
695 759507 : if (should_log(MASK_LOG_NTUN) && (flightmode->requires_position() || landing_with_GPS() || !flightmode->has_manual_throttle())) {
696 563177 : pos_control->write_log();
697 : }
698 759507 : if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
699 728322 : AP::ins().Write_Vibration();
700 : }
701 759507 : if (should_log(MASK_LOG_CTUN)) {
702 : #if HAL_PROXIMITY_ENABLED
703 728322 : g2.proximity.log(); // Write proximity sensor distances
704 : #endif
705 : #if AP_BEACON_ENABLED
706 728322 : beacon.log();
707 : #endif // AP_BEACON_ENABLED
708 : }
709 : #if AP_WINCH_ENABLED
710 759507 : if (should_log(MASK_LOG_ANY)) {
711 728799 : g2.winch.write_log();
712 : }
713 : #endif
714 : #if HAL_MOUNT_ENABLED
715 759507 : if (should_log(MASK_LOG_CAMERA)) {
716 698661 : camera_mount.write_log();
717 : }
718 : #endif
719 759507 : }
720 :
721 : // twentyfive_hz_logging - should be run at 25hz
722 1900261 : void Copter::twentyfive_hz_logging()
723 : {
724 1900261 : if (should_log(MASK_LOG_ATTITUDE_FAST)) {
725 25562 : Log_Write_EKF_POS();
726 : }
727 :
728 1900261 : if (should_log(MASK_LOG_IMU) && !(should_log(MASK_LOG_IMU_FAST))) {
729 1822111 : AP::ins().Write_IMU();
730 : }
731 :
732 : #if HAL_GYROFFT_ENABLED
733 1900261 : if (should_log(MASK_LOG_FTN_FAST)) {
734 0 : gyro_fft.write_log_messages();
735 : }
736 : #endif
737 1900261 : }
738 : #endif // HAL_LOGGING_ENABLED
739 :
740 : // three_hz_loop - 3hz loop
741 228158 : void Copter::three_hz_loop()
742 : {
743 : // check if we've lost contact with the ground station
744 228158 : failsafe_gcs_check();
745 :
746 : // check if we've lost terrain data
747 228158 : failsafe_terrain_check();
748 :
749 : // check for deadreckoning failsafe
750 228158 : failsafe_deadreckon_check();
751 :
752 : #if AP_RC_TRANSMITTER_TUNING_ENABLED
753 : //update transmitter based in flight tuning
754 228158 : tuning();
755 : #endif // AP_RC_TRANSMITTER_TUNING_ENABLED
756 :
757 : // check if avoidance should be enabled based on alt
758 228158 : low_alt_avoidance();
759 228158 : }
760 :
761 : // ap_value calculates a 32-bit bitmask representing various pieces of
762 : // state about the Copter. It replaces a global variable which was
763 : // used to track this state.
764 72241 : uint32_t Copter::ap_value() const
765 : {
766 72241 : uint32_t ret = 0;
767 :
768 72241 : const bool *b = (const bool *)≈
769 2022748 : for (uint8_t i=0; i<sizeof(ap); i++) {
770 1950507 : if (b[i]) {
771 436945 : ret |= 1U<<i;
772 : }
773 : }
774 :
775 72241 : return ret;
776 : }
777 :
778 : // one_hz_loop - runs at 1Hz
779 75225 : void Copter::one_hz_loop()
780 : {
781 : #if HAL_LOGGING_ENABLED
782 75225 : if (should_log(MASK_LOG_ANY)) {
783 72241 : Log_Write_Data(LogDataID::AP_STATE, ap_value());
784 : }
785 : #endif
786 :
787 75225 : if (!motors->armed()) {
788 25912 : update_using_interlock();
789 :
790 : // check the user hasn't updated the frame class or type
791 25912 : motors->set_frame_class_and_type((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
792 :
793 : #if FRAME_CONFIG != HELI_FRAME
794 : // set all throttle channel settings
795 23652 : motors->update_throttle_range();
796 : #endif
797 : }
798 :
799 : // update assigned functions and enable auxiliary servos
800 75225 : AP::srv().enable_aux_servos();
801 :
802 : #if HAL_LOGGING_ENABLED
803 : // log terrain data
804 75225 : terrain_logging();
805 : #endif
806 :
807 : #if HAL_ADSB_ENABLED
808 75225 : adsb.set_is_flying(!ap.land_complete);
809 : #endif
810 :
811 75225 : AP_Notify::flags.flying = !ap.land_complete;
812 :
813 : // slowly update the PID notches with the average loop rate
814 75225 : if (!using_rate_thread) {
815 74939 : attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
816 : }
817 75225 : pos_control->D_get_accel_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
818 : #if AC_CUSTOMCONTROL_MULTI_ENABLED
819 67166 : custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
820 : #endif
821 :
822 : #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
823 : // see if we should have a separate rate thread
824 75225 : if (!started_rate_thread && get_fast_rate_type() != FastRateType::FAST_RATE_DISABLED) {
825 3 : if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void),
826 : "rate",
827 : 1536, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) {
828 3 : started_rate_thread = true;
829 : } else {
830 0 : AP_BoardConfig::allocation_error("rate thread");
831 : }
832 : }
833 : #endif
834 75225 : }
835 :
836 552 : void Copter::init_simple_bearing()
837 : {
838 : // capture current cos_yaw and sin_yaw values
839 552 : simple_cos_yaw = ahrs.cos_yaw();
840 552 : simple_sin_yaw = ahrs.sin_yaw();
841 :
842 : // initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading
843 552 : super_simple_last_bearing_rad = wrap_2PI(ahrs.get_yaw_rad() + radians(180.0));
844 552 : super_simple_cos_yaw = simple_cos_yaw;
845 552 : super_simple_sin_yaw = simple_sin_yaw;
846 :
847 : #if HAL_LOGGING_ENABLED
848 : // log the simple bearing
849 552 : if (should_log(MASK_LOG_ANY)) {
850 552 : Log_Write_Data(LogDataID::INIT_SIMPLE_BEARING, ahrs.yaw_sensor);
851 : }
852 : #endif
853 552 : }
854 :
855 : // update_super_simple_bearing - adjusts simple bearing based on location
856 : // should be called after home_bearing_rad has been updated
857 3801507 : void Copter::update_super_simple_bearing(bool force_update)
858 : {
859 3801507 : if (!force_update) {
860 3801507 : if (simple_mode != SimpleMode::SUPERSIMPLE) {
861 3801507 : return;
862 : }
863 0 : if (home_distance_m() < SUPER_SIMPLE_RADIUS_M) {
864 0 : return;
865 : }
866 : }
867 :
868 0 : const float bearing_rad = home_bearing_rad();
869 :
870 : // check the bearing to home has changed by at least 5 degrees
871 : // todo: consider updating this continuously
872 0 : if (fabsf(wrap_PI(super_simple_last_bearing_rad - bearing_rad)) < radians(5.0)) {
873 0 : return;
874 : }
875 :
876 0 : super_simple_last_bearing_rad = bearing_rad;
877 0 : const float angle_rad = super_simple_last_bearing_rad + radians(180.0);
878 0 : super_simple_cos_yaw = cosf(angle_rad);
879 0 : super_simple_sin_yaw = sinf(angle_rad);
880 : }
881 :
882 30412448 : void Copter::read_AHRS(void)
883 : {
884 : // we tell AHRS to skip INS update as we have already done it in FAST_TASK.
885 30412448 : ahrs.update(true);
886 30412448 : }
887 :
888 : // read baro and log control tuning
889 759512 : void Copter::update_altitude()
890 : {
891 : // read in baro altitude
892 759512 : read_barometer();
893 :
894 : #if HAL_LOGGING_ENABLED
895 759512 : if (should_log(MASK_LOG_CTUN)) {
896 728325 : Log_Write_Control_Tuning();
897 728325 : if (!should_log(MASK_LOG_FTN_FAST)) {
898 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
899 728325 : AP::ins().write_notch_log_messages();
900 : #endif
901 : #if HAL_GYROFFT_ENABLED
902 728325 : gyro_fft.write_log_messages();
903 : #endif
904 : }
905 : }
906 : #endif
907 759512 : }
908 :
909 : // vehicle specific waypoint info helpers
910 : #if AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
911 6657 : bool Copter::get_wp_location(Location &loc) const
912 : {
913 6657 : return flightmode->get_wp(loc);
914 : }
915 : #endif // AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
916 :
917 75223 : bool Copter::get_wp_distance_m(float &distance) const
918 : {
919 : // see GCS_MAVLINK_Copter::send_nav_controller_output()
920 75223 : distance = flightmode->wp_distance_m();
921 75223 : return true;
922 : }
923 :
924 : // vehicle specific waypoint info helpers
925 75223 : bool Copter::get_wp_bearing_deg(float &bearing) const
926 : {
927 : // see GCS_MAVLINK_Copter::send_nav_controller_output()
928 75223 : bearing = flightmode->wp_bearing_deg();
929 75223 : return true;
930 : }
931 :
932 : // vehicle specific waypoint info helpers
933 75223 : bool Copter::get_wp_crosstrack_error_m(float &xtrack_error) const
934 : {
935 : // see GCS_MAVLINK_Copter::send_nav_controller_output()
936 75223 : xtrack_error = flightmode->crosstrack_error_m() * 0.01;
937 75223 : return true;
938 : }
939 :
940 : // get the target earth-frame angular velocities in rad/s (Z-axis component used by some gimbals)
941 11602 : bool Copter::get_rate_ef_targets(Vector3f& rate_ef_targets_rads) const
942 : {
943 : // always returns zero vector if landed or disarmed
944 11602 : if (copter.ap.land_complete) {
945 4836 : rate_ef_targets_rads.zero();
946 : } else {
947 6766 : rate_ef_targets_rads = attitude_control->get_rate_ef_target_rads();
948 : }
949 11602 : return true;
950 : }
951 :
952 : /*
953 : constructor for main Copter class
954 : */
955 1001 : Copter::Copter(void)
956 : :
957 1001 : pos_variance_filt(FS_EKF_FILT_DEFAULT),
958 1001 : vel_variance_filt(FS_EKF_FILT_DEFAULT),
959 1001 : flightmode(&mode_stabilize),
960 1001 : simple_cos_yaw(1.0f),
961 1001 : super_simple_cos_yaw(1.0),
962 1001 : land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
963 1001 : rc_throttle_control_in_filter(1.0f),
964 3003 : param_loader(var_info)
965 : {
966 1001 : }
967 :
968 : Copter copter;
969 : AP_Vehicle& vehicle = copter;
970 :
971 1001 : AP_HAL_MAIN_CALLBACKS(&copter);
|
