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 : This is the ArduRover firmware. It was originally derived from
18 : ArduPlane by Jean-Louis Naudin (JLN), and then rewritten after the
19 : AP_HAL merge by Andrew Tridgell
20 :
21 : Maintainer: Randy Mackay, Grant Morphett
22 :
23 : Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin, Grant Morphett
24 :
25 : Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier
26 :
27 : APMrover alpha version tester: Franco Borasio, Daniel Chapelat...
28 :
29 : Please contribute your ideas! See https://ardupilot.club/dev for details
30 : */
31 :
32 : #include "Rover.h"
33 :
34 : #define FORCE_VERSION_H_INCLUDE
35 : #include "version.h"
36 : #undef FORCE_VERSION_H_INCLUDE
37 :
38 : const AP_HAL::HAL& hal = AP_HAL::get_HAL();
39 :
40 : #define SCHED_TASK(func, rate_hz, _max_time_micros, _priority) SCHED_TASK_CLASS(Rover, &rover, func, rate_hz, _max_time_micros, _priority)
41 :
42 : /*
43 : scheduler table - all regular tasks should be listed here.
44 :
45 : All entries in this table must be ordered by priority.
46 :
47 : This table is interleaved with the table in AP_Vehicle to determine
48 : the order in which tasks are run. Convenience methods SCHED_TASK
49 : and SCHED_TASK_CLASS are provided to build entries in this structure:
50 :
51 : SCHED_TASK arguments:
52 : - name of static function to call
53 : - rate (in Hertz) at which the function should be called
54 : - expected time (in MicroSeconds) that the function should take to run
55 : - priority (0 through 255, lower number meaning higher priority)
56 :
57 : SCHED_TASK_CLASS arguments:
58 : - class name of method to be called
59 : - instance on which to call the method
60 : - method to call on that instance
61 : - rate (in Hertz) at which the method should be called
62 : - expected time (in MicroSeconds) that the method should take to run
63 : - priority (0 through 255, lower number meaning higher priority)
64 :
65 : scheduler table - all regular tasks are listed here, along with how
66 : often they should be called (in Hz) and the maximum time
67 : they are expected to take (in microseconds)
68 : */
69 : const AP_Scheduler::Task Rover::scheduler_tasks[] = {
70 : // Function name, Hz, us,
71 : SCHED_TASK(read_radio, 50, 200, 3),
72 : SCHED_TASK(ahrs_update, 400, 400, 6),
73 : #if AP_RANGEFINDER_ENABLED
74 : SCHED_TASK(read_rangefinders, 50, 200, 9),
75 : #endif
76 : #if AP_OPTICALFLOW_ENABLED
77 : SCHED_TASK_CLASS(AP_OpticalFlow, &rover.optflow, update, 200, 160, 11),
78 : #endif
79 : SCHED_TASK(update_current_mode, 400, 200, 12),
80 : SCHED_TASK(set_servos, 400, 200, 15),
81 : SCHED_TASK_CLASS(AP_GPS, &rover.gps, update, 50, 300, 18),
82 : SCHED_TASK_CLASS(AP_Baro, &rover.barometer, update, 10, 200, 21),
83 : #if HAL_PROXIMITY_ENABLED
84 : SCHED_TASK_CLASS(AP_Proximity, &rover.g2.proximity, update, 200, 200, 27),
85 : #endif
86 : SCHED_TASK_CLASS(AP_WindVane, &rover.g2.windvane, update, 20, 100, 30),
87 : SCHED_TASK(update_wheel_encoder, 50, 200, 36),
88 : SCHED_TASK(update_compass, 10, 200, 39),
89 : #if HAL_LOGGING_ENABLED
90 : SCHED_TASK(update_logging1, 10, 200, 45),
91 : SCHED_TASK(update_logging2, 10, 200, 48),
92 : #endif
93 : SCHED_TASK_CLASS(GCS, (GCS*)&rover._gcs, update_receive, 400, 500, 51),
94 : SCHED_TASK_CLASS(GCS, (GCS*)&rover._gcs, update_send, 400, 1000, 54),
95 : SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&rover.g2.rc_channels, read_mode_switch, 7, 200, 57),
96 : SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&rover.g2.rc_channels, read_aux_all, 10, 200, 60),
97 : SCHED_TASK_CLASS(AP_BattMonitor, &rover.battery, read, 10, 300, 63),
98 : #if AP_SERVORELAYEVENTS_ENABLED
99 : SCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events, 50, 200, 66),
100 : #endif
101 : #if AC_PRECLAND_ENABLED
102 : SCHED_TASK(update_precland, 400, 50, 70),
103 : #endif
104 : #if HAL_MOUNT_ENABLED
105 : SCHED_TASK_CLASS(AP_Mount, &rover.camera_mount, update, 50, 200, 75),
106 : #endif
107 : #if AP_CAMERA_ENABLED
108 : SCHED_TASK_CLASS(AP_Camera, &rover.camera, update, 50, 200, 78),
109 : #endif
110 : SCHED_TASK(gcs_failsafe_check, 10, 200, 81),
111 : #if AP_FENCE_ENABLED
112 : SCHED_TASK(fence_check, 10, 200, 84),
113 : #endif
114 : SCHED_TASK(ekf_check, 10, 100, 87),
115 : SCHED_TASK_CLASS(ModeSmartRTL, &rover.mode_smartrtl, save_position, 3, 200, 90),
116 : SCHED_TASK(one_second_loop, 1, 1500, 96),
117 : #if HAL_SPRAYER_ENABLED
118 : SCHED_TASK_CLASS(AC_Sprayer, &rover.g2.sprayer, update, 3, 90, 99),
119 : #endif
120 : #if HAL_LOGGING_ENABLED
121 : SCHED_TASK_CLASS(AP_Logger, &rover.logger, periodic_tasks, 50, 300, 108),
122 : #endif
123 : SCHED_TASK_CLASS(AP_InertialSensor, &rover.ins, periodic, 400, 200, 111),
124 : #if HAL_LOGGING_ENABLED
125 : SCHED_TASK_CLASS(AP_Scheduler, &rover.scheduler, update_logging, 0.1, 200, 114),
126 : #endif
127 : #if HAL_BUTTON_ENABLED
128 : SCHED_TASK_CLASS(AP_Button, &rover.button, update, 5, 200, 117),
129 : #endif
130 : SCHED_TASK(crash_check, 10, 200, 123),
131 : SCHED_TASK(cruise_learn_update, 50, 200, 126),
132 : #if AP_ROVER_ADVANCED_FAILSAFE_ENABLED
133 : SCHED_TASK(afs_fs_check, 10, 200, 129),
134 : #endif
135 : };
136 :
137 :
138 285 : void Rover::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
139 : uint8_t &task_count,
140 : uint32_t &log_bit)
141 : {
142 285 : tasks = &scheduler_tasks[0];
143 285 : task_count = ARRAY_SIZE(scheduler_tasks);
144 285 : log_bit = MASK_LOG_PM;
145 285 : }
146 :
147 : constexpr int8_t Rover::_failsafe_priorities[7];
148 :
149 285 : Rover::Rover(void) :
150 : AP_Vehicle(),
151 285 : param_loader(var_info),
152 285 : control_mode(&mode_initializing)
153 : {
154 285 : }
155 :
156 : #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
157 : // set target location (for use by external control and scripting)
158 0 : bool Rover::set_target_location(const Location& target_loc)
159 : {
160 : // exit if vehicle is not in Guided mode or Auto-Guided mode
161 0 : if (!control_mode->in_guided_mode()) {
162 0 : return false;
163 : }
164 :
165 0 : return mode_guided.set_desired_location(target_loc);
166 : }
167 : #endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
168 :
169 : #if AP_SCRIPTING_ENABLED
170 : // set target velocity (for use by scripting)
171 0 : bool Rover::set_target_velocity_NED(const Vector3f& vel_ned_ms, bool align_yaw_to_target)
172 : {
173 : // exit if vehicle is not in Guided mode or Auto-Guided mode
174 0 : if (!control_mode->in_guided_mode()) {
175 0 : return false;
176 : }
177 :
178 : // convert vector length into speed
179 0 : const float target_speed_ms = safe_sqrt(sq(vel_ned_ms.x) + sq(vel_ned_ms.y));
180 :
181 : // convert vector direction to target yaw
182 0 : const float target_yaw_cd = degrees(atan2f(vel_ned_ms.y, vel_ned_ms.x)) * 100.0f;
183 :
184 : // send target heading and speed
185 0 : mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_speed_ms);
186 :
187 0 : return true;
188 : }
189 :
190 : // set steering and throttle (-1 to +1) (for use by scripting)
191 20 : bool Rover::set_steering_and_throttle(float steering, float throttle)
192 : {
193 : // exit if vehicle is not in Guided mode or Auto-Guided mode
194 20 : if (!control_mode->in_guided_mode()) {
195 0 : return false;
196 : }
197 :
198 : // set steering and throttle
199 20 : mode_guided.set_steering_and_throttle(steering, throttle);
200 20 : return true;
201 : }
202 :
203 : // get steering and throttle (-1 to +1) (for use by scripting)
204 0 : bool Rover::get_steering_and_throttle(float& steering, float& throttle)
205 : {
206 0 : steering = g2.motors.get_steering() / 4500.0;
207 0 : throttle = g2.motors.get_throttle() * 0.01;
208 0 : return true;
209 : }
210 :
211 : // set desired turn rate (degrees/sec) and speed (m/s). Used for scripting
212 0 : bool Rover::set_desired_turn_rate_and_speed(float turn_rate_degs, float speed_ms)
213 : {
214 : // exit if vehicle is not in Guided mode or Auto-Guided mode
215 0 : if (!control_mode->in_guided_mode()) {
216 0 : return false;
217 : }
218 :
219 : // set turn rate and speed. Turn rate is expected in centidegrees/s and speed in meters/s
220 0 : mode_guided.set_desired_turn_rate_and_speed(turn_rate_degs * 100.0f, speed_ms);
221 0 : return true;
222 : }
223 :
224 : // set desired nav speed (m/s). Used for scripting.
225 0 : bool Rover::set_desired_speed(float speed_ms)
226 : {
227 0 : return control_mode->set_desired_speed(speed_ms);
228 : }
229 :
230 : // get control output (for use in scripting)
231 : // returns true on success and control_value is set to a value in the range -1 to +1
232 0 : bool Rover::get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value)
233 : {
234 0 : switch (control_output) {
235 0 : case AP_Vehicle::ControlOutput::Roll:
236 0 : control_value = constrain_float(g2.motors.get_roll(), -1.0f, 1.0f);
237 0 : return true;
238 0 : case AP_Vehicle::ControlOutput::Pitch:
239 0 : control_value = constrain_float(g2.motors.get_pitch(), -1.0f, 1.0f);
240 0 : return true;
241 0 : case AP_Vehicle::ControlOutput::Walking_Height:
242 0 : control_value = constrain_float(g2.motors.get_walking_height(), -1.0f, 1.0f);
243 0 : return true;
244 0 : case AP_Vehicle::ControlOutput::Throttle:
245 0 : control_value = constrain_float(g2.motors.get_throttle() * 0.01f, -1.0f, 1.0f);
246 0 : return true;
247 0 : case AP_Vehicle::ControlOutput::Yaw:
248 0 : control_value = constrain_float(g2.motors.get_steering() / 4500.0f, -1.0f, 1.0f);
249 0 : return true;
250 0 : case AP_Vehicle::ControlOutput::Lateral:
251 0 : control_value = constrain_float(g2.motors.get_lateral() * 0.01f, -1.0f, 1.0f);
252 0 : return true;
253 0 : case AP_Vehicle::ControlOutput::MainSail:
254 0 : control_value = constrain_float(g2.motors.get_mainsail() * 0.01f, -1.0f, 1.0f);
255 0 : return true;
256 0 : case AP_Vehicle::ControlOutput::WingSail:
257 0 : control_value = constrain_float(g2.motors.get_wingsail() * 0.01f, -1.0f, 1.0f);
258 0 : return true;
259 0 : default:
260 0 : return false;
261 : }
262 : return false;
263 : }
264 :
265 : // returns true if mode supports NAV_SCRIPT_TIME mission commands
266 0 : bool Rover::nav_scripting_enable(uint8_t mode)
267 : {
268 0 : return mode == (uint8_t)mode_auto.mode_number();
269 : }
270 :
271 : // lua scripts use this to retrieve the contents of the active command
272 0 : bool Rover::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4)
273 : {
274 0 : if (control_mode != &mode_auto) {
275 0 : return false;
276 : }
277 :
278 0 : return mode_auto.nav_script_time(id, cmd, arg1, arg2, arg3, arg4);
279 : }
280 :
281 : // lua scripts use this to indicate when they have complete the command
282 0 : void Rover::nav_script_time_done(uint16_t id)
283 : {
284 0 : if (control_mode != &mode_auto) {
285 0 : return;
286 : }
287 :
288 0 : return mode_auto.nav_script_time_done(id);
289 : }
290 : #endif // AP_SCRIPTING_ENABLED
291 :
292 : // update AHRS system
293 629048 : void Rover::ahrs_update()
294 : {
295 629048 : arming.update_soft_armed();
296 :
297 : // AHRS may use movement to calculate heading
298 629048 : update_ahrs_flyforward();
299 :
300 629048 : ahrs.update();
301 :
302 : // update position
303 629048 : have_position = ahrs.get_location(current_loc);
304 :
305 : // set home from EKF if necessary and possible
306 629048 : if (!ahrs.home_is_set()) {
307 68168 : if (!set_home_to_current_location(false)) {
308 : // ignore this failure
309 : }
310 : }
311 :
312 : // if using the EKF get a speed update now (from accelerometers)
313 629048 : Vector3f velocity;
314 629048 : if (ahrs.get_velocity_NED(velocity)) {
315 589379 : ground_speed = velocity.xy().length();
316 39669 : } else if (gps.status() >= AP_GPS_FixType::FIX_3D) {
317 0 : ground_speed = ahrs.groundspeed();
318 : }
319 :
320 : #if AP_FOLLOW_ENABLED
321 629048 : g2.follow.update_estimates();
322 : #endif
323 :
324 : #if HAL_LOGGING_ENABLED
325 629048 : if (should_log(MASK_LOG_ATTITUDE_FAST)) {
326 600310 : Log_Write_Attitude();
327 600310 : Log_Write_Sail();
328 : }
329 :
330 629048 : if (should_log(MASK_LOG_IMU)) {
331 599570 : AP::ins().Write_IMU();
332 : }
333 :
334 629048 : if (should_log(MASK_LOG_VIDEO_STABILISATION)) {
335 0 : ahrs.write_video_stabilisation();
336 : }
337 : #endif
338 629048 : }
339 :
340 : /*
341 : check for GCS failsafe - 10Hz
342 : */
343 125720 : void Rover::gcs_failsafe_check(void)
344 : {
345 125720 : if (g.fs_gcs_enabled == FS_GCS_DISABLED) {
346 : // gcs failsafe disabled
347 123710 : return;
348 : }
349 :
350 2010 : const uint32_t gcs_last_seen_ms = gcs().sysid_mygcs_last_seen_time_ms();
351 2010 : if (gcs_last_seen_ms == 0) {
352 : // we've never seen the GCS, so we never failsafe for not seeing it
353 0 : return;
354 : }
355 :
356 : // calc time since last gcs update
357 : // note: this only looks at the heartbeat from the device ids approved by gcs().sysid_is_gcs()
358 2010 : const uint32_t last_gcs_update_ms = millis() - gcs_last_seen_ms;
359 2010 : const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
360 :
361 2010 : const bool do_failsafe = last_gcs_update_ms >= gcs_timeout_ms ? true : false;
362 :
363 2010 : failsafe_trigger(FAILSAFE_EVENT_GCS, "GCS", do_failsafe);
364 : }
365 :
366 : #if HAL_LOGGING_ENABLED
367 : /*
368 : log some key data - 10Hz
369 : */
370 125720 : void Rover::update_logging1(void)
371 : {
372 125720 : if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
373 0 : Log_Write_Attitude();
374 0 : Log_Write_Sail();
375 : }
376 :
377 125720 : if (should_log(MASK_LOG_THR)) {
378 119840 : Log_Write_Throttle();
379 : #if AP_BEACON_ENABLED
380 119840 : beacon.log();
381 : #endif // AP_BEACON_ENABLED
382 : }
383 :
384 125720 : if (should_log(MASK_LOG_NTUN)) {
385 119840 : Log_Write_Nav_Tuning();
386 119840 : if (g2.pos_control.is_active()) {
387 35761 : g2.pos_control.write_log();
388 35761 : logger.Write_PID(LOG_PIDN_MSG, g2.pos_control.get_vel_pid().get_pid_info_x());
389 35761 : logger.Write_PID(LOG_PIDE_MSG, g2.pos_control.get_vel_pid().get_pid_info_y());
390 : }
391 : }
392 :
393 : #if HAL_PROXIMITY_ENABLED
394 125720 : if (should_log(MASK_LOG_RANGEFINDER)) {
395 119840 : g2.proximity.log();
396 : }
397 : #endif
398 125720 : }
399 :
400 : /*
401 : log some key data - 10Hz
402 : */
403 125720 : void Rover::update_logging2(void)
404 : {
405 125720 : if (should_log(MASK_LOG_STEERING)) {
406 119840 : Log_Write_Steering();
407 : }
408 :
409 125720 : if (should_log(MASK_LOG_RC)) {
410 119840 : Log_Write_RC();
411 119840 : g2.wheel_encoder.Log_Write();
412 : }
413 :
414 125720 : if (should_log(MASK_LOG_IMU)) {
415 119840 : AP::ins().Write_Vibration();
416 : #if HAL_GYROFFT_ENABLED
417 119840 : gyro_fft.write_log_messages();
418 : #endif
419 : }
420 : #if HAL_MOUNT_ENABLED
421 125720 : if (should_log(MASK_LOG_CAMERA)) {
422 119840 : camera_mount.write_log();
423 : }
424 : #endif
425 125720 : }
426 : #endif // HAL_LOGGING_ENABLED
427 :
428 : #if AP_ROVER_AUTO_ARM_ONCE_ENABLED
429 12526 : void Rover::handle_auto_arm_once()
430 : {
431 12526 : if (arming.is_armed()) {
432 : // never re-arm automatically if the user ever armed the vehicle
433 5012 : auto_arm_once.done = true;
434 5012 : return;
435 : }
436 7514 : if (auto_arm_once.done) {
437 7221 : return;
438 : }
439 293 : switch (arming.arming_required()) {
440 282 : case AP_Arming::Required::NO:
441 : case AP_Arming::Required::YES_MIN_PWM:
442 : case AP_Arming::Required::YES_ZERO_PWM:
443 : // in case the user changes the require parameter at runtime,
444 : // don't auto-arm:
445 282 : auto_arm_once.done = true;
446 282 : return;
447 11 : case AP_Arming::Required::YES_AUTO_ARM_MIN_PWM:
448 : case AP_Arming::Required::YES_AUTO_ARM_ZERO_PWM:
449 11 : break;
450 : }
451 :
452 : // don't try to arm if prearms are not passing:
453 11 : if (!arming.get_last_prearm_checks_result()) {
454 10 : return;
455 : }
456 :
457 1 : const uint32_t now_ms = AP_HAL::millis();
458 : // only attempt to auto arm once per 5 seconds:
459 1 : if (now_ms - auto_arm_once.last_arm_attempt_ms < 5000) {
460 0 : return;
461 : }
462 1 : auto_arm_once.last_arm_attempt_ms = now_ms;
463 :
464 1 : if (!arming.arm(AP_Arming::Method::AUTO_ARM_ONCE)) {
465 0 : return;
466 : }
467 :
468 1 : auto_arm_once.done = true;
469 : }
470 : #endif // AP_ROVER_AUTO_ARM_ONCE_ENABLED
471 :
472 : /*
473 : once a second events
474 : */
475 12526 : void Rover::one_second_loop(void)
476 : {
477 12526 : set_control_channels();
478 :
479 : // cope with changes to aux functions
480 12526 : AP::srv().enable_aux_servos();
481 :
482 : // update notify flags
483 12526 : AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
484 12526 : AP_Notify::flags.pre_arm_gps_check = true;
485 12526 : AP_Notify::flags.armed = arming.is_armed();
486 12526 : AP_Notify::flags.flying = arming.is_armed_and_safety_off();
487 :
488 : #if AP_ROVER_AUTO_ARM_ONCE_ENABLED
489 12526 : handle_auto_arm_once();
490 : #endif // AP_ROVER_AUTO_ARM_ONCE_ENABLED
491 :
492 : // attempt to update home position and baro calibration if not armed:
493 12526 : if (!arming.is_armed_and_safety_off()) {
494 7527 : update_home();
495 : }
496 :
497 : // need to set "likely flying" when armed to allow for compass
498 : // learning to run
499 12526 : set_likely_flying(arming.is_armed_and_safety_off());
500 :
501 : // send latest param values to wp_nav
502 12526 : g2.wp_nav.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering());
503 12526 : g2.pos_control.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering());
504 12526 : g2.wheel_rate_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
505 :
506 : #if AP_STATS_ENABLED
507 : // Update stats "flying" time
508 12526 : AP::stats()->set_flying(g2.motors.active());
509 : #endif
510 12526 : }
511 :
512 629048 : void Rover::update_current_mode(void)
513 : {
514 : // check for emergency stop
515 629048 : if (SRV_Channels::get_emergency_stop()) {
516 : // relax controllers, motor stopping done at output level
517 417 : g2.attitude_control.relax_I();
518 : }
519 :
520 629048 : control_mode->update();
521 629048 : }
522 :
523 : // vehicle specific waypoint info helpers
524 12526 : bool Rover::get_wp_distance_m(float &distance) const
525 : {
526 : // see GCS_MAVLINK_Rover::send_nav_controller_output()
527 12526 : if (!rover.control_mode->is_autopilot_mode()) {
528 7395 : return false;
529 : }
530 5131 : distance = control_mode->get_distance_to_destination();
531 5131 : return true;
532 : }
533 :
534 : // vehicle specific waypoint info helpers
535 5131 : bool Rover::get_wp_bearing_deg(float &bearing) const
536 : {
537 : // see GCS_MAVLINK_Rover::send_nav_controller_output()
538 5131 : if (!rover.control_mode->is_autopilot_mode()) {
539 0 : return false;
540 : }
541 5131 : bearing = control_mode->wp_bearing();
542 5131 : return true;
543 : }
544 :
545 : // vehicle specific waypoint info helpers
546 5131 : bool Rover::get_wp_crosstrack_error_m(float &xtrack_error) const
547 : {
548 : // see GCS_MAVLINK_Rover::send_nav_controller_output()
549 5131 : if (!rover.control_mode->is_autopilot_mode()) {
550 0 : return false;
551 : }
552 5131 : xtrack_error = control_mode->crosstrack_error_m();
553 5131 : return true;
554 : }
555 :
556 :
557 : Rover rover;
558 : AP_Vehicle& vehicle = rover;
559 :
560 285 : AP_HAL_MAIN_CALLBACKS(&rover);
|
