Line data Source code
1 : /*
2 : Lead developers: Matthew Ridley and Andrew Tridgell
3 :
4 : Please contribute your ideas! See https://ardupilot.club/dev for details
5 :
6 : This program is free software: you can redistribute it and/or modify
7 : it under the terms of the GNU General Public License as published by
8 : the Free Software Foundation, either version 3 of the License, or
9 : (at your option) any later version.
10 :
11 : This program is distributed in the hope that it will be useful,
12 : but WITHOUT ANY WARRANTY; without even the implied warranty of
13 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 : GNU General Public License for more details.
15 :
16 : You should have received a copy of the GNU General Public License
17 : along with this program. If not, see <http://www.gnu.org/licenses/>.
18 : */
19 :
20 : #include "Tracker.h"
21 :
22 : #define FORCE_VERSION_H_INCLUDE
23 : #include "version.h"
24 : #undef FORCE_VERSION_H_INCLUDE
25 :
26 : #define SCHED_TASK(func, rate_hz, _max_time_micros, _prio) SCHED_TASK_CLASS(Tracker, &tracker, func, rate_hz, _max_time_micros, _prio)
27 :
28 : /*
29 : All entries in this table must be ordered by priority.
30 :
31 : This table is interleaved with the table in AP_Vehicle to determine
32 : the order in which tasks are run. Convenience methods SCHED_TASK
33 : and SCHED_TASK_CLASS are provided to build entries in this structure:
34 :
35 : SCHED_TASK arguments:
36 : - name of static function to call
37 : - rate (in Hertz) at which the function should be called
38 : - expected time (in MicroSeconds) that the function should take to run
39 : - priority (0 through 255, lower number meaning higher priority)
40 :
41 : SCHED_TASK_CLASS arguments:
42 : - class name of method to be called
43 : - instance on which to call the method
44 : - method to call on that instance
45 : - rate (in Hertz) at which the method should be called
46 : - expected time (in MicroSeconds) that the method should take to run
47 : - priority (0 through 255, lower number meaning higher priority)
48 :
49 : */
50 : const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
51 : SCHED_TASK(update_ahrs, 50, 1000, 5),
52 : SCHED_TASK(read_radio, 50, 200, 10),
53 : SCHED_TASK(update_tracking, 50, 1000, 15),
54 : SCHED_TASK(update_GPS, 10, 4000, 20),
55 : SCHED_TASK(update_compass, 10, 1500, 25),
56 : SCHED_TASK_CLASS(AP_BattMonitor, &tracker.battery, read, 10, 1500, 35),
57 : SCHED_TASK_CLASS(AP_Baro, &tracker.barometer, update, 10, 1500, 40),
58 : SCHED_TASK_CLASS(GCS, (GCS*)&tracker._gcs, update_receive, 50, 1700, 45),
59 : SCHED_TASK_CLASS(GCS, (GCS*)&tracker._gcs, update_send, 50, 3000, 50),
60 : #if HAL_LOGGING_ENABLED
61 : SCHED_TASK(ten_hz_logging_loop, 10, 300, 60),
62 : SCHED_TASK_CLASS(AP_Logger, &tracker.logger, periodic_tasks, 50, 300, 65),
63 : #endif
64 : SCHED_TASK_CLASS(AP_InertialSensor, &tracker.ins, periodic, 50, 50, 70),
65 : SCHED_TASK(one_second_loop, 1, 3900, 80),
66 : SCHED_TASK(stats_update, 1, 200, 90),
67 : };
68 :
69 33 : void Tracker::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
70 : uint8_t &task_count,
71 : uint32_t &log_bit)
72 : {
73 33 : tasks = &scheduler_tasks[0];
74 33 : task_count = ARRAY_SIZE(scheduler_tasks);
75 33 : log_bit = (uint32_t)-1;
76 33 : }
77 :
78 1702 : void Tracker::one_second_loop()
79 : {
80 : // update assigned functions and enable auxiliary servos
81 1702 : AP::srv().enable_aux_servos();
82 :
83 : // updated armed/disarmed status LEDs
84 1702 : update_armed_disarmed();
85 :
86 1702 : if (!ahrs.home_is_set()) {
87 : // set home to current location
88 539 : Location temp_loc;
89 539 : if (ahrs.get_location(temp_loc)) {
90 273 : if (!set_home(temp_loc, false)) {
91 : // fail silently
92 : }
93 : }
94 : }
95 :
96 : // need to set "likely flying" when armed to allow for compass
97 : // learning to run
98 1702 : set_likely_flying(hal.util->get_soft_armed());
99 :
100 1702 : AP_Notify::flags.flying = hal.util->get_soft_armed();
101 :
102 1702 : g.pidYaw2Srv.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
103 1702 : }
104 :
105 : #if HAL_LOGGING_ENABLED
106 17294 : void Tracker::ten_hz_logging_loop()
107 : {
108 17294 : if (should_log(MASK_LOG_IMU)) {
109 17294 : AP::ins().Write_IMU();
110 : }
111 17294 : if (should_log(MASK_LOG_ATTITUDE)) {
112 17294 : Log_Write_Attitude();
113 : }
114 17294 : if (should_log(MASK_LOG_RCIN)) {
115 17294 : logger.Write_RCIN();
116 : }
117 17294 : if (should_log(MASK_LOG_RCOUT)) {
118 17294 : logger.Write_RCOUT();
119 : }
120 17294 : }
121 : #endif
122 :
123 32 : Mode *Tracker::mode_from_mode_num(const Mode::Number num)
124 : {
125 32 : Mode *ret = nullptr;
126 32 : switch (num) {
127 0 : case Mode::Number::MANUAL:
128 0 : ret = &mode_manual;
129 0 : break;
130 1 : case Mode::Number::STOP:
131 1 : ret = &mode_stop;
132 1 : break;
133 0 : case Mode::Number::SCAN:
134 0 : ret = &mode_scan;
135 0 : break;
136 0 : case Mode::Number::GUIDED:
137 0 : ret = &mode_guided;
138 0 : break;
139 0 : case Mode::Number::SERVOTEST:
140 0 : ret = &mode_servotest;
141 0 : break;
142 31 : case Mode::Number::AUTO:
143 31 : ret = &mode_auto;
144 31 : break;
145 0 : case Mode::Number::INITIALISING:
146 0 : ret = &mode_initialising;
147 0 : break;
148 : }
149 32 : return ret;
150 : }
151 :
152 : /*
153 : update AP_Stats
154 : */
155 1702 : void Tracker::stats_update(void)
156 : {
157 1702 : AP::stats()->set_flying(hal.util->get_soft_armed());
158 1702 : }
159 :
160 : const AP_HAL::HAL& hal = AP_HAL::get_HAL();
161 :
162 : Tracker tracker;
163 : AP_Vehicle& vehicle = tracker;
164 :
165 33 : AP_HAL_MAIN_CALLBACKS(&tracker);
|
