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 : main Rover class, containing all vehicle specific state
17 : */
18 : #pragma once
19 :
20 : #include <cmath>
21 : #include <stdarg.h>
22 : #include <stdint.h>
23 :
24 : // Libraries
25 : #include <AP_Common/AP_Common.h>
26 : #include <AP_HAL/AP_HAL.h>
27 : #include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
28 : #include <AP_Camera/AP_Camera.h> // Camera triggering
29 : #include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
30 : #include <AP_Param/AP_Param.h>
31 : #include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
32 : #include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
33 : #include <AP_RPM/AP_RPM.h> // RPM input library
34 : #include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
35 : #include <AP_Vehicle/AP_Vehicle.h> // needed for AHRS build
36 : #include <AP_WheelEncoder/AP_WheelEncoder.h>
37 : #include <AP_WheelEncoder/AP_WheelRateControl.h>
38 : #include <AP_Logger/AP_Logger.h>
39 : #include <AP_OSD/AP_OSD.h>
40 : #include <AR_Motors/AP_MotorsUGV.h>
41 : #include <AP_Mission/AP_Mission.h>
42 : #include <AP_Mission/AP_Mission_ChangeDetector.h>
43 : #include <AR_WPNav/AR_WPNav_OA.h>
44 : #include <AP_OpticalFlow/AP_OpticalFlow.h>
45 : #include <AC_PrecLand/AC_PrecLand_config.h>
46 : #include <AP_Follow/AP_Follow_config.h>
47 : #include <AP_ExternalControl/AP_ExternalControl_config.h>
48 : #if AP_EXTERNAL_CONTROL_ENABLED
49 : #include "AP_ExternalControl_Rover.h"
50 : #endif
51 :
52 : // Configuration
53 : #include "defines.h"
54 : #include "config.h"
55 :
56 : #if AP_SCRIPTING_ENABLED
57 : #include <AP_Scripting/AP_Scripting.h>
58 : #endif
59 :
60 : // Local modules
61 : #include "AP_Arming_Rover.h"
62 : #include "sailboat.h"
63 : #if AP_ROVER_ADVANCED_FAILSAFE_ENABLED
64 : #include "afs_rover.h"
65 : #endif
66 : #include "Parameters.h"
67 : #include "GCS_MAVLink_Rover.h"
68 : #include "GCS_Rover.h"
69 : #include "AP_Rally.h"
70 : #if AC_PRECLAND_ENABLED
71 : #include <AC_PrecLand/AC_PrecLand.h>
72 : #endif
73 : #include "RC_Channel_Rover.h" // RC Channel Library
74 :
75 : #include "mode.h"
76 :
77 : class Rover : public AP_Vehicle {
78 : public:
79 : friend class GCS_MAVLINK_Rover;
80 : friend class Parameters;
81 : friend class ParametersG2;
82 : friend class AP_Rally_Rover;
83 : friend class AP_Arming_Rover;
84 : #if AP_ROVER_ADVANCED_FAILSAFE_ENABLED
85 : friend class AP_AdvancedFailsafe_Rover;
86 : #endif
87 : #if AP_EXTERNAL_CONTROL_ENABLED
88 : friend class AP_ExternalControl_Rover;
89 : #endif
90 : friend class GCS_Rover;
91 : friend class Mode;
92 : friend class ModeAcro;
93 : friend class ModeAuto;
94 : friend class ModeCircle;
95 : friend class ModeGuided;
96 : friend class ModeHold;
97 : friend class ModeLoiter;
98 : friend class ModeSteering;
99 : friend class ModeManual;
100 : friend class ModeRTL;
101 : friend class ModeSmartRTL;
102 : #if MODE_FOLLOW_ENABLED
103 : friend class ModeFollow;
104 : #endif
105 : friend class ModeSimple;
106 : #if MODE_DOCK_ENABLED
107 : friend class ModeDock;
108 : #endif
109 :
110 : friend class RC_Channel_Rover;
111 : friend class RC_Channels_Rover;
112 :
113 : friend class Sailboat;
114 :
115 : Rover(void);
116 :
117 : private:
118 :
119 : // must be the first AP_Param variable declared to ensure its
120 : // constructor runs before the constructors of the other AP_Param
121 : // variables
122 : AP_Param param_loader;
123 :
124 : // all settable parameters
125 : Parameters g;
126 : ParametersG2 g2;
127 :
128 : // mapping between input channels
129 : RCMapper rcmap;
130 :
131 : // primary control channels
132 : RC_Channel *channel_steer;
133 : RC_Channel *channel_throttle;
134 : RC_Channel *channel_lateral;
135 : RC_Channel *channel_roll;
136 : RC_Channel *channel_pitch;
137 : RC_Channel *channel_walking_height;
138 :
139 : // Arming/Disarming management class
140 : AP_Arming_Rover arming;
141 :
142 : // external control implementation
143 : #if AP_EXTERNAL_CONTROL_ENABLED
144 : AP_ExternalControl_Rover external_control;
145 : #endif
146 :
147 : #if AP_OPTICALFLOW_ENABLED
148 : AP_OpticalFlow optflow;
149 : #endif
150 :
151 : #if OSD_ENABLED || OSD_PARAM_ENABLED
152 : AP_OSD osd;
153 : #endif
154 : #if AC_PRECLAND_ENABLED
155 : AC_PrecLand precland;
156 : #endif
157 : // GCS handling
158 : GCS_Rover _gcs; // avoid using this; use gcs()
159 3300 : GCS_Rover &gcs() { return _gcs; }
160 :
161 : // RC Channels:
162 1322225 : RC_Channels_Rover &rc() { return g2.rc_channels; }
163 :
164 : // The rover's current location
165 : Location current_loc;
166 :
167 : // Camera
168 : #if AP_CAMERA_ENABLED
169 : AP_Camera camera{MASK_LOG_CAMERA};
170 : #endif
171 :
172 : // Camera/Antenna mount tracking and stabilisation stuff
173 : #if HAL_MOUNT_ENABLED
174 : AP_Mount camera_mount;
175 : #endif
176 :
177 : // true if initialisation has completed
178 : bool initialised;
179 :
180 : // This is the state of the flight control system
181 : // There are multiple states defined such as MANUAL, AUTO, ...
182 : Mode *control_mode;
183 :
184 : // Used to maintain the state of the previous control switch position
185 : // This is set to -1 when we need to re-read the switch
186 : uint8_t oldSwitchPosition;
187 :
188 : // structure for holding failsafe state
189 : struct {
190 : uint8_t bits; // bit flags of failsafes that have started (but not necessarily triggered an action)
191 : uint32_t start_time; // start time of the earliest failsafe
192 : uint8_t triggered; // bit flags of failsafes that have triggered an action
193 : uint32_t last_valid_rc_ms; // system time of most recent RC input from pilot
194 : bool ekf;
195 : } failsafe;
196 :
197 : // true if we have a position estimate from AHRS
198 : bool have_position;
199 :
200 : #if AP_RANGEFINDER_ENABLED
201 : // range finder last update for each instance (used for DPTH logging)
202 : uint32_t rangefinder_last_reading_ms[RANGEFINDER_MAX_INSTANCES];
203 : #endif
204 :
205 : // Ground speed
206 : // The amount current ground speed is below min ground speed. meters per second
207 : float ground_speed;
208 :
209 : // Battery Sensors
210 : AP_BattMonitor battery{MASK_LOG_CURRENT,
211 : FUNCTOR_BIND_MEMBER(&Rover::handle_battery_failsafe, void, const char*, const int8_t),
212 : _failsafe_priorities};
213 :
214 : // flyforward timer
215 : uint32_t flyforward_start_ms;
216 :
217 : static const AP_Scheduler::Task scheduler_tasks[];
218 :
219 : static const AP_Param::Info var_info[];
220 : #if HAL_LOGGING_ENABLED
221 : static const LogStructure log_structure[];
222 : #endif
223 :
224 : // latest wheel encoder values
225 : float wheel_encoder_last_distance_m[WHEELENCODER_MAX_INSTANCES]; // total distance recorded by wheel encoder (for reporting to GCS)
226 : bool wheel_encoder_initialised; // true once arrays below have been initialised to sensors initial values
227 : float wheel_encoder_last_angle_rad[WHEELENCODER_MAX_INSTANCES]; // distance in radians at time of last update to EKF
228 : uint32_t wheel_encoder_last_reading_ms[WHEELENCODER_MAX_INSTANCES]; // system time of last ping from each encoder
229 : uint8_t wheel_encoder_last_index_sent; // index of the last wheel encoder sent to the EKF
230 :
231 : // True when we are doing motor test
232 : bool motor_test;
233 :
234 : ModeInitializing mode_initializing;
235 : ModeHold mode_hold;
236 : ModeManual mode_manual;
237 : ModeAcro mode_acro;
238 : ModeGuided mode_guided;
239 : ModeAuto mode_auto;
240 : ModeLoiter mode_loiter;
241 : ModeSteering mode_steering;
242 : ModeRTL mode_rtl;
243 : ModeSmartRTL mode_smartrtl;
244 : #if MODE_FOLLOW_ENABLED
245 : ModeFollow mode_follow;
246 : #endif
247 : ModeSimple mode_simple;
248 : #if MODE_DOCK_ENABLED
249 : ModeDock mode_dock;
250 : #endif
251 :
252 : // cruise throttle and speed learning
253 : typedef struct {
254 : LowPassFilterFloat speed_filt{2.0f};
255 : LowPassFilterFloat throttle_filt{2.0f};
256 : uint32_t learn_start_ms;
257 : uint32_t log_count;
258 : } cruise_learn_t;
259 : cruise_learn_t cruise_learn;
260 :
261 : // Rover.cpp
262 : #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
263 : bool set_target_location(const Location& target_loc) override;
264 : #endif
265 :
266 : #if AP_SCRIPTING_ENABLED
267 : bool set_target_velocity_NED(const Vector3f& vel_ned_ms, bool align_yaw_to_target) override;
268 : bool set_steering_and_throttle(float steering, float throttle) override;
269 : bool get_steering_and_throttle(float& steering, float& throttle) override;
270 : // set desired turn rate (degrees/sec) and speed (m/s). Used for scripting
271 : bool set_desired_turn_rate_and_speed(float turn_rate_degs, float speed_ms) override;
272 : bool set_desired_speed(float speed_ms) override;
273 : bool get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value) override;
274 : bool nav_scripting_enable(uint8_t mode) override;
275 : bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override;
276 : void nav_script_time_done(uint16_t id) override;
277 : #endif // AP_SCRIPTING_ENABLED
278 : void ahrs_update();
279 : void gcs_failsafe_check(void);
280 : void update_logging1(void);
281 : void update_logging2(void);
282 : void one_second_loop(void);
283 : void update_current_mode(void);
284 :
285 : // balance_bot.cpp
286 : void balancebot_pitch_control(float &throttle);
287 : bool is_balancebot() const;
288 :
289 : // commands.cpp
290 : bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
291 : bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
292 : void update_home();
293 :
294 : // crash_check.cpp
295 : void crash_check();
296 : bool is_crashed() const override;
297 :
298 : // cruise_learn.cpp
299 : void cruise_learn_start();
300 : void cruise_learn_update();
301 : void cruise_learn_complete();
302 : void log_write_cruise_learn() const;
303 :
304 : // ekf_check.cpp
305 : void ekf_check();
306 : bool ekf_over_threshold();
307 : bool ekf_position_ok();
308 : void failsafe_ekf_event();
309 : void failsafe_ekf_off_event(void);
310 :
311 : // failsafe.cpp
312 : void failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool on);
313 : void handle_battery_failsafe(const char* type_str, const int8_t action);
314 : #if AP_ROVER_ADVANCED_FAILSAFE_ENABLED
315 : void afs_fs_check(void);
316 : #endif
317 : #if AP_FENCE_ENABLED
318 : // fence.cpp
319 : void fence_checks_async() override;
320 : void fence_check();
321 : #endif
322 : // GCS_Mavlink.cpp
323 : void send_wheel_encoder_distance(mavlink_channel_t chan);
324 :
325 : #if HAL_LOGGING_ENABLED
326 : // methods for AP_Vehicle:
327 285 : const AP_Int32 &get_log_bitmask() override { return g.log_bitmask; }
328 285 : const struct LogStructure *get_log_structures() const override {
329 285 : return log_structure;
330 : }
331 : uint8_t get_num_log_structures() const override;
332 :
333 : // Log.cpp
334 : void Log_Write_Attitude();
335 : void Log_Write_Depth();
336 : void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
337 : void Log_Write_Nav_Tuning();
338 : void Log_Write_Sail();
339 : void Log_Write_Steering();
340 : void Log_Write_Throttle();
341 : void Log_Write_RC(void);
342 : void Log_Write_Vehicle_Startup_Messages();
343 : void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
344 : #endif
345 :
346 : // mode.cpp
347 : Mode *mode_from_mode_num(enum Mode::Number num);
348 :
349 : // Parameters.cpp
350 : void load_parameters(void) override;
351 :
352 : // precision_landing.cpp
353 : void init_precland();
354 : void update_precland();
355 :
356 : // radio.cpp
357 : void set_control_channels(void) override;
358 : void init_rc_in();
359 : void read_radio();
360 : void radio_failsafe_check(uint16_t pwm);
361 :
362 : // sensors.cpp
363 : void update_compass(void);
364 : void update_wheel_encoder();
365 : #if AP_RANGEFINDER_ENABLED
366 : void read_rangefinders(void);
367 : #endif
368 :
369 : // Steering.cpp
370 : void set_servos(void);
371 :
372 : // Rover.cpp
373 : void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
374 : uint8_t &task_count,
375 : uint32_t &log_bit) override;
376 :
377 : // system.cpp
378 : void init_ardupilot() override;
379 : void startup_ground(void);
380 : void update_ahrs_flyforward();
381 : bool gcs_mode_enabled(const Mode::Number mode_num) const;
382 : bool set_mode(Mode &new_mode, ModeReason reason);
383 : bool set_mode(const uint8_t new_mode, ModeReason reason) override;
384 : bool set_mode(Mode::Number new_mode, ModeReason reason);
385 0 : uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); }
386 140159 : bool current_mode_requires_mission() const override {
387 140159 : return control_mode == &mode_auto;
388 : }
389 :
390 : // Return mask of enabled modes, order does not matter, its just for tracking changes
391 : uint32_t get_available_mode_enabled_mask() const override;
392 :
393 : void startup_INS(void);
394 : void notify_mode(const Mode *new_mode);
395 : uint8_t check_digital_pin(uint8_t pin);
396 : bool should_log(uint32_t mask);
397 : bool is_boat() const;
398 :
399 : // vehicle specific waypoint info helpers
400 : bool get_wp_distance_m(float &distance) const override;
401 : bool get_wp_bearing_deg(float &bearing) const override;
402 : bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
403 :
404 : enum class FailsafeAction: int8_t {
405 : None = 0,
406 : RTL = 1,
407 : Hold = 2,
408 : SmartRTL = 3,
409 : SmartRTL_Hold = 4,
410 : Terminate = 5,
411 : Loiter_Hold = 6,
412 : };
413 :
414 : enum class Failsafe_Options : uint32_t {
415 : Failsafe_Option_Active_In_Hold = (1<<0)
416 : };
417 :
418 : static constexpr int8_t _failsafe_priorities[] = {
419 : (int8_t)FailsafeAction::Terminate,
420 : (int8_t)FailsafeAction::Hold,
421 : (int8_t)FailsafeAction::RTL,
422 : (int8_t)FailsafeAction::SmartRTL_Hold,
423 : (int8_t)FailsafeAction::SmartRTL,
424 : (int8_t)FailsafeAction::None,
425 : -1 // the priority list must end with a sentinel of -1
426 : };
427 : static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
428 : "_failsafe_priorities is missing the sentinel");
429 :
430 :
431 : public:
432 : void failsafe_check();
433 : // Motor test
434 : void motor_test_output();
435 : bool mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc, AP_MotorsUGV::motor_test_order motor_instance, uint8_t throttle_type, int16_t throttle_value);
436 : MAV_RESULT mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, AP_MotorsUGV::motor_test_order motor_instance, uint8_t throttle_type, int16_t throttle_value, float timeout_sec);
437 : void motor_test_stop();
438 :
439 : // frame type
440 283 : uint8_t get_frame_type() const { return g2.frame_type.get(); }
441 : AP_WheelRateControl& get_wheel_rate_control() { return g2.wheel_rate_control; }
442 :
443 : // Simple mode
444 : float simple_sin_yaw;
445 :
446 : #if AP_ROVER_AUTO_ARM_ONCE_ENABLED
447 : struct {
448 : uint32_t last_arm_attempt_ms;
449 : bool done;
450 : } auto_arm_once;
451 : void handle_auto_arm_once();
452 : #endif // AP_ROVER_AUTO_ARM_ONCE_ENABLED
453 : };
454 :
455 : extern Rover rover;
456 :
457 : using AP_HAL::millis;
458 : using AP_HAL::micros;
|
