Line data Source code
1 : #include "Rover.h"
2 :
3 15369390 : static void failsafe_check_static()
4 : {
5 15369390 : rover.failsafe_check();
6 15369390 : }
7 :
8 285 : void Rover::init_ardupilot()
9 : {
10 : // initialise notify system
11 285 : notify.init();
12 285 : notify_mode(control_mode);
13 :
14 285 : battery.init();
15 :
16 : #if AP_RSSI_ENABLED
17 285 : rssi.init();
18 : #endif
19 :
20 285 : g2.windvane.init(serial_manager);
21 :
22 : // init baro before we start the GCS, so that the CLI baro test works
23 285 : barometer.init();
24 :
25 : // setup telem slots with serial ports
26 285 : gcs().setup_uarts();
27 :
28 : #if OSD_ENABLED
29 285 : osd.init();
30 : #endif
31 :
32 : // initialise compass
33 285 : AP::compass().set_log_bit(MASK_LOG_COMPASS);
34 285 : AP::compass().init();
35 :
36 : #if AP_AIRSPEED_ENABLED
37 285 : airspeed.set_log_bit(MASK_LOG_IMU);
38 : #endif
39 :
40 : #if AP_RANGEFINDER_ENABLED
41 : // initialise rangefinder
42 285 : rangefinder.set_log_rfnd_bit(MASK_LOG_RANGEFINDER);
43 285 : rangefinder.init(ROTATION_NONE);
44 : #endif
45 :
46 : #if HAL_PROXIMITY_ENABLED
47 : // init proximity sensor
48 285 : g2.proximity.init();
49 : #endif
50 :
51 : // and baro for EKF
52 285 : barometer.set_log_baro_bit(MASK_LOG_IMU);
53 285 : barometer.calibrate();
54 :
55 : // Do GPS init
56 283 : gps.set_log_gps_bit(MASK_LOG_GPS);
57 283 : gps.init();
58 :
59 283 : ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
60 :
61 283 : init_rc_in(); // sets up rc channels deadzone
62 283 : g2.motors.init(get_frame_type()); // init motors including setting servo out channels ranges
63 283 : AP::srv().enable_aux_servos();
64 :
65 : // init wheel encoders
66 283 : g2.wheel_encoder.init();
67 :
68 : #if HAL_TORQEEDO_ENABLED
69 : // init torqeedo motor driver
70 283 : g2.torqeedo.init();
71 : #endif
72 :
73 : #if AP_OPTICALFLOW_ENABLED
74 : // initialise optical flow sensor
75 283 : optflow.init(MASK_LOG_OPTFLOW);
76 : #endif // AP_OPTICALFLOW_ENABLED
77 :
78 : #if AP_RELAY_ENABLED
79 283 : relay.init();
80 : #endif
81 :
82 : #if HAL_MOUNT_ENABLED
83 : // initialise camera mount
84 283 : camera_mount.init();
85 : #endif
86 :
87 : #if AP_CAMERA_ENABLED
88 : // initialise camera
89 283 : camera.init();
90 : #endif
91 :
92 : #if AC_PRECLAND_ENABLED
93 : // initialise precision landing
94 283 : init_precland();
95 : #endif
96 :
97 : /*
98 : setup the 'main loop is dead' check. Note that this relies on
99 : the RC library being initialised.
100 : */
101 283 : hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
102 :
103 : // initialize SmartRTL
104 283 : g2.smart_rtl.init();
105 :
106 : #if AP_OAPATHPLANNER_ENABLED
107 : // initialise object avoidance
108 283 : g2.oa.init();
109 : #endif
110 :
111 283 : set_mode(mode_initializing, ModeReason::INITIALISED);
112 :
113 283 : startup_INS();
114 :
115 : #if AP_MISSION_ENABLED
116 : // initialise mission library
117 283 : mode_auto.mission.init();
118 : #if HAL_LOGGING_ENABLED
119 283 : mode_auto.mission.set_log_start_mission_item_bit(MASK_LOG_CMD);
120 : #endif
121 : #endif
122 :
123 : // initialise AP_Logger library
124 : #if HAL_LOGGING_ENABLED
125 283 : logger.setVehicle_Startup_Writer(
126 : FUNCTOR_BIND(&rover, &Rover::Log_Write_Vehicle_Startup_Messages, void)
127 : );
128 : #endif
129 :
130 283 : Mode *initial_mode = mode_from_mode_num((enum Mode::Number)g.initial_mode.get());
131 283 : if (initial_mode == nullptr) {
132 0 : initial_mode = &mode_initializing;
133 : }
134 283 : set_mode(*initial_mode, ModeReason::INITIALISED);
135 :
136 : // initialise rc channels
137 283 : rc().init();
138 :
139 283 : rover.g2.sailboat.init();
140 :
141 : // boat should loiter after completing a mission to avoid drifting off
142 283 : if (is_boat()) {
143 7 : rover.g2.mis_done_behave.set_default(uint8_t(ModeAuto::DoneBehaviour::LOITER));
144 : }
145 :
146 : // flag that initialisation has completed
147 283 : initialised = true;
148 283 : }
149 :
150 : // update the ahrs flyforward setting which can allow
151 : // the vehicle's movements to be used to estimate heading
152 629048 : void Rover::update_ahrs_flyforward()
153 : {
154 629048 : bool flyforward = false;
155 :
156 : // boats never use movement to estimate heading
157 629048 : if (!is_boat()) {
158 : // throttle threshold is 15% or 1/2 cruise throttle
159 578737 : bool throttle_over_thresh = g2.motors.get_throttle() > MIN(g.throttle_cruise * 0.50f, 15.0f);
160 : // desired speed threshold of 1m/s
161 578737 : bool desired_speed_over_thresh = g2.attitude_control.speed_control_active() && (g2.attitude_control.get_desired_speed() > 0.5f);
162 578737 : if (throttle_over_thresh || (is_positive(g2.motors.get_throttle()) && desired_speed_over_thresh)) {
163 144866 : uint32_t now = AP_HAL::millis();
164 : // if throttle over threshold start timer
165 144866 : if (flyforward_start_ms == 0) {
166 186 : flyforward_start_ms = now;
167 : }
168 : // if throttle over threshold for 2 seconds set flyforward to true
169 144866 : flyforward = (now - flyforward_start_ms > 2000);
170 : } else {
171 : // reset timer
172 433871 : flyforward_start_ms = 0;
173 : }
174 : }
175 :
176 629048 : ahrs.set_fly_forward(flyforward);
177 629048 : }
178 :
179 : // Check if this mode can be entered from the GCS
180 138294 : bool Rover::gcs_mode_enabled(const Mode::Number mode_num) const
181 : {
182 : // List of modes that can be blocked, index is bit number in parameter bitmask
183 : static const uint8_t mode_list [] {
184 : (uint8_t)Mode::Number::MANUAL,
185 : (uint8_t)Mode::Number::ACRO,
186 : (uint8_t)Mode::Number::STEERING,
187 : (uint8_t)Mode::Number::LOITER,
188 : (uint8_t)Mode::Number::FOLLOW,
189 : (uint8_t)Mode::Number::SIMPLE,
190 : (uint8_t)Mode::Number::CIRCLE,
191 : (uint8_t)Mode::Number::AUTO,
192 : (uint8_t)Mode::Number::RTL,
193 : (uint8_t)Mode::Number::SMART_RTL,
194 : (uint8_t)Mode::Number::GUIDED,
195 : #if MODE_DOCK_ENABLED
196 : (uint8_t)Mode::Number::DOCK
197 : #endif
198 : };
199 :
200 138294 : return !block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list));
201 : }
202 :
203 : // Return mask of enabled modes, order does not matter, its just for tracking changes
204 12526 : uint32_t Rover::get_available_mode_enabled_mask() const
205 : {
206 12526 : const Mode* flight_modes[] {
207 : &rover.mode_manual,
208 : &rover.mode_acro,
209 : &rover.mode_steering,
210 : &rover.mode_hold,
211 : &rover.mode_loiter,
212 : #if MODE_FOLLOW_ENABLED
213 : &rover.mode_follow,
214 : #endif
215 : &rover.mode_simple,
216 : &rover.g2.mode_circle,
217 : &rover.mode_auto,
218 : &rover.mode_rtl,
219 : &rover.mode_smartrtl,
220 : &rover.mode_guided,
221 : &rover.mode_initializing,
222 : #if MODE_DOCK_ENABLED
223 12526 : (Mode *)rover.g2.mode_dock_ptr,
224 : #endif
225 12526 : };
226 :
227 : static_assert(ARRAY_SIZE(flight_modes) <= 32, "Flight modes must fit in 32 bit bitmask");
228 :
229 12526 : uint32_t mask = 0;
230 187890 : for (uint8_t i = 0; i < ARRAY_SIZE(flight_modes); i++) {
231 175364 : const Mode* mode = flight_modes[i];
232 :
233 : // the check here must be the same as the one in `send_available_mode`
234 175364 : const bool user_selectable = mode->enabled() && rover.gcs_mode_enabled(mode->mode_number());
235 :
236 175364 : if (user_selectable) {
237 138088 : mask |= 1U << i;
238 : }
239 : }
240 :
241 12526 : return mask;
242 : }
243 :
244 :
245 1266 : bool Rover::set_mode(Mode &new_mode, ModeReason reason)
246 : {
247 1266 : if (control_mode == &new_mode) {
248 : // don't switch modes if we are already in the correct mode.
249 724 : return true;
250 : }
251 :
252 : // Check if GCS mode change is disabled via parameter
253 542 : if ((reason == ModeReason::GCS_COMMAND) && !gcs_mode_enabled((Mode::Number)new_mode.mode_number())) {
254 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"Mode change to %s denied, GCS entry disabled (FLTMODE_GCSBLOCK)", new_mode.name());
255 0 : return false;
256 : }
257 :
258 542 : Mode &old_mode = *control_mode;
259 542 : if (!new_mode.enter()) {
260 : // Log error that we failed to enter desired flight mode
261 21 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE,
262 : LogErrorCode(new_mode.mode_number()));
263 21 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Flight mode change failed");
264 21 : return false;
265 : }
266 :
267 521 : control_mode = &new_mode;
268 :
269 : #if AP_FENCE_ENABLED
270 : // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
271 : // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
272 : // but it should be harmless to disable the fence temporarily in these situations as well
273 521 : fence.manual_recovery_start();
274 : #endif
275 :
276 : #if AP_CAMERA_ENABLED
277 521 : camera.set_is_auto_mode(control_mode->mode_number() == Mode::Number::AUTO);
278 : #endif
279 :
280 521 : old_mode.exit();
281 :
282 521 : control_mode_reason = reason;
283 : #if HAL_LOGGING_ENABLED
284 521 : logger.Write_Mode((uint8_t)control_mode->mode_number(), control_mode_reason);
285 : #endif
286 521 : gcs().send_message(MSG_HEARTBEAT);
287 :
288 521 : notify_mode(control_mode);
289 521 : return true;
290 : }
291 :
292 348 : bool Rover::set_mode(const uint8_t new_mode, ModeReason reason)
293 : {
294 : static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
295 348 : return rover.set_mode(static_cast<Mode::Number>(new_mode), reason);
296 : }
297 :
298 662 : bool Rover::set_mode(Mode::Number new_mode, ModeReason reason)
299 : {
300 662 : Mode *mode = rover.mode_from_mode_num(new_mode);
301 662 : if (mode == nullptr) {
302 2 : notify_no_such_mode((uint8_t)new_mode);
303 2 : return false;
304 : }
305 660 : return rover.set_mode(*mode, reason);
306 : }
307 :
308 283 : void Rover::startup_INS(void)
309 : {
310 283 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Beginning INS calibration. Do not move vehicle");
311 283 : hal.scheduler->delay(100);
312 :
313 283 : ahrs.init();
314 : // say to EKF that rover only move by going forward
315 283 : ahrs.set_fly_forward(true);
316 283 : ahrs.set_vehicle_class(AP_AHRS::VehicleClass::GROUND);
317 :
318 283 : ins.init(scheduler.get_loop_rate_hz());
319 283 : ahrs.reset();
320 283 : }
321 :
322 : // update notify with mode change
323 806 : void Rover::notify_mode(const Mode *mode)
324 : {
325 806 : AP_Notify::flags.autopilot_mode = mode->is_autopilot_mode();
326 806 : notify.flags.flight_mode = (uint8_t)mode->mode_number();
327 806 : notify.set_flight_mode_str(mode->name4());
328 806 : }
329 :
330 : /*
331 : check a digital pin for high,low (1/0)
332 : */
333 0 : uint8_t Rover::check_digital_pin(uint8_t pin)
334 : {
335 : // ensure we are in input mode
336 0 : hal.gpio->pinMode(pin, HAL_GPIO_INPUT);
337 :
338 : // enable pullup
339 0 : hal.gpio->write(pin, 1);
340 :
341 0 : return hal.gpio->read(pin);
342 : }
343 :
344 : #if HAL_LOGGING_ENABLED
345 : /*
346 : should we log a message type now?
347 : */
348 3012744 : bool Rover::should_log(uint32_t mask)
349 : {
350 3012744 : return logger.should_log(mask);
351 : }
352 : #endif
353 :
354 : // returns true if vehicle is a boat
355 : // this affects whether the vehicle tries to maintain position after reaching waypoints
356 1491317 : bool Rover::is_boat() const
357 : {
358 1491317 : return ((enum frame_class)g2.frame_class.get() == FRAME_BOAT);
359 : }
360 :
361 : #include <AP_Avoidance/AP_Avoidance.h>
362 : #if AP_ADSB_AVOIDANCE_ENABLED
363 : // dummy method to avoid linking AP_Avoidance
364 0 : AP_Avoidance *AP::ap_avoidance() { return nullptr; }
365 : #endif // AP_ADSB_AVOIDANCE_ENABLED
|
