fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 14:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: add missing uORB publication of tunes

Browse Source 
The old tune device interface is not working anymore and we need to
publish to uORB tune_control.

This solution is not optimal though because blocks the receiving thread.
This commit is contained in:
Julian Oes
2020-04-28 08:45:39 +02:00
committed by Beat Küng
parent 48cf38d623
commit 18b39545ab
2 changed files with 30 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+28 -11
View File
@@ -47,6 +47,7 @@
#include <drivers/drv_tone_alarm.h>
#include <ecl/geo/geo.h>
#include <systemlib/px4_macros.h>
#include <lib/tunes/tunes.h>
#include <math.h>
#include <poll.h>
@@ -1758,20 +1759,36 @@ MavlinkReceiver::handle_message_play_tune(mavlink_message_t *msg)
mavlink_play_tune_t play_tune;
mavlink_msg_play_tune_decode(msg, &play_tune);
char *tune = play_tune.tune;
if ((mavlink_system.sysid == play_tune.target_system || play_tune.target_system == 0) &&
(mavlink_system.compid == play_tune.target_component || play_tune.target_component == 0)) {
if ((mavlink_system.sysid == play_tune.target_system ||
play_tune.target_system == 0) &&
(mavlink_system.compid == play_tune.target_component ||
play_tune.target_component == 0)) {
Tunes tunes;
if (*tune == 'M') {
int fd = px4_open(TONE_ALARM0_DEVICE_PATH, PX4_F_WRONLY);
tune_control_s tune_control {};
tune_control.tune_id = 0;
tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
if (fd >= 0) {
px4_write(fd, tune, strlen(tune) + 1);
px4_close(fd);
}
// Let's make sure the input is 0 terminated and we don't ever overrun it.
play_tune.tune2[sizeof(play_tune.tune2) - 1] = '\0';
tunes.set_string(play_tune.tune, tune_control.volume);
unsigned frequency;
unsigned duration;
unsigned silence;
uint8_t volume;
while (tunes.get_next_note(frequency, duration, silence, volume) > 0) {
tune_control.tune_id = 0;
tune_control.frequency = (uint16_t)frequency;
tune_control.duration = (uint32_t)duration;
tune_control.silence = (uint32_t)silence;
tune_control.volume = (uint8_t)volume;
tune_control.timestamp = hrt_absolute_time();
_tune_control_pub.publish(tune_control);
// FIXME: this blocks this receiver thread
px4_usleep(duration + silence);
}
}
}
src/modules/mavlink/mavlink_receiver.h
+2
View File
@@ -85,6 +85,7 @@
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/tune_control.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
@@ -266,6 +267,7 @@ private:
uORB::PublicationQueued<transponder_report_s> _transponder_report_pub{ORB_ID(transponder_report)};
uORB::PublicationQueued<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::PublicationQueued<vehicle_command_s> _cmd_pub{ORB_ID(vehicle_command)};
uORB::PublicationQueued<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
// ORB subscriptions
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};