Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 16:56:25 +08:00
Code Issues Actions 4 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-27 17:06:06 +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/CMakeLists.txt
+1
View File
@@ -58,6 +58,7 @@ px4_add_module(
mavlink_stream.cpp
mavlink_ulog.cpp
mavlink_timesync.cpp
tune_publisher.cpp
MODULE_CONFIG
module.yaml
DEPENDS
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);
}
}
}
src/modules/mavlink/mavlink_receiver.h
+2 -8
View File
@@ -46,12 +46,12 @@
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
#include "mavlink_timesync.h"
#include "tune_publisher.h"
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/tunes/tunes.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
@@ -210,7 +210,6 @@ private:
void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);
void schedule_tune(const char *tune);
void send_next_tune(hrt_abstime now);
/**
* @brief Updates the battery, optical flow, and flight ID subscribed parameters.
@@ -272,7 +271,6 @@ 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)};
@@ -300,12 +298,8 @@ private:
hrt_abstime _last_utm_global_pos_com{0};
static constexpr unsigned MAX_TUNE_LEN {248};
// Allocated if needed.
Tunes *_tunes {nullptr};
char *_tune_buffer {nullptr};
hrt_abstime _next_tune_time {0};
TunePublisher *_tune_publisher{nullptr};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
src/modules/mavlink/tune_publisher.cpp
+94
View File
@@ -0,0 +1,94 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "tune_publisher.h"
#include "string.h"
#include <px4_platform_common/log.h>
void TunePublisher::set_tune_string(const char *tune, const hrt_abstime &now)
{
// 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;
}
strncpy(_tune_buffer, tune, MAX_TUNE_LEN);
_tunes.set_string(_tune_buffer, tune_control_s::VOLUME_LEVEL_DEFAULT);
_next_publish_time = now;
}
void TunePublisher::publish_next_tune(const hrt_abstime now)
{
if (_next_publish_time == 0) {
// Nothing to play.
return;
}
if (now < _next_publish_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 = now;
_tune_control_pub.publish(tune_control);
_next_publish_time = now + duration + silence;
} else {
// We're done, let's reset.
_next_publish_time = 0;
}
}
src/modules/mavlink/tune_publisher.h
+55
View File
@@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <uORB/PublicationQueued.hpp>
#include <drivers/drv_hrt.h>
#include <lib/tunes/tunes.h>
class TunePublisher
{
public:
void set_tune_string(const char *tune, const hrt_abstime &now);
void publish_next_tune(const hrt_abstime now);
private:
static constexpr unsigned MAX_TUNE_LEN {248};
Tunes _tunes {};
char _tune_buffer[MAX_TUNE_LEN] {0};
hrt_abstime _next_publish_time {0};
uORB::PublicationQueued<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
};