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 : #include "Sub.h"
17 :
18 : #define FORCE_VERSION_H_INCLUDE
19 : #include "version.h"
20 : #undef FORCE_VERSION_H_INCLUDE
21 :
22 : const AP_HAL::HAL& hal = AP_HAL::get_HAL();
23 :
24 : /*
25 : constructor for main Sub class
26 : */
27 83 : Sub::Sub()
28 : :
29 :
30 : #if !AP_SUB_RC_ENABLED
31 : control_mode(Mode::Number::MANUAL),
32 : #endif
33 83 : motors(MAIN_LOOP_RATE),
34 83 : auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
35 83 : ahrs_view(ahrs, ROTATION_NONE),
36 83 : attitude_control(ahrs_view, motors),
37 83 : pos_control(ahrs_view, motors, attitude_control),
38 83 : wp_nav(ahrs_view, pos_control, attitude_control),
39 83 : loiter_nav(ahrs_view, pos_control, attitude_control),
40 83 : circle_nav(ahrs_view, pos_control),
41 83 : param_loader(var_info),
42 83 : flightmode(&mode_manual),
43 83 : auto_mode(Auto_WP),
44 581 : guided_mode(Guided_WP)
45 : {
46 83 : failsafe.pilot_input = true;
47 83 : if (_singleton != nullptr) {
48 0 : AP_HAL::panic("Can only be one Sub");
49 : }
50 83 : _singleton = this;
51 83 : }
52 :
53 : #define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Sub, &sub, func, rate_hz, max_time_micros, priority)
54 : #define FAST_TASK(func) FAST_TASK_CLASS(Sub, &sub, func)
55 :
56 : /*
57 : scheduler table - all tasks should be listed here.
58 :
59 : All entries in this table must be ordered by priority.
60 :
61 : This table is interleaved with the table in AP_Vehicle to determine
62 : the order in which tasks are run. Convenience methods SCHED_TASK
63 : and SCHED_TASK_CLASS are provided to build entries in this structure:
64 :
65 : SCHED_TASK arguments:
66 : - name of static function to call
67 : - rate (in Hertz) at which the function should be called
68 : - expected time (in MicroSeconds) that the function should take to run
69 : - priority (0 through 255, lower number meaning higher priority)
70 :
71 : SCHED_TASK_CLASS arguments:
72 : - class name of method to be called
73 : - instance on which to call the method
74 : - method to call on that instance
75 : - rate (in Hertz) at which the method should be called
76 : - expected time (in MicroSeconds) that the method should take to run
77 : - priority (0 through 255, lower number meaning higher priority)
78 :
79 : */
80 :
81 : const AP_Scheduler::Task Sub::scheduler_tasks[] = {
82 : // update INS immediately to get current gyro data populated
83 : FAST_TASK_CLASS(AP_InertialSensor, &sub.ins, update),
84 : // run low level rate controllers that only require IMU data
85 : FAST_TASK(run_rate_controller),
86 : // send outputs to the motors library immediately
87 : FAST_TASK(motors_output),
88 : // run EKF state estimator (expensive)
89 : FAST_TASK(read_AHRS),
90 : // Inertial Nav
91 : FAST_TASK(read_inertia),
92 : // check if ekf has reset target heading
93 : FAST_TASK(check_ekf_yaw_reset),
94 : // run the attitude controllers
95 : FAST_TASK(update_flight_mode),
96 : // update home from EKF if necessary
97 : FAST_TASK(update_home_from_EKF),
98 : // check if we've reached the surface or bottom
99 : FAST_TASK(update_surface_and_bottom_detector),
100 : #if HAL_MOUNT_ENABLED
101 : // camera mount's fast update
102 : FAST_TASK_CLASS(AP_Mount, &sub.camera_mount, update_fast),
103 : #endif
104 :
105 : SCHED_TASK(fifty_hz_loop, 50, 75, 3),
106 : #if AP_SUB_RC_ENABLED
107 : SCHED_TASK(rc_loop, 50, 130, 3),
108 : #endif
109 : SCHED_TASK_CLASS(AP_GPS, &sub.gps, update, 50, 200, 6),
110 : #if AP_OPTICALFLOW_ENABLED
111 : SCHED_TASK_CLASS(AP_OpticalFlow, &sub.optflow, update, 200, 160, 9),
112 : #endif
113 : SCHED_TASK(update_batt_compass, 10, 120, 12),
114 : SCHED_TASK(read_rangefinder, 20, 100, 15),
115 : SCHED_TASK(update_altitude, 10, 100, 18),
116 : #if AP_SUB_RC_ENABLED
117 : SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&sub.g2.rc_channels, read_aux_all, 10, 50, 18),
118 : #endif
119 : SCHED_TASK(three_hz_loop, 3, 75, 21),
120 : SCHED_TASK(update_turn_counter, 10, 50, 24),
121 : SCHED_TASK(one_hz_loop, 1, 100, 33),
122 : SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_receive, 400, 180, 36),
123 : SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_send, 400, 550, 39),
124 : #if HAL_MOUNT_ENABLED
125 : SCHED_TASK_CLASS(AP_Mount, &sub.camera_mount, update, 50, 75, 45),
126 : #endif
127 : #if AP_CAMERA_ENABLED
128 : SCHED_TASK_CLASS(AP_Camera, &sub.camera, update, 50, 75, 48),
129 : #endif
130 : #if HAL_LOGGING_ENABLED
131 : SCHED_TASK(ten_hz_logging_loop, 10, 350, 51),
132 : SCHED_TASK(twentyfive_hz_logging, 25, 110, 54),
133 : SCHED_TASK(loop_rate_logging, LOOP_RATE, 50, 55),
134 : SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300, 57),
135 : #endif
136 : SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50, 60),
137 : #if HAL_LOGGING_ENABLED
138 : SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75, 63),
139 : #endif
140 : SCHED_TASK(terrain_update, 10, 100, 72),
141 : #if AP_STATS_ENABLED
142 : SCHED_TASK(stats_update, 1, 200, 76),
143 : #endif
144 : #ifdef USERHOOK_FASTLOOP
145 : SCHED_TASK(userhook_FastLoop, 100, 75, 78),
146 : #endif
147 : #ifdef USERHOOK_50HZLOOP
148 : SCHED_TASK(userhook_50Hz, 50, 75, 81),
149 : #endif
150 : #ifdef USERHOOK_MEDIUMLOOP
151 : SCHED_TASK(userhook_MediumLoop, 10, 75, 84),
152 : #endif
153 : #ifdef USERHOOK_SLOWLOOP
154 : SCHED_TASK(userhook_SlowLoop, 3.3, 75, 87),
155 : #endif
156 : #ifdef USERHOOK_SUPERSLOWLOOP
157 : SCHED_TASK(userhook_SuperSlowLoop, 1, 75, 90),
158 : #endif
159 :
160 : };
161 :
162 83 : void Sub::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
163 : uint8_t &task_count,
164 : uint32_t &log_bit)
165 : {
166 83 : tasks = &scheduler_tasks[0];
167 83 : task_count = ARRAY_SIZE(scheduler_tasks);
168 83 : log_bit = MASK_LOG_PM;
169 83 : }
170 :
171 : constexpr int8_t Sub::_failsafe_priorities[5];
172 :
173 2180683 : void Sub::run_rate_controller()
174 : {
175 2180683 : const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
176 2180683 : motors.set_dt_s(last_loop_time_s);
177 2180683 : attitude_control.set_dt_s(last_loop_time_s);
178 2180683 : pos_control.set_dt_s(last_loop_time_s);
179 :
180 : //don't run rate controller in manual or motordetection modes
181 2180683 : if (control_mode != Mode::Number::MANUAL && control_mode != Mode::Number::MOTOR_DETECT) {
182 : // run low level rate controllers that only require IMU data and set loop time
183 1134713 : attitude_control.rate_controller_run();
184 : }
185 2180683 : }
186 :
187 : // 50 Hz tasks
188 2180683 : void Sub::fifty_hz_loop()
189 : {
190 : // check pilot input failsafe
191 2180683 : failsafe_pilot_input_check();
192 :
193 2180683 : failsafe_crash_check();
194 :
195 2180683 : failsafe_ekf_check();
196 :
197 2180683 : failsafe_sensors_check();
198 : #if !AP_SUB_RC_ENABLED
199 : rc().read_input();
200 : #endif
201 2180683 : g2.actuators.update_actuators();
202 2180683 : }
203 :
204 : // update_batt_compass - read battery and compass
205 : // should be called at 10hz
206 54502 : void Sub::update_batt_compass()
207 : {
208 : // read battery before compass because it may be used for motor interference compensation
209 54502 : battery.read();
210 :
211 54502 : if (AP::compass().available()) {
212 : // update compass with throttle value - used for compassmot
213 54502 : compass.set_throttle(motors.get_throttle());
214 54502 : compass.read();
215 : }
216 54502 : }
217 :
218 : #if HAL_LOGGING_ENABLED
219 : // ten_hz_logging_loop
220 : // should be run at 10hz
221 54479 : void Sub::ten_hz_logging_loop()
222 : {
223 : // log attitude data if we're not already logging at the higher rate
224 54479 : if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
225 52321 : Log_Write_Attitude();
226 52321 : attitude_control.Write_ANG();
227 52321 : attitude_control.Write_Rate(pos_control);
228 52321 : if (should_log(MASK_LOG_PID)) {
229 52321 : logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
230 52321 : logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
231 52321 : logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
232 52321 : logger.Write_PID(LOG_PIDA_MSG, pos_control.D_get_accel_pid().get_pid_info());
233 : }
234 : }
235 54479 : if (should_log(MASK_LOG_MOTBATT)) {
236 52321 : motors.Log_Write();
237 : }
238 54479 : if (should_log(MASK_LOG_RCIN)) {
239 52321 : logger.Write_RCIN();
240 : }
241 54479 : if (should_log(MASK_LOG_RCOUT)) {
242 52321 : logger.Write_RCOUT();
243 : }
244 54479 : if (should_log(MASK_LOG_NTUN) && (sub.flightmode->requires_GPS() || sub.flightmode->requires_altitude())) {
245 27291 : pos_control.write_log();
246 : }
247 54479 : if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
248 52321 : AP::ins().Write_Vibration();
249 : }
250 : #if HAL_MOUNT_ENABLED
251 54479 : if (should_log(MASK_LOG_CAMERA)) {
252 52321 : camera_mount.write_log();
253 : }
254 : #endif
255 54479 : }
256 :
257 : // twentyfive_hz_logging_loop
258 : // should be run at 25hz
259 136233 : void Sub::twentyfive_hz_logging()
260 : {
261 136233 : if (should_log(MASK_LOG_ATTITUDE_FAST)) {
262 0 : Log_Write_Attitude();
263 0 : attitude_control.Write_ANG();
264 0 : attitude_control.Write_Rate(pos_control);
265 0 : if (should_log(MASK_LOG_PID)) {
266 0 : logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
267 0 : logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
268 0 : logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
269 0 : logger.Write_PID(LOG_PIDA_MSG, pos_control.D_get_accel_pid().get_pid_info());
270 : }
271 : }
272 :
273 : // log IMU data if we're not already logging at the higher rate
274 136233 : if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_FAST)) {
275 130836 : AP::ins().Write_IMU();
276 : }
277 136233 : }
278 :
279 : // Full rate logging of IMU
280 2180614 : void Sub::loop_rate_logging()
281 : {
282 2180614 : if (should_log(MASK_LOG_IMU_FAST)) {
283 0 : AP::ins().Write_IMU();
284 : }
285 2180614 : }
286 : #endif // HAL_LOGGING_ENABLED
287 :
288 : // three_hz_loop - 3.3hz loop
289 16343 : void Sub::three_hz_loop()
290 : {
291 16343 : leak_detector.update();
292 :
293 16343 : failsafe_leak_check();
294 :
295 16343 : failsafe_internal_pressure_check();
296 :
297 16343 : failsafe_internal_temperature_check();
298 :
299 : // check if we've lost contact with the ground station
300 16343 : failsafe_gcs_check();
301 :
302 : // check if we've lost terrain data
303 16343 : failsafe_terrain_check();
304 :
305 : #if AP_SERVORELAYEVENTS_ENABLED
306 16343 : ServoRelayEvents.update_events();
307 : #endif
308 16343 : }
309 :
310 : // one_hz_loop - runs at 1Hz
311 5409 : void Sub::one_hz_loop()
312 : {
313 5409 : bool arm_check = arming.pre_arm_checks(false);
314 5409 : ap.pre_arm_check = arm_check;
315 5409 : AP_Notify::flags.pre_arm_check = arm_check;
316 5409 : AP_Notify::flags.pre_arm_gps_check = position_ok();
317 5409 : AP_Notify::flags.flying = motors.armed();
318 :
319 : #if HAL_LOGGING_ENABLED
320 5409 : if (should_log(MASK_LOG_ANY)) {
321 5198 : Log_Write_Data(LogDataID::AP_STATE, ap.value);
322 : }
323 : #endif
324 :
325 5409 : if (!motors.armed()) {
326 2319 : motors.update_throttle_range();
327 : }
328 :
329 : // update assigned functions and enable auxiliary servos
330 5409 : AP::srv().enable_aux_servos();
331 :
332 : #if HAL_LOGGING_ENABLED
333 : // log terrain data
334 5409 : terrain_logging();
335 : #endif
336 :
337 : // need to set "likely flying" when armed to allow for compass
338 : // learning to run
339 5409 : set_likely_flying(hal.util->get_soft_armed());
340 :
341 5409 : attitude_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
342 5409 : pos_control.D_get_accel_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
343 5409 : }
344 :
345 2180683 : void Sub::read_AHRS()
346 : {
347 : // Perform IMU calculations and get attitude info
348 : //-----------------------------------------------
349 : // <true> tells AHRS to skip INS update as we have already done it in fast_loop()
350 2180683 : ahrs.update(true);
351 2180683 : ahrs_view.update();
352 2180683 : }
353 :
354 : // read baro and rangefinder altitude at 10hz
355 54502 : void Sub::update_altitude()
356 : {
357 : // read in baro altitude
358 54502 : read_barometer();
359 :
360 : #if HAL_LOGGING_ENABLED
361 54502 : if (should_log(MASK_LOG_CTUN)) {
362 52342 : Log_Write_Control_Tuning();
363 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
364 52342 : AP::ins().write_notch_log_messages();
365 : #endif
366 : #if HAL_GYROFFT_ENABLED
367 52342 : gyro_fft.write_log_messages();
368 : #endif
369 : }
370 : #endif // HAL_LOGGING_ENABLED
371 54502 : }
372 :
373 55 : bool Sub::control_check_barometer()
374 : {
375 55 : if (!ap.depth_sensor_present) { // can't hold depth without a depth sensor
376 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Depth sensor is not connected.");
377 0 : return false;
378 55 : } else if (failsafe.sensor_health) {
379 1 : gcs().send_text(MAV_SEVERITY_WARNING, "Depth sensor error.");
380 1 : return false;
381 : }
382 54 : return true;
383 : }
384 :
385 : // vehicle specific waypoint info helpers
386 5409 : bool Sub::get_wp_distance_m(float &distance) const
387 : {
388 : // see GCS_MAVLINK_Sub::send_nav_controller_output()
389 5409 : distance = sub.wp_nav.get_wp_distance_to_destination_cm() * 0.01;
390 5409 : return true;
391 : }
392 :
393 : // vehicle specific waypoint info helpers
394 5409 : bool Sub::get_wp_bearing_deg(float &bearing) const
395 : {
396 : // see GCS_MAVLINK_Sub::send_nav_controller_output()
397 5409 : bearing = sub.wp_nav.get_wp_bearing_to_destination_cd() * 0.01;
398 5409 : return true;
399 : }
400 :
401 : // vehicle specific waypoint info helpers
402 5409 : bool Sub::get_wp_crosstrack_error_m(float &xtrack_error) const
403 : {
404 : // no crosstrack error reported, see GCS_MAVLINK_Sub::send_nav_controller_output()
405 5409 : xtrack_error = 0;
406 5409 : return true;
407 : }
408 :
409 : #if AP_STATS_ENABLED
410 : /*
411 : update AP_Stats
412 : */
413 5409 : void Sub::stats_update(void)
414 : {
415 5409 : AP::stats()->set_flying(motors.armed());
416 5409 : }
417 : #endif
418 :
419 : // get the altitude relative to the home position or the ekf origin
420 81587 : float Sub::get_alt_rel() const
421 : {
422 81587 : if (!ap.depth_sensor_present) {
423 72 : return 0;
424 : }
425 :
426 : // get relative position
427 : float posD;
428 81515 : ahrs.get_relative_position_D_home(posD);
429 81515 : return -posD;
430 : }
431 :
432 : // get the altitude above mean sea level
433 137539 : float Sub::get_alt_msl() const
434 : {
435 137539 : if (!ap.depth_sensor_present) {
436 239 : return 0;
437 : }
438 :
439 137300 : Location origin;
440 137300 : if (!ahrs.get_origin(origin)) {
441 16217 : return 0;
442 : }
443 :
444 : // get relative position
445 : float posD;
446 121083 : if (!ahrs.get_relative_position_D_origin_float(posD)) {
447 : // fall back to the barometer reading
448 20 : posD = -AP::baro().get_altitude();
449 : }
450 :
451 : // add in the ekf origin altitude
452 121083 : posD -= static_cast<float>(origin.alt) * 0.01f;
453 :
454 : // convert down to up
455 121083 : return -posD;
456 : }
457 :
458 : #if AP_SUB_RC_ENABLED
459 2180683 : void Sub::rc_loop()
460 : {
461 : // Read radio and 3-position switch on radio
462 : // -----------------------------------------
463 2180683 : read_radio();
464 2180683 : rc().read_mode_switch();
465 2180683 : }
466 : #endif
467 :
468 : #if AP_SCRIPTING_ENABLED
469 : // set target position, velocity and acceleration (for use by scripting)
470 1226 : bool Sub::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)
471 : {
472 : // exit if vehicle is not in Guided mode or Auto-Guided mode
473 1226 : if ((control_mode != Mode::Number::GUIDED) && !(control_mode == Mode::Number::AUTO && auto_mode == Auto_NavGuided)) {
474 0 : return false;
475 : }
476 :
477 1226 : const Vector3f pos_neu_cm(target_pos_ned_m.x * 100.0f, target_pos_ned_m.y * 100.0f, -target_pos_ned_m.z * 100.0f);
478 1226 : const Vector3f vel_neu_cms(target_vel_ned_ms.x * 100.0f, target_vel_ned_ms.y * 100.0f, -target_vel_ned_ms.z * 100.0f);
479 1226 : const Vector3f accel_neu_cmss(target_accel_ned_mss.x * 100.0f, target_accel_ned_mss.y * 100.0f, -target_accel_ned_mss.z * 100.0f);
480 :
481 1226 : return mode_guided.guided_set_posvelaccel(pos_neu_cm, vel_neu_cms, accel_neu_cmss, use_yaw, yaw_deg * 100.0f, use_yaw_rate, yaw_rate_degs * 100.0f, yaw_relative);
482 : }
483 : #endif
484 :
485 : Sub *Sub::_singleton = nullptr;
486 :
487 : Sub sub;
488 : AP_Vehicle& vehicle = sub;
489 :
490 83 : AP_HAL_MAIN_CALLBACKS(&sub);
|
