Line data Source code
1 : /*
2 : * AP_ToneAlarm driver
3 : */
4 : /*
5 : * This program is free software: you can redistribute it and/or modify
6 : * it under the terms of the GNU General Public License as published by
7 : * the Free Software Foundation, either version 3 of the License, or
8 : * (at your option) any later version.
9 : *
10 : * This program is distributed in the hope that it will be useful,
11 : * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 : * GNU General Public License for more details.
14 : *
15 : * You should have received a copy of the GNU General Public License
16 : * along with this program. If not, see <http://www.gnu.org/licenses/>.
17 : */
18 :
19 : #include <AP_HAL/AP_HAL.h>
20 : #include <AP_Math/AP_Math.h>
21 :
22 : #include "ToneAlarm.h"
23 : #include "AP_Notify.h"
24 :
25 : #include <string.h>
26 :
27 : #include <stdio.h>
28 :
29 : #include <AP_Filesystem/AP_Filesystem.h>
30 :
31 : extern const AP_HAL::HAL& hal;
32 :
33 : // Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY
34 : // statement, with some exceptions and extensions.
35 : // See https://firmware.ardupilot.club/Tools/ToneTester/
36 : const AP_ToneAlarm::Tone AP_ToneAlarm::_tones[] {
37 : #define AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK 0
38 : { "MFT200L4<<<B#A#2", false },
39 : #define AP_NOTIFY_TONE_LOUD_NEG_FEEDBACK 1
40 : { "MFT100L4>B#A#2P8B#A#2", false },
41 : #define AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK 2
42 : { "MFT200L4<B#", false },
43 : #define AP_NOTIFY_TONE_LOUD_NEU_FEEDBACK 3
44 : { "MFT100L4>B#", false },
45 : #define AP_NOTIFY_TONE_QUIET_POS_FEEDBACK 4
46 : { "MFT200L4<A#B#", false },
47 : #define AP_NOTIFY_TONE_LOUD_POS_FEEDBACK 5
48 : { "MFT100L4>A#B#", false },
49 : #define AP_NOTIFY_TONE_LOUD_READY_OR_FINISHED 6
50 : { "MFT100L4>G#6A#6B#4", false },
51 : #define AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED 7
52 : { "MFT200L4<G#6A#6B#4", false },
53 : #define AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED 8
54 : { "MFT100L4>A#A#A#A#", false },
55 : #define AP_NOTIFY_TONE_QUIET_ARMING_WARNING 9
56 : { "MNT75L1O2G", false },
57 : #define AP_NOTIFY_TONE_LOUD_WP_COMPLETE 10
58 : { "MFT200L8G>C3", false },
59 : #define AP_NOTIFY_TONE_LOUD_LAND_WARNING_CTS 11
60 : { "MBT200L2A-G-A-G-A-G-", true },
61 : #define AP_NOTIFY_TONE_LOUD_VEHICLE_LOST_CTS 12
62 : { "MBT200>A#1", true },
63 : #define AP_NOTIFY_TONE_LOUD_BATTERY_ALERT_CTS 13
64 : { "MBNT255>A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8", true },
65 : #define AP_NOTIFY_TONE_QUIET_CALIBRATING_CTS 14
66 : { "MBNT255<C16P2", true },
67 : #define AP_NOTIFY_TONE_WAITING_FOR_THROW 15
68 : { "MBNT90L4O2A#O3DFN0N0N0", true},
69 : #define AP_NOTIFY_TONE_LOUD_1 16
70 : { "MFT100L8>B", false},
71 : #define AP_NOTIFY_TONE_LOUD_2 17
72 : { "MFT100L8>BB", false},
73 : #define AP_NOTIFY_TONE_LOUD_3 18
74 : { "MFT100L8>BBB", false},
75 : #define AP_NOTIFY_TONE_LOUD_4 19
76 : { "MFT100L8>BBBB", false},
77 : #define AP_NOTIFY_TONE_LOUD_5 20
78 : { "MFT100L8>BBBBB", false},
79 : #define AP_NOTIFY_TONE_LOUD_6 21
80 : { "MFT100L8>BBBBBB", false},
81 : #define AP_NOTIFY_TONE_LOUD_7 22
82 : { "MFT100L8>BBBBBBB", false},
83 : #define AP_NOTIFY_TONE_TUNING_START 23
84 : { "MFT100L20>C#D#", false},
85 : #define AP_NOTIFY_TONE_TUNING_SAVE 24
86 : { "MFT100L10DBDB>", false},
87 : #define AP_NOTIFY_TONE_TUNING_ERROR 25
88 : { "MFT100L10>BBBBBBBB", false},
89 : #define AP_NOTIFY_TONE_LEAK_DETECTED 26
90 : { "MBT255L8>A+AA-", true},
91 : #define AP_NOTIFY_TONE_QUIET_SHUTDOWN 27
92 : { "MFMST200L32O3ceP32cdP32ceP32c<c>c<cccP8L32>c>c<P32<c<c", false },
93 : #define AP_NOTIFY_TONE_QUIET_NOT_READY_OR_NOT_FINISHED 28
94 : { "MFT200L4<B#4A#6G#6", false },
95 : #define AP_NOTIFY_TONE_STARTUP 29
96 : { "MFT240L8O4aO5dcO4aO5dcO4aO5dcL16dcdcdcdc", false },
97 : #define AP_NOTIFY_TONE_NO_SDCARD 30
98 : { "MNBGG", false },
99 : #define AP_NOTIFY_TONE_EKF_ALERT 31
100 : { "MBNT255>A#8A#8A#8A#8P8A#8A#8A#8A#8P8A#8A#8A#8A#8P8A#8A#8A#8A#8", true },
101 : };
102 :
103 2094 : bool AP_ToneAlarm::init()
104 : {
105 2094 : if (pNotify->buzzer_enabled() == false) {
106 0 : return false;
107 : }
108 : #if AP_NOTIFY_TONEALARM_ENABLED
109 2094 : if (!hal.util->toneAlarm_init(pNotify->get_buzzer_types())) {
110 2094 : return false;
111 : }
112 : #endif
113 :
114 :
115 : // set initial boot states. This prevents us issuing a arming
116 : // warning in plane and rover on every boot
117 0 : flags.armed = AP_Notify::flags.armed;
118 0 : flags.failsafe_battery = AP_Notify::flags.failsafe_battery;
119 0 : flags.pre_arm_check = 1;
120 0 : _cont_tone_playing = -1;
121 0 : hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_ToneAlarm::_timer_task, void));
122 :
123 : #if (AP_FILESYSTEM_POSIX_ENABLED || AP_FILESYSTEM_FATFS_ENABLED) && CONFIG_HAL_BOARD != HAL_BOARD_LINUX
124 : // if we don't have a SDcard then play a failure tone instead of
125 : // normal startup tone. This gives the user a chance to fix it
126 : // before they try to arm. We don't do this on Linux as Linux
127 : // flight controllers don't usually have removable storage
128 : struct stat st;
129 0 : if (AP::FS().stat(HAL_BOARD_STORAGE_DIRECTORY, &st) != 0) {
130 0 : play_tone(AP_NOTIFY_TONE_NO_SDCARD);
131 0 : return true;
132 : }
133 : #endif
134 :
135 : #ifndef HAL_BUILD_AP_PERIPH
136 0 : play_tone(AP_NOTIFY_TONE_STARTUP);
137 : #endif
138 0 : return true;
139 : }
140 :
141 : // play_tune - play one of the pre-defined tunes
142 0 : void AP_ToneAlarm::play_tone(const uint8_t tone_index)
143 : {
144 0 : uint32_t tnow_ms = AP_HAL::millis();
145 0 : const Tone &tone_requested = _tones[tone_index];
146 :
147 0 : if (tone_requested.continuous) {
148 0 : _cont_tone_playing = tone_index;
149 : }
150 :
151 0 : _tone_playing = tone_index;
152 0 : _tone_beginning_ms = tnow_ms;
153 :
154 0 : play_tune(tone_requested.str);
155 0 : }
156 :
157 0 : void AP_ToneAlarm::_timer_task()
158 : {
159 0 : WITH_SEMAPHORE(_sem);
160 0 : _mml_player.update();
161 0 : }
162 :
163 0 : void AP_ToneAlarm::play_tune(const char *str)
164 : {
165 0 : WITH_SEMAPHORE(_sem);
166 :
167 0 : _mml_player.stop();
168 0 : strncpy(_tone_buf, str, AP_NOTIFY_TONEALARM_TONE_BUF_SIZE);
169 0 : _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE-1] = 0;
170 0 : _mml_player.play(_tone_buf);
171 0 : }
172 :
173 0 : void AP_ToneAlarm::stop_cont_tone()
174 : {
175 0 : if (_cont_tone_playing == _tone_playing) {
176 0 : play_tune("");
177 0 : _tone_playing = -1;
178 : }
179 0 : _cont_tone_playing = -1;
180 0 : }
181 :
182 0 : void AP_ToneAlarm::check_cont_tone()
183 : {
184 0 : uint32_t tnow_ms = AP_HAL::millis();
185 : // if we are supposed to be playing a continuous tone,
186 : // and it was interrupted, and the interrupting tone has timed out,
187 : // resume the continuous tone
188 :
189 0 : if (_cont_tone_playing != -1 && _tone_playing != _cont_tone_playing && tnow_ms-_tone_beginning_ms > AP_NOTIFY_TONEALARM_MAX_TONE_LENGTH_MS) {
190 0 : play_tone(_cont_tone_playing);
191 : }
192 0 : }
193 :
194 : // update - updates led according to timed_updated. Should be called at 50Hz
195 0 : void AP_ToneAlarm::update()
196 : {
197 : // exit if buzzer is not enabled
198 0 : if (pNotify->buzzer_enabled() == false) {
199 0 : return;
200 : }
201 :
202 0 : check_cont_tone();
203 :
204 0 : if (AP_Notify::flags.powering_off) {
205 0 : if (!flags.powering_off) {
206 0 : play_tone(AP_NOTIFY_TONE_QUIET_SHUTDOWN);
207 : }
208 0 : flags.powering_off = AP_Notify::flags.powering_off;
209 0 : return;
210 : }
211 :
212 0 : if (AP_Notify::flags.compass_cal_running != flags.compass_cal_running) {
213 0 : if (AP_Notify::flags.compass_cal_running) {
214 0 : play_tone(AP_NOTIFY_TONE_QUIET_CALIBRATING_CTS);
215 0 : play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK);
216 : } else {
217 0 : if (_cont_tone_playing == AP_NOTIFY_TONE_QUIET_CALIBRATING_CTS) {
218 0 : stop_cont_tone();
219 : }
220 : }
221 : }
222 0 : flags.compass_cal_running = AP_Notify::flags.compass_cal_running;
223 :
224 0 : if (AP_Notify::events.compass_cal_canceled) {
225 0 : play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK);
226 0 : return;
227 : }
228 :
229 0 : if (AP_Notify::events.initiated_compass_cal) {
230 0 : play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK);
231 0 : return;
232 : }
233 :
234 0 : if (AP_Notify::events.compass_cal_saved ||
235 0 : AP_Notify::events.temp_cal_saved) {
236 0 : play_tone(AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED);
237 0 : return;
238 : }
239 :
240 0 : if (AP_Notify::events.compass_cal_failed ||
241 0 : AP_Notify::events.temp_cal_failed) {
242 0 : play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
243 0 : return;
244 : }
245 :
246 0 : if (AP_Notify::events.initiated_temp_cal) {
247 0 : play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK);
248 0 : return;
249 : }
250 :
251 0 : if (AP_Notify::flags.temp_cal_running != flags.temp_cal_running) {
252 0 : if (AP_Notify::flags.temp_cal_running) {
253 0 : play_tone(AP_NOTIFY_TONE_QUIET_CALIBRATING_CTS);
254 0 : play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK);
255 : } else {
256 0 : if (_cont_tone_playing == AP_NOTIFY_TONE_QUIET_CALIBRATING_CTS) {
257 0 : stop_cont_tone();
258 : }
259 : }
260 : }
261 0 : flags.temp_cal_running = AP_Notify::flags.temp_cal_running;
262 :
263 : // don't play other tones if cal is running
264 0 : if (AP_Notify::flags.compass_cal_running ||
265 0 : AP_Notify::flags.temp_cal_running) {
266 0 : return;
267 : }
268 :
269 : // notify the user when autotune or mission completes
270 0 : if (AP_Notify::flags.armed && (AP_Notify::events.autotune_complete || AP_Notify::events.mission_complete)) {
271 0 : play_tone(AP_NOTIFY_TONE_LOUD_READY_OR_FINISHED);
272 : }
273 :
274 : //notify the user when autotune fails
275 0 : if (AP_Notify::flags.armed && (AP_Notify::events.autotune_failed)) {
276 0 : play_tone(AP_NOTIFY_TONE_LOUD_NEG_FEEDBACK);
277 : }
278 :
279 : // notify the user when a waypoint completes
280 0 : if (AP_Notify::events.waypoint_complete) {
281 0 : play_tone(AP_NOTIFY_TONE_LOUD_WP_COMPLETE);
282 : }
283 :
284 : // notify the user when their mode change was successful
285 0 : if (AP_Notify::events.user_mode_change) {
286 0 : if (AP_Notify::flags.armed) {
287 0 : play_tone(AP_NOTIFY_TONE_LOUD_NEU_FEEDBACK);
288 : } else {
289 0 : play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK);
290 : }
291 : }
292 :
293 : // notify the user when their mode change failed
294 0 : if (AP_Notify::events.user_mode_change_failed) {
295 0 : if (AP_Notify::flags.armed) {
296 0 : play_tone(AP_NOTIFY_TONE_LOUD_NEG_FEEDBACK);
297 : } else {
298 0 : play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
299 : }
300 : }
301 :
302 : // failsafe initiated mode change
303 0 : if (AP_Notify::events.failsafe_mode_change) {
304 0 : play_tone(AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED);
305 : }
306 :
307 : // notify the user when arming fails
308 0 : if (AP_Notify::events.arming_failed) {
309 0 : play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
310 : }
311 :
312 : // notify the user when RC contact is lost
313 0 : if (flags.failsafe_radio != AP_Notify::flags.failsafe_radio) {
314 0 : flags.failsafe_radio = AP_Notify::flags.failsafe_radio;
315 0 : if (flags.failsafe_radio) {
316 : // armed case handled by events.failsafe_mode_change
317 0 : if (!AP_Notify::flags.armed) {
318 0 : play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
319 : }
320 : } else {
321 0 : if (AP_Notify::flags.armed) {
322 0 : play_tone(AP_NOTIFY_TONE_LOUD_POS_FEEDBACK);
323 : } else {
324 0 : play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK);
325 : }
326 : }
327 : }
328 :
329 : // notify the user when GCS failsafe is triggered
330 0 : if (flags.failsafe_gcs != AP_Notify::flags.failsafe_gcs) {
331 0 : flags.failsafe_gcs = AP_Notify::flags.failsafe_gcs;
332 0 : if (flags.failsafe_gcs) {
333 : // armed case handled by events.failsafe_mode_change
334 0 : if (!AP_Notify::flags.armed) {
335 0 : play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
336 : }
337 : } else {
338 0 : if (AP_Notify::flags.armed) {
339 0 : play_tone(AP_NOTIFY_TONE_LOUD_POS_FEEDBACK);
340 : } else {
341 0 : play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK);
342 : }
343 : }
344 : }
345 :
346 : // notify the user when pre_arm checks are passing
347 0 : if (flags.pre_arm_check != AP_Notify::flags.pre_arm_check) {
348 0 : flags.pre_arm_check = AP_Notify::flags.pre_arm_check;
349 0 : if (flags.pre_arm_check) {
350 0 : play_tone(AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED);
351 0 : _have_played_ready_tone = true;
352 : } else {
353 : // only play sad tone if we've ever played happy tone:
354 0 : if (_have_played_ready_tone) {
355 0 : play_tone(AP_NOTIFY_TONE_QUIET_NOT_READY_OR_NOT_FINISHED);
356 : }
357 : }
358 : }
359 :
360 : // check if arming status has changed
361 0 : if (flags.armed != AP_Notify::flags.armed) {
362 0 : flags.armed = AP_Notify::flags.armed;
363 0 : if (flags.armed) {
364 : // arming tune
365 0 : play_tone(AP_NOTIFY_TONE_QUIET_ARMING_WARNING);
366 : } else {
367 : // disarming tune
368 0 : play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK);
369 0 : if (!flags.leak_detected) {
370 0 : stop_cont_tone();
371 : }
372 : }
373 : }
374 :
375 : // check if battery status has changed
376 0 : if (flags.failsafe_battery != AP_Notify::flags.failsafe_battery) {
377 0 : flags.failsafe_battery = AP_Notify::flags.failsafe_battery;
378 0 : if (flags.failsafe_battery) {
379 : // battery warning tune
380 0 : play_tone(AP_NOTIFY_TONE_LOUD_BATTERY_ALERT_CTS);
381 : }
382 : }
383 :
384 : // check parachute release
385 0 : if (flags.parachute_release != AP_Notify::flags.parachute_release) {
386 0 : flags.parachute_release = AP_Notify::flags.parachute_release;
387 0 : if (flags.parachute_release) {
388 : // parachute release warning tune
389 0 : play_tone(AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED);
390 : }
391 : }
392 :
393 : // lost vehicle tone
394 0 : if (flags.vehicle_lost != AP_Notify::flags.vehicle_lost) {
395 0 : flags.vehicle_lost = AP_Notify::flags.vehicle_lost;
396 0 : if (flags.vehicle_lost) {
397 0 : play_tone(AP_NOTIFY_TONE_LOUD_VEHICLE_LOST_CTS);
398 : } else {
399 0 : stop_cont_tone();
400 : }
401 : }
402 :
403 : // waiting to be thrown vehicle tone
404 0 : if (flags.waiting_for_throw != AP_Notify::flags.waiting_for_throw) {
405 0 : flags.waiting_for_throw = AP_Notify::flags.waiting_for_throw;
406 0 : if (flags.waiting_for_throw) {
407 0 : play_tone(AP_NOTIFY_TONE_WAITING_FOR_THROW);
408 : } else {
409 0 : stop_cont_tone();
410 : }
411 : }
412 :
413 0 : if (flags.leak_detected != AP_Notify::flags.leak_detected) {
414 0 : flags.leak_detected = AP_Notify::flags.leak_detected;
415 0 : if (flags.leak_detected) {
416 0 : play_tone(AP_NOTIFY_TONE_LEAK_DETECTED);
417 : } else {
418 0 : stop_cont_tone();
419 : }
420 : }
421 :
422 0 : if (AP_Notify::events.tune_started) {
423 0 : play_tone(AP_NOTIFY_TONE_TUNING_START);
424 0 : AP_Notify::events.tune_started = 0;
425 : }
426 0 : if (AP_Notify::events.tune_next) {
427 : // signify which parameter in the set is starting
428 0 : play_tone(AP_NOTIFY_TONE_LOUD_1 + (AP_Notify::events.tune_next-1));
429 0 : AP_Notify::events.tune_next = 0;
430 : }
431 0 : if (AP_Notify::events.tune_save) {
432 0 : play_tone(AP_NOTIFY_TONE_TUNING_SAVE);
433 0 : AP_Notify::events.tune_save = 0;
434 : }
435 0 : if (AP_Notify::events.tune_error) {
436 0 : play_tone(AP_NOTIFY_TONE_TUNING_ERROR);
437 0 : AP_Notify::events.tune_error = 0;
438 : }
439 :
440 : // notify the user when ekf failsafe is triggered
441 0 : if (flags.failsafe_ekf != AP_Notify::flags.failsafe_ekf) {
442 0 : flags.failsafe_ekf = AP_Notify::flags.failsafe_ekf;
443 0 : if (flags.failsafe_ekf) {
444 0 : play_tone(AP_NOTIFY_TONE_EKF_ALERT);
445 : }
446 : }
447 : }
448 :
449 :
450 : #if AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED
451 : /*
452 : * handle a PLAY_TUNE message
453 : */
454 0 : void AP_Notify::handle_play_tune(const mavlink_message_t &msg)
455 : {
456 : // decode mavlink message
457 : mavlink_play_tune_t packet;
458 :
459 0 : mavlink_msg_play_tune_decode(&msg, &packet);
460 :
461 0 : char _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE] {}; // ~100 bytes
462 0 : strncpy(_tone_buf, packet.tune, MIN(sizeof(packet.tune), sizeof(_tone_buf)-1));
463 0 : uint8_t len = strlen(_tone_buf);
464 0 : uint8_t len2 = strnlen(packet.tune2, sizeof(packet.tune2));
465 0 : len2 = MIN((sizeof(_tone_buf)-1)-len, len2);
466 0 : memcpy(_tone_buf+len, packet.tune2, len2); // not strncpy to avoid truncation warning
467 0 : _tone_buf[sizeof(_tone_buf)-1] = 0;
468 :
469 0 : play_tune(_tone_buf);
470 0 : }
471 : #endif
|
