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

mavlink: move tune publication to separate class

Browse Source 
This makes it easier to allocate in MavlinkReceiver.
This commit is contained in:
Julian Oes
2020-04-28 08:45:39 +02:00
committed by Beat Küng
parent 2d5184fcfe
commit 48c60d354d
5 changed files with 165 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+13 -75
View File
@@ -82,8 +82,7 @@ using matrix::wrap_2pi;
MavlinkReceiver::~MavlinkReceiver()
{
delete _tunes;
delete[] _tune_buffer;
delete _tune_publisher;
delete _px4_accel;
delete _px4_baro;
delete _px4_gyro;
@@ -1797,88 +1796,25 @@ MavlinkReceiver::handle_message_play_tune_v2(mavlink_message_t *msg)
void MavlinkReceiver::schedule_tune(const char *tune)
{
// The tune string needs to be 0 terminated.
const unsigned tune_len = strlen(tune);
// We don't expect the tune string to be longer than what can come in via MAVLink including 0 termination.
if (tune_len >= MAX_TUNE_LEN) {
PX4_ERR("Tune string too long.");
return;
}
// We only allocate the buffer and the tunes object if we ever use it but we
// We only allocate the TunePublisher object if we ever use it but we
// don't remove it to avoid fragmentation over time.
if (_tune_buffer == nullptr) {
_tune_buffer = new char[MAX_TUNE_LEN];
if (_tune_publisher == nullptr) {
_tune_publisher = new TunePublisher();
if (_tune_buffer == nullptr) {
PX4_ERR("Could not allocate tune buffer");
if (_tune_publisher == nullptr) {
PX4_ERR("Could not allocate tune publisher");
return;
}
}
if (_tunes == nullptr) {
_tunes = new Tunes();
if (_tunes == nullptr) {
PX4_ERR("Could not allocate tune");
return;
}
}
strncpy(_tune_buffer, tune, MAX_TUNE_LEN);
_tunes->set_string(_tune_buffer, tune_control_s::VOLUME_LEVEL_DEFAULT);
// Send first one straightaway.
const hrt_abstime now = hrt_absolute_time();
_next_tune_time = now;
send_next_tune(now);
_tune_publisher->set_tune_string(tune, now);
// Send first one straightaway.
_tune_publisher->publish_next_tune(now);
}
void MavlinkReceiver::send_next_tune(const hrt_abstime now)
{
if (_tune_buffer == nullptr || _tunes == nullptr) {
return;
}
if (_next_tune_time == 0) {
// Nothing to play.
return;
}
if (now < _next_tune_time) {
// Too early, try again later.
return;
}
unsigned frequency;
unsigned duration;
unsigned silence;
uint8_t volume;
if (_tunes->get_next_note(frequency, duration, silence, volume) > 0) {
tune_control_s tune_control {};
tune_control.tune_id = 0;
tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
tune_control.tune_id = 0;
tune_control.frequency = static_cast<uint16_t>(frequency);
tune_control.duration = static_cast<uint32_t>(duration);
tune_control.silence = static_cast<uint32_t>(silence);
tune_control.volume = static_cast<uint8_t>(volume);
tune_control.timestamp = hrt_absolute_time();
_tune_control_pub.publish(tune_control);
_next_tune_time = now + duration + silence;
} else {
// We're done, let's reset.
_next_tune_time = 0;
}
}
void
MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
{
@@ -3026,7 +2962,9 @@ MavlinkReceiver::Run()
last_send_update = t;
}
send_next_tune(t);
if (_tune_publisher != nullptr) {
_tune_publisher->publish_next_tune(t);
}
}
}