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 : #pragma once
16 : /*
17 : This is the main Sub class
18 : */
19 :
20 : ////////////////////////////////////////////////////////////////////////////////
21 : // Header includes
22 : ////////////////////////////////////////////////////////////////////////////////
23 :
24 : #include <cmath>
25 : #include <stdio.h>
26 : #include <stdarg.h>
27 :
28 : #include <AP_HAL/AP_HAL.h>
29 :
30 : // Common dependencies
31 : #include <AP_Common/AP_Common.h>
32 : #include <AP_Common/Location.h>
33 : #include <AP_Param/AP_Param.h>
34 : #include <StorageManager/StorageManager.h>
35 : #include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
36 : #include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
37 : #include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
38 :
39 : // Application dependencies
40 : #include <AP_GPS/AP_GPS.h> // ArduPilot GPS library
41 : #include <AP_Logger/AP_Logger.h> // ArduPilot Mega Flash Memory Library
42 : #include <AP_Baro/AP_Baro.h>
43 : #include <AP_Compass/AP_Compass.h> // ArduPilot Mega Magnetometer Library
44 : #include <AP_InertialSensor/AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
45 : #include <AP_AHRS/AP_AHRS.h>
46 : #include <AP_Mission/AP_Mission.h> // Mission command library
47 : #include <AC_AttitudeControl/AC_AttitudeControl_Sub.h> // Attitude control library
48 : #include <AC_AttitudeControl/AC_PosControl.h> // Position control library
49 : #include <AP_Motors/AP_Motors.h> // AP Motors library
50 : #include <Filter/Filter.h> // Filter library
51 : #include <AP_Relay/AP_Relay.h> // APM relay
52 : #include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
53 : #include <AP_Vehicle/AP_Vehicle.h> // needed for AHRS build
54 : #include <AC_WPNav/AC_WPNav.h> // Waypoint navigation library
55 : #include <AC_WPNav/AC_Loiter.h>
56 : #include <AC_WPNav/AC_Circle.h> // circle navigation library
57 : #include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
58 : #include <AP_Scheduler/PerfInfo.h> // loop perf monitoring
59 : #include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
60 : #include <AP_Terrain/AP_Terrain.h>
61 : #include <AP_JSButton/AP_JSButton.h> // Joystick/gamepad button function assignment
62 : #include <AP_LeakDetector/AP_LeakDetector.h> // Leak detector
63 : #include <AP_Proximity/AP_Proximity.h>
64 : #include <AP_Rally/AP_Rally.h>
65 : #include <AP_OSD/AP_OSD.h>
66 :
67 : // Local modules
68 : #include "defines.h"
69 : #include "config.h"
70 : #include "GCS_MAVLink_Sub.h"
71 : #include "RC_Channel_Sub.h" // RC Channel Library
72 : #include "Parameters.h"
73 : #include "AP_Arming_Sub.h"
74 : #include "GCS_Sub.h"
75 : #include "mode.h"
76 : #include "script_button.h"
77 :
78 :
79 : #include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
80 :
81 : // libraries which are dependent on #defines in defines.h and/or config.h
82 : #if RCMAP_ENABLED
83 : #include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
84 : #endif
85 :
86 : #include <AP_RPM/AP_RPM_config.h>
87 :
88 : #if AP_RPM_ENABLED
89 : #include <AP_RPM/AP_RPM.h>
90 : #endif
91 :
92 : #if AVOIDANCE_ENABLED
93 : #include <AC_Avoidance/AC_Avoid.h> // Stop at fence library
94 : #endif
95 :
96 : #include <AP_Camera/AP_Camera.h> // Photo or video camera
97 :
98 : #if AP_SCRIPTING_ENABLED
99 : #include <AP_Scripting/AP_Scripting.h>
100 : #endif
101 :
102 : class Sub : public AP_Vehicle {
103 : public:
104 : friend class GCS_MAVLINK_Sub;
105 : friend class GCS_Sub;
106 : friend class Parameters;
107 : friend class ParametersG2;
108 : friend class AP_Arming_Sub;
109 : friend class RC_Channels_Sub;
110 : friend class RC_Channel_Sub;
111 : friend class Mode;
112 : friend class ModeManual;
113 : friend class ModeStabilize;
114 : friend class ModeAcro;
115 : friend class ModeAlthold;
116 : friend class ModeSurftrak;
117 : friend class ModeGuided;
118 : friend class ModePoshold;
119 : friend class ModeAuto;
120 : friend class ModeCircle;
121 : friend class ModeSurface;
122 : friend class ModeMotordetect;
123 :
124 : Sub(void);
125 :
126 : protected:
127 :
128 69 : bool should_zero_rc_outputs_on_reboot() const override { return true; }
129 :
130 : private:
131 :
132 : // Global parameters are all contained within the 'g' class.
133 : Parameters g;
134 : ParametersG2 g2;
135 :
136 : // primary input control channels
137 : RC_Channel *channel_roll;
138 : RC_Channel *channel_pitch;
139 : RC_Channel *channel_throttle;
140 : RC_Channel *channel_yaw;
141 : RC_Channel *channel_forward;
142 : RC_Channel *channel_lateral;
143 :
144 : AP_LeakDetector leak_detector;
145 :
146 : struct {
147 : bool enabled;
148 : bool alt_healthy; // true if we can trust the altitude from the rangefinder
149 : float alt; // tilt compensated altitude from rangefinder
150 : float min; // min rangefinder distance
151 : float max; // max rangefinder distance
152 : uint32_t last_healthy_ms;
153 : float inertial_alt_cm; // inertial alt at time of last rangefinder sample
154 : float rangefinder_terrain_offset_cm; // terrain height above EKF origin
155 : LowPassFilterFloat alt_filt; // altitude filter
156 : } rangefinder_state = { false, false, 0, 0, 0, 0, 0, 0 };
157 :
158 : // Mission library
159 : AP_Mission mission{
160 : FUNCTOR_BIND_MEMBER(&Sub::start_command, bool, const AP_Mission::Mission_Command &),
161 : FUNCTOR_BIND_MEMBER(&Sub::verify_command_callback, bool, const AP_Mission::Mission_Command &),
162 : FUNCTOR_BIND_MEMBER(&Sub::exit_mission, void)};
163 :
164 : // Optical flow sensor
165 : #if AP_OPTICALFLOW_ENABLED
166 : AP_OpticalFlow optflow;
167 : #endif
168 :
169 : #if OSD_ENABLED || OSD_PARAM_ENABLED
170 : AP_OSD osd;
171 : #endif
172 :
173 : // count number of times the AHRS yaw has been reset:
174 : uint16_t ahrs_yaw_reset_count;
175 :
176 : // GCS selection
177 : GCS_Sub _gcs; // avoid using this; use gcs()
178 689 : GCS_Sub &gcs() { return _gcs; }
179 :
180 : // User variables
181 : #ifdef USERHOOK_VARIABLES
182 : # include USERHOOK_VARIABLES
183 : #endif
184 :
185 : // Documentation of Globals:
186 : union {
187 : struct {
188 : uint8_t pre_arm_check : 1; // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
189 : uint8_t logging_started : 1; // true if logging has started
190 : uint8_t compass_mot : 1; // true if we are currently performing compassmot calibration
191 : uint8_t motor_test : 1; // true if we are currently performing the motors test
192 : uint8_t initialised : 1; // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes
193 : uint8_t gps_base_pos_set : 1; // true when the gps base position has been set (used for RTK gps only)
194 : uint8_t at_bottom : 1; // true if we are at the bottom
195 : uint8_t at_surface : 1; // true if we are at the surface
196 : uint8_t depth_sensor_present: 1; // true if there is a depth sensor detected at boot
197 : uint8_t unused1 : 1; // was compass_init_location; true when the compass's initial location has been set
198 : };
199 : uint32_t value;
200 : } ap;
201 :
202 : // This is the state of the flight control system
203 : // There are multiple states defined such as STABILIZE, ACRO,
204 : Mode::Number control_mode;
205 :
206 : Mode::Number prev_control_mode;
207 :
208 : #if RCMAP_ENABLED
209 : RCMapper rcmap;
210 : #endif
211 :
212 : // Failsafe
213 : struct {
214 : uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs
215 : uint32_t last_gcs_warn_ms;
216 : uint32_t last_pilot_input_ms; // last time we received pilot input in the form of MANUAL_CONTROL or RC_CHANNELS_OVERRIDE messages
217 : uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure
218 : uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed
219 : uint32_t last_crash_warn_ms; // last time a crash warning was sent to gcs
220 : uint32_t last_ekf_warn_ms; // last time an ekf warning was sent to gcs
221 : #if AP_SUB_RC_ENABLED
222 : int8_t radio_counter; // number of iterations with throttle below throttle_fs_value
223 : uint8_t radio : 1; // A status flag for the radio failsafe
224 : #endif
225 :
226 : uint8_t pilot_input : 1; // true if pilot input failsafe is active, handles things like joystick being disconnected during operation
227 : uint8_t gcs : 1; // A status flag for the ground station failsafe
228 : uint8_t ekf : 1; // true if ekf failsafe has occurred
229 : uint8_t terrain : 1; // true if the missing terrain data failsafe has occurred
230 : uint8_t leak : 1; // true if leak recently detected
231 : uint8_t internal_pressure : 1; // true if internal pressure is over threshold
232 : uint8_t internal_temperature : 1; // true if temperature is over threshold
233 : uint8_t crash : 1; // true if we are crashed
234 : uint8_t sensor_health : 1; // true if at least one sensor has triggered a failsafe (currently only used for depth in depth enabled modes)
235 : } failsafe;
236 :
237 6094 : bool any_failsafe_triggered() const {
238 : return (
239 6094 : failsafe.pilot_input
240 5788 : || battery.has_failsafed()
241 5788 : || failsafe.gcs
242 5736 : || failsafe.ekf
243 5736 : || failsafe.terrain
244 5715 : || failsafe.leak
245 5633 : || failsafe.internal_pressure
246 5633 : || failsafe.internal_temperature
247 5633 : || failsafe.crash
248 11882 : || failsafe.sensor_health
249 6094 : );
250 : }
251 :
252 : // sensor health for logging
253 : struct {
254 : uint8_t depth : 1; // true if depth sensor is healthy
255 : uint8_t compass : 1; // true if compass is healthy
256 : } sensor_health;
257 :
258 : // Baro sensor instance index of the external water pressure sensor
259 : uint8_t depth_sensor_idx;
260 :
261 : AP_Motors6DOF motors;
262 :
263 : // Circle
264 : bool circle_pilot_yaw_override; // true if pilot is overriding yaw
265 :
266 : // Stores initial bearing when armed
267 : int32_t initial_armed_bearing;
268 :
269 : // Loiter control
270 : uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds)
271 : uint32_t loiter_time; // How long have we been loitering - The start time in millis
272 :
273 : // Delay the next navigation command
274 : uint32_t nav_delay_time_max_ms; // used for delaying the navigation commands
275 : uint32_t nav_delay_time_start_ms;
276 :
277 : // Battery Sensors
278 : AP_BattMonitor battery{MASK_LOG_CURRENT,
279 : FUNCTOR_BIND_MEMBER(&Sub::handle_battery_failsafe, void, const char*, const int8_t),
280 : _failsafe_priorities};
281 :
282 : AP_Arming_Sub arming;
283 :
284 : // Turn counter
285 : int32_t quarter_turn_count;
286 : uint8_t last_turn_state;
287 :
288 : // Input gain
289 : float gain;
290 :
291 : // Flag indicating if we are currently using input hold
292 : bool input_hold_engaged;
293 :
294 : // Flag indicating if we are currently controlling Pitch and Roll instead of forward/lateral
295 : bool roll_pitch_flag = false;
296 :
297 : // 3D Location vectors
298 : // Current location of the Sub (altitude is relative to home)
299 : Location current_loc;
300 :
301 : // Navigation Yaw control
302 : // auto flight mode's yaw mode
303 : uint8_t auto_yaw_mode;
304 :
305 : // Parameter to set yaw rate only
306 : bool yaw_rate_only;
307 :
308 : // Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI
309 : Vector3f roi_WP_neu_cm;
310 :
311 : // bearing from current location to the yaw_look_at_WP
312 : float yaw_look_at_WP_bearing;
313 :
314 : float yaw_xtrack_correct_heading;
315 :
316 : // yaw used for YAW_LOOK_AT_HEADING yaw_mode
317 : int32_t yaw_look_at_heading;
318 :
319 : // Deg/s we should turn
320 : int16_t yaw_look_at_heading_slew;
321 :
322 : // heading when in yaw_look_ahead_bearing
323 : float yaw_look_ahead_bearing;
324 :
325 : // Delay Mission Scripting Command
326 : int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
327 : uint32_t condition_start;
328 :
329 : AP_AHRS_View ahrs_view;
330 :
331 : // Attitude, Position and Waypoint navigation objects
332 : // To-Do: move inertial nav up or other navigation variables down here
333 : AC_AttitudeControl_Sub attitude_control;
334 :
335 : AC_PosControl pos_control;
336 :
337 : AC_WPNav wp_nav;
338 : AC_Loiter loiter_nav;
339 : AC_Circle circle_nav;
340 :
341 : // Camera
342 : #if AP_CAMERA_ENABLED
343 : AP_Camera camera{MASK_LOG_CAMERA};
344 : #endif
345 :
346 : // Camera/Antenna mount tracking and stabilisation stuff
347 : #if HAL_MOUNT_ENABLED
348 : AP_Mount camera_mount;
349 : #endif
350 :
351 : #if AVOIDANCE_ENABLED
352 : AC_Avoid avoid;
353 : #endif
354 :
355 : // Rally library
356 : #if HAL_RALLY_ENABLED
357 : AP_Rally rally;
358 : #endif
359 :
360 : // terrain handling
361 : #if AP_TERRAIN_AVAILABLE
362 : AP_Terrain terrain;
363 : #endif
364 :
365 : // used to allow attitude and depth control without a position system
366 : struct attitude_no_gps_struct {
367 : uint32_t last_message_ms;
368 : mavlink_set_attitude_target_t packet;
369 : };
370 :
371 : attitude_no_gps_struct set_attitude_target_no_gps {0};
372 :
373 : // Top-level logic
374 : // setup the var_info table
375 : AP_Param param_loader;
376 :
377 : float last_pilot_heading_rad;
378 : uint32_t last_pilot_yaw_input_ms;
379 : uint32_t fs_terrain_recover_start_ms;
380 :
381 : static const AP_Scheduler::Task scheduler_tasks[];
382 : static const AP_Param::Info var_info[];
383 : static const struct LogStructure log_structure[];
384 :
385 : void run_rate_controller();
386 : void fifty_hz_loop();
387 : void update_batt_compass(void);
388 : void ten_hz_logging_loop();
389 : void twentyfive_hz_logging();
390 : void loop_rate_logging();
391 : void three_hz_loop();
392 : void one_hz_loop();
393 : void update_turn_counter();
394 : void read_AHRS(void);
395 : void update_altitude();
396 : float get_smoothing_gain();
397 : void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
398 : float get_pilot_desired_yaw_rate(int16_t stick_angle) const;
399 : void check_ekf_yaw_reset();
400 : float get_roi_yaw();
401 : float get_look_ahead_yaw();
402 : float get_pilot_desired_climb_rate(float throttle_control);
403 : float get_pilot_desired_horizontal_rate(RC_Channel *channel) const;
404 : void rotate_body_frame_to_NE(float &x, float &y);
405 : #if HAL_LOGGING_ENABLED
406 : // methods for AP_Vehicle:
407 83 : const AP_Int32 &get_log_bitmask() override { return g.log_bitmask; }
408 83 : const struct LogStructure *get_log_structures() const override {
409 83 : return log_structure;
410 : }
411 : uint8_t get_num_log_structures() const override;
412 :
413 : void Log_Write_Control_Tuning();
414 : void Log_Write_Attitude();
415 : void Log_Write_Data(LogDataID id, int32_t value);
416 : void Log_Write_Data(LogDataID id, uint32_t value);
417 : void Log_Write_Data(LogDataID id, int16_t value);
418 : void Log_Write_Data(LogDataID id, uint16_t value);
419 : void Log_Write_Data(LogDataID id, float value);
420 : void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target_neu_cm, const Vector3f& vel_target_neu_cms, const Vector3f& acc_target_neu_cmss);
421 : void Log_Write_Vehicle_Startup_Messages();
422 : #endif
423 : void load_parameters(void) override;
424 : void userhook_init();
425 : void userhook_FastLoop();
426 : void userhook_50Hz();
427 : void userhook_MediumLoop();
428 : void userhook_SlowLoop();
429 : void userhook_SuperSlowLoop();
430 : void update_home_from_EKF();
431 : bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
432 : bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
433 : float get_alt_rel() const WARN_IF_UNUSED;
434 : float get_alt_msl() const WARN_IF_UNUSED;
435 : void exit_mission();
436 : void set_origin(const Location& loc);
437 : bool verify_loiter_unlimited();
438 : bool verify_loiter_time();
439 : bool verify_wait_delay();
440 : bool verify_within_distance();
441 : bool verify_yaw();
442 :
443 : void failsafe_sensors_check(void);
444 : void failsafe_crash_check();
445 : void failsafe_ekf_check(void);
446 : void handle_battery_failsafe(const char* type_str, const int8_t action);
447 : void failsafe_gcs_check();
448 : void failsafe_pilot_input_check(void);
449 : void set_neutral_controls(void);
450 : void failsafe_terrain_check();
451 : void failsafe_terrain_set_status(bool data_ok);
452 : void failsafe_terrain_on_event();
453 : void mainloop_failsafe_enable();
454 : void mainloop_failsafe_disable();
455 : #if AP_FENCE_ENABLED
456 : void fence_check();
457 : void fence_checks_async() override;
458 : #endif
459 : bool set_mode(Mode::Number mode, ModeReason reason);
460 : bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
461 19412 : uint8_t get_mode() const override { return (uint8_t)control_mode; }
462 : void update_flight_mode();
463 : void exit_mode(Mode::Number old_control_mode, Mode::Number new_control_mode);
464 : void notify_flight_mode();
465 : void read_inertia();
466 : void update_surface_and_bottom_detector();
467 : void set_surfaced(bool at_surface);
468 : void set_bottomed(bool at_bottom);
469 : void motors_output();
470 : void init_rc_in();
471 : void init_rc_out();
472 : #if AP_SUB_RC_ENABLED
473 : void rc_loop();
474 : void read_radio();
475 : // last valid RC input time
476 : uint32_t last_radio_update_ms;
477 : void set_failsafe_radio(bool b);
478 : void set_throttle_and_failsafe(uint16_t throttle_pwm);
479 : void failsafe_radio_off_event();
480 : void failsafe_radio_on_event();
481 : #endif
482 : void enable_motor_output();
483 : void init_joystick();
484 : void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions,
485 : int16_t s,
486 : int16_t t,
487 : int16_t aux1,
488 : int16_t aux2,
489 : int16_t aux3,
490 : int16_t aux4,
491 : int16_t aux5,
492 : int16_t aux6);
493 : void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false);
494 : void handle_jsbutton_release(uint8_t button, bool shift);
495 : JSButton* get_button(uint8_t index);
496 : void default_js_buttons(void);
497 : void clear_input_hold();
498 : bool jsbutton_function_is_assigned(JSButton::button_function_t function);
499 : void read_barometer(void);
500 : void init_rangefinder(void);
501 : void read_rangefinder(void);
502 : void terrain_update();
503 : void terrain_logging();
504 : void init_ardupilot() override;
505 : void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
506 : uint8_t &task_count,
507 : uint32_t &log_bit) override;
508 : void startup_INS_ground();
509 : bool position_ok();
510 : bool ekf_position_ok();
511 : bool optflow_position_ok();
512 : bool should_log(uint32_t mask);
513 : bool start_command(const AP_Mission::Mission_Command& cmd);
514 : bool verify_command(const AP_Mission::Mission_Command& cmd);
515 : bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
516 :
517 : bool do_guided(const AP_Mission::Mission_Command& cmd);
518 : void do_nav_wp(const AP_Mission::Mission_Command& cmd);
519 : void do_surface(const AP_Mission::Mission_Command& cmd);
520 : void do_RTL(void);
521 : void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
522 : void do_circle(const AP_Mission::Mission_Command& cmd);
523 : void do_loiter_time(const AP_Mission::Mission_Command& cmd);
524 : #if NAV_GUIDED
525 : void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
526 : void do_guided_limits(const AP_Mission::Mission_Command& cmd);
527 : #endif
528 : void do_nav_delay(const AP_Mission::Mission_Command& cmd);
529 : void do_wait_delay(const AP_Mission::Mission_Command& cmd);
530 : void do_within_distance(const AP_Mission::Mission_Command& cmd);
531 : void do_yaw(const AP_Mission::Mission_Command& cmd);
532 : void do_change_speed(const AP_Mission::Mission_Command& cmd);
533 : void do_set_home(const AP_Mission::Mission_Command& cmd);
534 : void do_roi(const AP_Mission::Mission_Command& cmd);
535 : void do_mount_control(const AP_Mission::Mission_Command& cmd);
536 :
537 : bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
538 : bool verify_surface(const AP_Mission::Mission_Command& cmd);
539 : bool verify_RTL(void);
540 : bool verify_circle(const AP_Mission::Mission_Command& cmd);
541 : #if NAV_GUIDED
542 : bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
543 : #endif
544 : bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
545 :
546 : void failsafe_leak_check();
547 : void failsafe_internal_pressure_check();
548 : void failsafe_internal_temperature_check();
549 :
550 : void failsafe_terrain_act(void);
551 :
552 :
553 : void translate_wpnav_rp(float &lateral_out, float &forward_out);
554 : void translate_circle_nav_rp(float &lateral_out, float &forward_out);
555 : void translate_pos_control_rp(float &lateral_out, float &forward_out);
556 :
557 : void stats_update();
558 :
559 : uint16_t get_pilot_speed_dn() const;
560 :
561 :
562 : #if LEAKDETECTOR_MAX_INSTANCES > 0
563 : void update_leak_pins();
564 : #endif
565 : #if AP_RELAY_ENABLED
566 : void update_relay_pins();
567 : #endif
568 :
569 : void update_actuators_from_jsbuttons();
570 : void update_lights_from_rcin();
571 : bool handle_do_motor_test(mavlink_command_int_t command);
572 : bool init_motor_test();
573 : bool verify_motor_test();
574 :
575 : uint32_t last_do_motor_test_fail_ms = 0;
576 : uint32_t last_do_motor_test_ms = 0;
577 :
578 : bool control_check_barometer();
579 :
580 : // vehicle specific waypoint info helpers
581 : bool get_wp_distance_m(float &distance) const override;
582 : bool get_wp_bearing_deg(float &bearing) const override;
583 : bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
584 :
585 : enum Failsafe_Action {
586 : Failsafe_Action_None = 0,
587 : Failsafe_Action_Warn = 1,
588 : Failsafe_Action_Disarm = 2,
589 : Failsafe_Action_Surface = 3
590 : };
591 :
592 : static constexpr int8_t _failsafe_priorities[] = {
593 : Failsafe_Action_Disarm,
594 : Failsafe_Action_Surface,
595 : Failsafe_Action_Warn,
596 : Failsafe_Action_None,
597 : -1 // the priority list must end with a sentinel of -1
598 : };
599 :
600 : static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
601 : "_failsafe_priorities is missing the sentinel");
602 :
603 : Mode *mode_from_mode_num(const Mode::Number num);
604 : void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode);
605 :
606 : Mode *flightmode;
607 : ModeManual mode_manual;
608 : ModeStabilize mode_stabilize;
609 : ModeAcro mode_acro;
610 : ModeAlthold mode_althold;
611 : ModeAuto mode_auto;
612 : ModeGuided mode_guided;
613 : ModePoshold mode_poshold;
614 : ModeCircle mode_circle;
615 : ModeSurface mode_surface;
616 : ModeMotordetect mode_motordetect;
617 : ModeSurftrak mode_surftrak;
618 :
619 : // Auto
620 : AutoSubMode auto_mode; // controls which auto controller is run
621 : GuidedSubMode guided_mode;
622 :
623 : #if AP_SCRIPTING_ENABLED
624 : ScriptButton script_buttons[4];
625 : #endif // AP_SCRIPTING_ENABLED
626 :
627 : public:
628 : void mainloop_failsafe_check();
629 : bool rangefinder_alt_ok() const WARN_IF_UNUSED;
630 :
631 : static Sub *_singleton;
632 :
633 3177 : static Sub *get_singleton() {
634 3177 : return _singleton;
635 : }
636 :
637 : #if AP_SCRIPTING_ENABLED
638 : // For Lua scripting, so index is 1..4, not 0..3
639 : bool is_button_pressed(uint8_t index);
640 :
641 : // For Lua scripting, so index is 1..4, not 0..3
642 : uint8_t get_and_clear_button_count(uint8_t index);
643 :
644 : // Set targets in GUIDED mode
645 : bool 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) override;
646 :
647 : #if AP_RANGEFINDER_ENABLED
648 3143 : float get_rangefinder_target_cm() const WARN_IF_UNUSED { return mode_surftrak.get_rangefinder_target_cm(); }
649 34 : bool set_rangefinder_target_cm(float new_target_cm) { return mode_surftrak.set_rangefinder_target_cm(new_target_cm); }
650 : #endif // AP_RANGEFINDER_ENABLED
651 : #endif // AP_SCRIPTING_ENABLED
652 : };
653 :
654 : extern const AP_HAL::HAL& hal;
655 : extern Sub sub;
|
