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 : Abstract Telemetry library
18 : */
19 :
20 : #include "AP_RCTelemetry.h"
21 :
22 : #include <AP_AHRS/AP_AHRS.h>
23 : #include <AP_Common/AP_FWVersion.h>
24 : #include <GCS_MAVLink/GCS.h>
25 : #include <stdio.h>
26 : #include <math.h>
27 : #include <AP_Vehicle/AP_Vehicle_Type.h>
28 : #include <AP_Baro/AP_Baro.h>
29 :
30 : #ifdef TELEM_DEBUG
31 : # define debug(fmt, args...) hal.console->printf("Telem: " fmt "\n", ##args)
32 : #else
33 : # define debug(fmt, args...) do {} while(0)
34 : #endif
35 :
36 : extern const AP_HAL::HAL& hal;
37 :
38 : /*
39 : setup ready for passthrough telem
40 : */
41 10 : bool AP_RCTelemetry::init(void)
42 : {
43 : #if HAL_GCS_ENABLED && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
44 : // make telemetry available to GCS_MAVLINK (used to queue statustext messages from GCS_MAVLINK)
45 : // add firmware and frame info to message queue
46 10 : const char* _frame_string = gcs().frame_string();
47 10 : if (_frame_string == nullptr) {
48 4 : queue_message(MAV_SEVERITY_INFO, AP::fwversion().fw_string);
49 : } else {
50 : char firmware_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
51 6 : snprintf(firmware_buf, sizeof(firmware_buf), "%s %s", AP::fwversion().fw_string, _frame_string);
52 6 : queue_message(MAV_SEVERITY_INFO, firmware_buf);
53 : }
54 : #endif
55 10 : setup_wfq_scheduler();
56 :
57 10 : return true;
58 : }
59 :
60 2717 : void AP_RCTelemetry::update_avg_packet_rate()
61 : {
62 2717 : uint32_t poll_now = AP_HAL::millis();
63 :
64 2717 : _scheduler.avg_packet_counter++;
65 :
66 2717 : if (poll_now - _scheduler.last_poll_timer > 1000) { //average in last 1000ms
67 : // initialize
68 97 : if (_scheduler.avg_packet_rate == 0) _scheduler.avg_packet_rate = _scheduler.avg_packet_counter;
69 : // moving average
70 97 : _scheduler.avg_packet_rate = (uint16_t)_scheduler.avg_packet_rate * 0.75f + _scheduler.avg_packet_counter * 0.25f;
71 : // reset
72 97 : _scheduler.last_poll_timer = poll_now;
73 97 : _scheduler.avg_packet_counter = 0;
74 : debug("avg packet rate %dHz, rates(Hz) %d %d %d %d %d %d %d %d", _scheduler.avg_packet_rate,
75 : _scheduler.packet_rate[0],
76 : _scheduler.packet_rate[1],
77 : _scheduler.packet_rate[2],
78 : _scheduler.packet_rate[3],
79 : _scheduler.packet_rate[4],
80 : _scheduler.packet_rate[5],
81 : _scheduler.packet_rate[6],
82 : _scheduler.packet_rate[7]);
83 : }
84 2717 : }
85 :
86 : /*
87 : * WFQ scheduler
88 : * returns the actual packet type index (if any) sent by the scheduler
89 : */
90 2717 : uint8_t AP_RCTelemetry::run_wfq_scheduler(const bool use_shaper)
91 : {
92 2717 : update_avg_packet_rate();
93 2717 : update_max_packet_rate();
94 :
95 2717 : uint32_t now = AP_HAL::millis();
96 2717 : int8_t max_delay_idx = -1;
97 :
98 2717 : float max_delay = 0;
99 2717 : float delay = 0;
100 2717 : bool packet_ready = false;
101 :
102 : // queue messages for any unhealthy sensors
103 2717 : check_sensor_status_flags();
104 : // build message queue for ekf_status
105 2717 : check_ekf_status();
106 :
107 : // dynamic priorities
108 : bool queue_empty;
109 : {
110 2717 : WITH_SEMAPHORE(_statustext.sem);
111 2717 : queue_empty = !_statustext.available && _statustext.queue.is_empty();
112 2717 : }
113 :
114 2717 : adjust_packet_weight(queue_empty);
115 :
116 : // search the packet with the longest delay after the scheduled time
117 39414 : for (int i=0; i<_time_slots; i++) {
118 : // normalize packet delay relative to packet weight
119 36697 : delay = (now - _scheduler.packet_timer[i])/static_cast<float>(_scheduler.packet_weight[i]);
120 : // use >= so with equal delays we choose the packet with lowest priority
121 : // this is ensured by the packets being sorted by desc frequency
122 : // apply the rate limiter
123 36697 : if (delay >= max_delay && check_scheduler_entry_time_constraints(now, i, use_shaper)) {
124 11296 : packet_ready = is_scheduler_entry_enabled(i) && is_packet_ready(i, queue_empty);
125 :
126 11296 : if (packet_ready) {
127 2166 : max_delay = delay;
128 2166 : max_delay_idx = i;
129 : }
130 : }
131 : }
132 2717 : if (max_delay_idx < 0) { // nothing was ready
133 1156 : return max_delay_idx;
134 : }
135 :
136 1561 : now = AP_HAL::millis();
137 : #ifdef TELEM_DEBUG
138 : _scheduler.packet_rate[max_delay_idx] = (_scheduler.packet_rate[max_delay_idx] + 1000 / (now - _scheduler.packet_timer[max_delay_idx])) / 2;
139 : #endif
140 1561 : _scheduler.packet_timer[max_delay_idx] = now;
141 : //debug("process_packet(%d): %f", max_delay_idx, max_delay);
142 : // send packet
143 1561 : process_packet(max_delay_idx);
144 : // let the caller know which packet type was sent
145 1561 : return max_delay_idx;
146 : }
147 :
148 : /*
149 : * do not run the scheduler and process a specific entry
150 : */
151 0 : bool AP_RCTelemetry::process_scheduler_entry(const uint8_t slot )
152 : {
153 0 : if (slot >= TELEM_TIME_SLOT_MAX) {
154 0 : return false;
155 : }
156 0 : if (!is_scheduler_entry_enabled(slot)) {
157 0 : return false;
158 : }
159 : bool queue_empty;
160 : {
161 0 : WITH_SEMAPHORE(_statustext.sem);
162 0 : queue_empty = !_statustext.available && _statustext.queue.is_empty();
163 0 : }
164 0 : if (!is_packet_ready(slot, queue_empty)) {
165 0 : return false;
166 : }
167 0 : process_packet(slot);
168 :
169 0 : return true;
170 : }
171 :
172 : /*
173 : * add message to message cue for transmission through link
174 : */
175 113 : void AP_RCTelemetry::queue_message(MAV_SEVERITY severity, const char *text)
176 : {
177 113 : mavlink_statustext_t statustext{};
178 :
179 113 : statustext.severity = severity;
180 113 : strncpy_noterm(statustext.text, text, sizeof(statustext.text));
181 :
182 : // The force push will ensure comm links do not block other comm links forever if they fail.
183 : // If we push to a full buffer then we overwrite the oldest entry, effectively removing the
184 : // block but not until the buffer fills up.
185 113 : WITH_SEMAPHORE(_statustext.sem);
186 113 : _statustext.queue.push_force(statustext);
187 113 : }
188 :
189 : /*
190 : * add unhealthy_sensors() information to message cue, normally passed as sys_status mavlink messages to the GCS, for transmission through FrSky link
191 : */
192 2717 : void AP_RCTelemetry::check_sensor_status_flags(void)
193 : {
194 2717 : uint32_t now = AP_HAL::millis();
195 :
196 2717 : const uint32_t _sensor_status_flags = unhealthy_sensors();
197 :
198 2717 : if ((now - check_sensor_status_timer) >= 5000) { // prevent repeating any system_status messages unless 5 seconds have passed
199 : // only one error is reported at a time (in order of preference). Same setup and displayed messages as Mission Planner.
200 1756 : if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_GPS) > 0) {
201 0 : queue_message(MAV_SEVERITY_CRITICAL, "Bad GPS Health");
202 0 : check_sensor_status_timer = now;
203 1756 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_GYRO) > 0) {
204 0 : queue_message(MAV_SEVERITY_CRITICAL, "Bad Gyro Health");
205 0 : check_sensor_status_timer = now;
206 1756 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_ACCEL) > 0) {
207 0 : queue_message(MAV_SEVERITY_CRITICAL, "Bad Accel Health");
208 0 : check_sensor_status_timer = now;
209 1756 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_MAG) > 0) {
210 0 : queue_message(MAV_SEVERITY_CRITICAL, "Bad Compass Health");
211 0 : check_sensor_status_timer = now;
212 1756 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) > 0) {
213 0 : queue_message(MAV_SEVERITY_CRITICAL, "Bad Baro Health");
214 0 : check_sensor_status_timer = now;
215 1756 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_LASER_POSITION) > 0) {
216 0 : queue_message(MAV_SEVERITY_CRITICAL, "Bad LiDAR Health");
217 0 : check_sensor_status_timer = now;
218 1756 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW) > 0) {
219 0 : queue_message(MAV_SEVERITY_CRITICAL, "Bad OptFlow Health");
220 0 : check_sensor_status_timer = now;
221 1756 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_TERRAIN) > 0) {
222 2 : queue_message(MAV_SEVERITY_CRITICAL, "Bad or No Terrain Data");
223 2 : check_sensor_status_timer = now;
224 1754 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_GEOFENCE) > 0) {
225 0 : queue_message(MAV_SEVERITY_CRITICAL, "Geofence Breach");
226 0 : check_sensor_status_timer = now;
227 1754 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_AHRS) > 0) {
228 0 : queue_message(MAV_SEVERITY_CRITICAL, "Bad AHRS");
229 0 : check_sensor_status_timer = now;
230 1754 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_RC_RECEIVER) > 0) {
231 0 : queue_message(MAV_SEVERITY_CRITICAL, "No RC Receiver");
232 0 : check_sensor_status_timer = now;
233 1754 : } else if ((_sensor_status_flags & MAV_SYS_STATUS_LOGGING) > 0) {
234 0 : queue_message(MAV_SEVERITY_CRITICAL, "Bad Logging");
235 0 : check_sensor_status_timer = now;
236 : }
237 : }
238 2717 : }
239 :
240 : /*
241 : * add innovation variance information to message cue, normally passed as ekf_status_report mavlink messages to the GCS, for transmission through FrSky link
242 : */
243 2717 : void AP_RCTelemetry::check_ekf_status(void)
244 : {
245 : // get variances
246 : bool get_variance;
247 : float velVar, posVar, hgtVar, tasVar;
248 2717 : Vector3f magVar;
249 : {
250 2717 : AP_AHRS &_ahrs = AP::ahrs();
251 2717 : WITH_SEMAPHORE(_ahrs.get_semaphore());
252 2717 : get_variance = _ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar);
253 2717 : }
254 2717 : if (get_variance) {
255 2423 : uint32_t now = AP_HAL::millis();
256 2423 : if ((now - check_ekf_status_timer) >= 10000) { // prevent repeating any ekf_status message unless 10 seconds have passed
257 : // multiple errors can be reported at a time. Same setup as Mission Planner.
258 1703 : if (velVar >= 0.8f) {
259 0 : queue_message(MAV_SEVERITY_CRITICAL, "Error velocity variance");
260 0 : check_ekf_status_timer = now;
261 : }
262 1703 : if (posVar >= 0.8f) {
263 0 : queue_message(MAV_SEVERITY_CRITICAL, "Error pos horiz variance");
264 0 : check_ekf_status_timer = now;
265 : }
266 1703 : if (hgtVar >= 0.8f) {
267 0 : queue_message(MAV_SEVERITY_CRITICAL, "Error pos vert variance");
268 0 : check_ekf_status_timer = now;
269 : }
270 1703 : if (magVar.length() >= 0.8f) {
271 0 : queue_message(MAV_SEVERITY_CRITICAL, "Error compass variance");
272 0 : check_ekf_status_timer = now;
273 : }
274 1703 : if (tasVar >= 0.8f) {
275 0 : queue_message(MAV_SEVERITY_CRITICAL, "Error terrain alt variance");
276 0 : check_ekf_status_timer = now;
277 : }
278 : }
279 : }
280 2717 : }
281 :
282 : // returns a bitmask of sensors which are present and enabled but also
283 : // not healthy
284 2717 : uint32_t AP_RCTelemetry::unhealthy_sensors() const
285 : {
286 : uint32_t present;
287 : uint32_t enabled;
288 : uint32_t health;
289 : #if HAL_GCS_ENABLED
290 2717 : gcs().get_sensor_status_flags(present, enabled, health);
291 : #else
292 : present = 0;
293 : enabled = 0;
294 : health = 0;
295 : #endif
296 :
297 2717 : return ~health & enabled & present;
298 : }
299 :
300 : /*
301 : * get vertical speed from ahrs, if not available fall back to baro climbrate, units is m/s
302 : */
303 146 : float AP_RCTelemetry::get_vspeed_ms(void)
304 : {
305 : {
306 : // release semaphore as soon as possible
307 146 : AP_AHRS &_ahrs = AP::ahrs();
308 146 : Vector3f v;
309 146 : WITH_SEMAPHORE(_ahrs.get_semaphore());
310 146 : if (_ahrs.get_velocity_NED(v)) {
311 100 : return -v.z;
312 : }
313 146 : }
314 46 : auto &_baro = AP::baro();
315 46 : WITH_SEMAPHORE(_baro.get_semaphore());
316 46 : return _baro.get_climb_rate();
317 46 : }
318 :
319 : /*
320 : * prepare altitude between vehicle and home location data
321 : */
322 124 : float AP_RCTelemetry::get_nav_alt_m(Location::AltFrame frame)
323 : {
324 124 : Location loc;
325 124 : float current_height = 0;
326 :
327 124 : AP_AHRS &_ahrs = AP::ahrs();
328 124 : WITH_SEMAPHORE(_ahrs.get_semaphore());
329 :
330 124 : if (frame == Location::AltFrame::ABOVE_HOME) {
331 0 : _ahrs.get_relative_position_D_home(current_height);
332 0 : return -current_height;
333 : }
334 :
335 124 : if (_ahrs.get_location(loc)) {
336 79 : if (!loc.get_alt_m(frame, current_height)) {
337 : // ignore this error
338 : }
339 : }
340 124 : return current_height;
341 124 : }
|
