dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

introduce uORB::PublicationQueued and transition most orb_advertise_queue usage

This commit is contained in:
Daniel Agar 2019-07-03 16:30:21 -04:00
parent 839787568c
commit e69398c09f
62 changed files with 421 additions and 674 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/gps_dump.msg
View File

@ -6,3 +6,5 @@ uint64 timestamp # time since system start (microseconds)
uint8 len # length of data, MSB bit set = message to the gps device,
# clear = message from the device
uint8[79] data # data to write to the log
uint8 ORB_QUEUE_LENGTH = 8

2
msg/gps_inject_data.msg
View File

@ -2,3 +2,5 @@ uint64 timestamp # time since system start (microseconds)
uint8 len # length of data
uint8 flags # LSB: 1=fragmented
uint8[182] data # data to write to GPS device (RTCM message)
uint8 ORB_QUEUE_LENGTH = 6

2
msg/led_control.msg
View File

@ -33,3 +33,5 @@ uint8 mode # see MODE_*
uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite
# in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20
uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY)
uint8 ORB_QUEUE_LENGTH = 4 # needs to match BOARD_MAX_LEDS

2
msg/mavlink_log.msg
View File

@ -2,3 +2,5 @@ uint64 timestamp # time since system start (microseconds)
char[50] text
uint8 severity # log level (same as in the linux kernel, starting with 0)
uint8 ORB_QUEUE_LENGTH = 5

2
msg/subsystem_info.msg
View File

@ -34,4 +34,4 @@ bool enabled
bool ok
uint64 subsystem_type
uint32 ORB_QUEUE_LENGTH = 5
uint8 ORB_QUEUE_LENGTH = 5

2
msg/telemetry_status.msg
View File

@ -55,3 +55,5 @@ float32 rate_rx
float32 rate_tx
float32 rate_txerr
uint8 ORB_QUEUE_LENGTH = 3

2
msg/test_motor.msg
View File

@ -3,3 +3,5 @@ uint8 NUM_MOTOR_OUTPUTS = 8
uint32 motor_number # number of motor to spin
float32 value # output power, range [0..1]
uint8 ORB_QUEUE_LENGTH = 4

2
msg/transponder_report.msg
View File

@ -22,4 +22,4 @@ uint16 PX4_ADSB_FLAGS_VALID_CALLSIGN = 16
uint16 PX4_ADSB_FLAGS_VALID_SQUAWK = 32
uint16 PX4_ADSB_FLAGS_RETRANSLATE = 256
uint32 ORB_QUEUE_LENGTH = 10
uint8 ORB_QUEUE_LENGTH = 10

2
msg/uavcan_parameter_request.msg
View File

@ -7,3 +7,5 @@ int16 param_index # -1 if the param_id field should be used as identifier
uint8 param_type # MAVLink parameter type
int64 int_value # current value if param_type is int-like
float32 real_value # current value if param_type is float-like
uint8 ORB_QUEUE_LENGTH = 3

2
msg/vehicle_command_ack.msg
View File

@ -13,7 +13,7 @@ uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3
uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4
uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5
uint32 ORB_QUEUE_LENGTH = 3
uint8 ORB_QUEUE_LENGTH = 3
uint16 command
uint8 result

105
src/drivers/camera_capture/camera_capture.cpp
View File

@ -45,25 +45,12 @@
namespace camera_capture
{
CameraCapture *g_camera_capture;
CameraCapture *g_camera_capture{nullptr};
}
CameraCapture::CameraCapture() :
_capture_enabled(false),
_gpio_capture(false),
_trigger_pub(nullptr),
_command_ack_pub(nullptr),
_command_sub(-1),
_trig_buffer(nullptr),
_camera_capture_mode(0),
_camera_capture_edge(0),
_capture_seq(0),
_last_trig_begin_time(0),
_last_exposure_time(0),
_capture_overflows(0)
ScheduledWorkItem(px4::wq_configurations::lp_default)
{
memset(&_work, 0, sizeof(_work));
memset(&_work_publisher, 0, sizeof(_work_publisher));
// Capture Parameters
@ -75,7 +62,6 @@ CameraCapture::CameraCapture() :
_p_camera_capture_edge = param_find("CAM_CAP_EDGE");
param_get(_p_camera_capture_edge, &_camera_capture_edge);
}
CameraCapture::~CameraCapture()
@ -89,10 +75,8 @@ CameraCapture::~CameraCapture()
}
void
CameraCapture::capture_callback(uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
{
_trigger.chan_index = chan_index;
_trigger.edge_time = edge_time;
_trigger.edge_state = edge_state;
@ -114,7 +98,6 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg)
work_queue(HPWORK, &_work_publisher, (worker_t)&CameraCapture::publish_trigger_trampoline, dev, 0);
return PX4_OK;
}
void
@ -130,7 +113,7 @@ CameraCapture::publish_trigger()
{
bool publish = false;
struct camera_trigger_s trigger {};
camera_trigger_s trigger{};
// MODES 1 and 2 are not fully tested
if (_camera_capture_mode == 0 || _gpio_capture) {
@ -173,48 +156,23 @@ CameraCapture::publish_trigger()
return;
}
if (_trigger_pub == nullptr) {
_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
} else {
orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger);
}
_trigger_pub.publish(trigger);
}
void
CameraCapture::capture_trampoline(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
CameraCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
uint32_t overflow)
{
camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow);
}
void
CameraCapture::cycle_trampoline(void *arg)
CameraCapture::Run()
{
CameraCapture *dev = reinterpret_cast<CameraCapture *>(arg);
dev->cycle();
}
void
CameraCapture::cycle()
{
if (_command_sub < 0) {
_command_sub = orb_subscribe(ORB_ID(vehicle_command));
}
bool updated = false;
orb_check(_command_sub, &updated);
// Command handling
if (updated) {
vehicle_command_s cmd{};
vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _command_sub, &cmd);
if (_command_sub.update(&cmd)) {
// TODO : this should eventuallly be a capture control command
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
@ -235,31 +193,17 @@ CameraCapture::cycle()
}
// Acknowledge the command
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd.command,
.result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED,
.from_external = false,
.result_param1 = 0,
.target_system = cmd.source_system,
.target_component = cmd.source_component
};
vehicle_command_ack_s command_ack{};
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
} else {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
}
_command_ack_pub.publish(command_ack);
}
}
work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, camera_capture::g_camera_capture,
USEC2TICK(100000)); // 100ms
}
void
@ -267,9 +211,7 @@ CameraCapture::set_capture_control(bool enabled)
{
#if !defined CONFIG_ARCH_BOARD_AV_X_V1
int fd = -1;
fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
int fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0) {
PX4_ERR("open fail");
@ -343,7 +285,9 @@ err_out:
void
CameraCapture::reset_statistics(bool reset_seq)
{
if (reset_seq) { _capture_seq = 0; }
if (reset_seq) {
_capture_seq = 0;
}
_last_trig_begin_time = 0;
_last_exposure_time = 0;
@ -361,9 +305,8 @@ CameraCapture::start()
return PX4_ERROR;
}
// start to monitor at low rates for capture control commands
work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, this,
USEC2TICK(1));
// run every 100 ms (10 Hz)
ScheduleOnInterval(100000, 10000);
return PX4_OK;
}
@ -371,8 +314,8 @@ CameraCapture::start()
void
CameraCapture::stop()
{
ScheduleClear();
work_cancel(LPWORK, &_work);
work_cancel(HPWORK, &_work_publisher);
if (camera_capture::g_camera_capture != nullptr) {

76
src/drivers/camera_capture/camera_capture.hpp
View File

@ -38,22 +38,23 @@
#pragma once
#include <fcntl.h>
#include <string.h>
#include <parameters/param.h>
#include <px4_defines.h>
#include <px4_workqueue.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_input_capture.h>
#include <drivers/drv_pwm_output.h>
#include <lib/parameters/param.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_module.h>
#include <px4_tasks.h>
#include <px4_workqueue.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/uORB.h>
#define PX4FMU_DEVICE_PATH "/dev/px4fmu"
@ -61,7 +62,7 @@
#define GPIO_TRIG_AVX /* PD14 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN14)
class CameraCapture
class CameraCapture : public px4::ScheduledWorkItem
{
public:
/**
@ -86,10 +87,10 @@ public:
void status();
void cycle();
// Low-rate command handling loop
void Run() override;
static void capture_trampoline(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state,
static void capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
uint32_t overflow);
void set_capture_control(bool enabled);
@ -99,20 +100,16 @@ public:
void publish_trigger();
static struct work_s _work;
static struct work_s _work_publisher;
private:
bool _capture_enabled;
bool _gpio_capture;
// Publishers
orb_advert_t _trigger_pub;
orb_advert_t _command_ack_pub;
uORB::PublicationQueued<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<camera_trigger_s> _trigger_pub{ORB_ID(camera_trigger)};
// Subscribers
int _command_sub;
uORB::Subscription _command_sub{ORB_ID(vehicle_command)};
// Trigger Buffer
struct _trig_s {
@ -120,30 +117,30 @@ private:
hrt_abstime edge_time;
uint32_t edge_state;
uint32_t overflow;
};
} _trigger{};
struct _trig_s _trigger;
ringbuffer::RingBuffer *_trig_buffer{nullptr};
ringbuffer::RingBuffer *_trig_buffer;
bool _capture_enabled{false};
bool _gpio_capture{false};
// Parameters
param_t _p_strobe_delay;
float _strobe_delay;
param_t _p_camera_capture_mode;
int32_t _camera_capture_mode;
param_t _p_camera_capture_edge;
int32_t _camera_capture_edge;
param_t _p_strobe_delay{PARAM_INVALID};
float _strobe_delay{0.0f};
param_t _p_camera_capture_mode{PARAM_INVALID};
int32_t _camera_capture_mode{0};
param_t _p_camera_capture_edge{PARAM_INVALID};
int32_t _camera_capture_edge{0};
// Signal capture statistics
uint32_t _capture_seq;
hrt_abstime _last_trig_begin_time;
hrt_abstime _last_exposure_time;
hrt_abstime _last_trig_time;
uint32_t _capture_overflows;
uint32_t _capture_seq{0};
hrt_abstime _last_trig_begin_time{0};
hrt_abstime _last_exposure_time{0};
hrt_abstime _last_trig_time{0};
uint32_t _capture_overflows{0};
// Signal capture callback
void capture_callback(uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
// GPIO interrupt routine (for AV_X board)
static int gpio_interrupt_routine(int irq, void *context, void *arg);
@ -151,9 +148,6 @@ private:
// Signal capture publish
static void publish_trigger_trampoline(void *arg);
// Low-rate command handling loop
static void cycle_trampoline(void *arg);
};
struct work_s CameraCapture::_work;
struct work_s CameraCapture::_work_publisher;

23
src/drivers/camera_trigger/camera_trigger.cpp
View File

@ -55,6 +55,7 @@
#include <parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/camera_capture.h>
@ -174,7 +175,8 @@ private:
uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)};
orb_advert_t _trigger_pub;
orb_advert_t _cmd_ack_pub;
uORB::PublicationQueued<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
param_t _p_mode;
param_t _p_activation_time;
@ -192,7 +194,7 @@ private:
/**
* Vehicle command handler
*/
void Run();
void Run() override;
/**
* Fires trigger
@ -250,7 +252,6 @@ CameraTrigger::CameraTrigger() :
_last_shoot_position(0.0f, 0.0f),
_valid_position(false),
_trigger_pub(nullptr),
_cmd_ack_pub(nullptr),
_trigger_mode(TRIGGER_MODE_NONE),
_cam_cap_fback(0),
_camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO),
@ -477,12 +478,13 @@ CameraTrigger::stop()
void
CameraTrigger::test()
{
vehicle_command_s vcmd = {};
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.param5 = 1.0;
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
}
void
@ -682,20 +684,15 @@ CameraTrigger::Run()
// Command ACK handling
if (updated && need_ack) {
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)cmd_result;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
if (_cmd_ack_pub == nullptr) {
_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _cmd_ack_pub, &command_ack);
}
_cmd_ack_pub.publish(command_ack);
}
ScheduleDelayed(poll_interval_usec);

6
src/drivers/drv_led.h
View File

@ -49,14 +49,12 @@
#define BOARD_MAX_LEDS 4
#endif
static_assert(led_control_s::ORB_QUEUE_LENGTH >= BOARD_MAX_LEDS, "led_control_s::ORB_QUEUE_LENGTH too small");
#if BOARD_MAX_LEDS > 8 // because led_mask is uint8_t
#error "BOARD_MAX_LEDS too large. You need to change the led_mask type in the led_control uorb topic (and where it's used)"
#endif
// set the queue size to the number of LED's, so that each led can be controlled individually
static const int LED_UORB_QUEUE_LENGTH = BOARD_MAX_LEDS;
// Legacy paths - 2 are need to allow both pwm and i2c drviers to co-exist
#define RGBLED0_DEVICE_PATH "/dev/rgbled0" // Primary RGB LED on i2c
#define RGBLED1_DEVICE_PATH "/dev/rgbled1" // Primary RGB LED(NCP5623C) on i2c

18
src/drivers/gps/gps.cpp
View File

@ -52,6 +52,7 @@
#include <px4_cli.h>
#include <px4_getopt.h>
#include <px4_module.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/gps_dump.h>
#include <uORB/topics/gps_inject_data.h>
@ -172,9 +173,10 @@ private:
const Instance _instance;
uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)};
orb_advert_t _dump_communication_pub{nullptr}; ///< if non-null, dump communication
uORB::PublicationQueued<gps_dump_s> _dump_communication_pub{ORB_ID(gps_dump)};
gps_dump_s *_dump_to_device{nullptr};
gps_dump_s *_dump_from_device{nullptr};
bool _should_dump_communication{false}; ///< if true, dump communication
static volatile bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1
/// and thus we wait until the first one publishes at least one message.
@ -549,16 +551,16 @@ void GPS::initializeCommunicationDump()
memset(_dump_to_device, 0, sizeof(gps_dump_s));
memset(_dump_from_device, 0, sizeof(gps_dump_s));
int instance;
//make sure to use a large enough queue size, so that we don't lose messages. You may also want
//to increase the logger rate for that.
_dump_communication_pub = orb_advertise_multi_queue(ORB_ID(gps_dump), _dump_from_device, &instance,
ORB_PRIO_DEFAULT, 8);
_dump_communication_pub.publish(*_dump_from_device);
_should_dump_communication = true;
}
void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
{
if (!_dump_communication_pub) {
if (!_should_dump_communication) {
return;
}
@ -582,7 +584,7 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
}
dump_data->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(gps_dump), _dump_communication_pub, dump_data);
_dump_communication_pub.publish(*dump_data);
dump_data->len = 0;
}
}
@ -820,10 +822,6 @@ GPS::run()
PX4_INFO("exiting");
if (_dump_communication_pub) {
orb_unadvertise(_dump_communication_pub);
}
if (_serial_fd >= 0) {
::close(_serial_fd);
_serial_fd = -1;

11
src/drivers/px4io/px4io.cpp
View File

@ -76,6 +76,7 @@
#include <circuit_breaker/circuit_breaker.h>
#include <systemlib/mavlink_log.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
@ -738,7 +739,8 @@ PX4IO::init()
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;
/* send command once */
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
/* spin here until IO's state has propagated into the system */
do {
@ -755,7 +757,7 @@ PX4IO::init()
/* re-send if necessary */
if (!actuator_armed.force_failsafe) {
orb_publish(ORB_ID(vehicle_command), pub, &vcmd);
vcmd_pub.publish(vcmd);
PX4_WARN("re-sending flight termination cmd");
}
@ -769,7 +771,8 @@ PX4IO::init()
vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
/* send command once */
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
/* spin here until IO's state has propagated into the system */
do {
@ -786,7 +789,7 @@ PX4IO::init()
/* re-send if necessary */
if (!actuator_armed.armed) {
orb_publish(ORB_ID(vehicle_command), pub, &vcmd);
vcmd_pub.publish(vcmd);
PX4_WARN("re-sending arm cmd");
}

15
src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
View File

@ -1111,14 +1111,8 @@ void IridiumSBD::publish_iridium_status()
// publish the status if it changed
if (need_to_publish) {
if (_iridiumsbd_status_pub == nullptr) {
_iridiumsbd_status_pub = orb_advertise(ORB_ID(iridiumsbd_status), &_status);
} else {
orb_publish(ORB_ID(iridiumsbd_status), _iridiumsbd_status_pub, &_status);
}
_iridiumsbd_status_pub.publish(_status);
}
}
void IridiumSBD::publish_subsystem_status()
@ -1134,12 +1128,7 @@ void IridiumSBD::publish_subsystem_status()
_info.enabled = enabled;
_info.ok = ok;
if (_subsystem_pub == nullptr) {
_subsystem_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &_info);
}
_subsystem_pub.publish(_info);
}
}

7
src/drivers/telemetry/iridiumsbd/IridiumSBD.h
View File

@ -39,7 +39,8 @@
#include <lib/cdev/CDev.hpp>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/subsystem_info.h>
@ -304,8 +305,8 @@ private:
bool _writing_mavlink_packet = false;
uint16_t _packet_length = 0;
orb_advert_t _iridiumsbd_status_pub = nullptr;
orb_advert_t _subsystem_pub = nullptr;
uORB::Publication<iridiumsbd_status_s> _iridiumsbd_status_pub{ORB_ID(iridiumsbd_status)};
uORB::PublicationQueued<subsystem_info_s> _subsystem_pub{ORB_ID(subsystem_info)};
bool _test_pending = false;
char _test_command[32];

10
src/drivers/uavcan/uavcan_servers.cpp
View File

@ -564,20 +564,14 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
}
// Acknowledge the received command
vehicle_command_ack_s ack = {};
vehicle_command_ack_s ack{};
ack.timestamp = hrt_absolute_time();
ack.command = cmd.command;
ack.result = cmd_ack_result;
ack.target_system = cmd.source_system;
ack.target_component = cmd.source_component;
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &ack);
}
_command_ack_pub.publish(ack);
}
// Shut down once armed

6
src/drivers/uavcan/uavcan_servers.hpp
View File

@ -36,7 +36,9 @@
#include <px4_config.h>
#include "uavcan_driver.hpp"
#include <drivers/device/device.h>
#include <perf/perf_counter.h>
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/vehicle_command_ack.h>
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
@ -160,7 +162,7 @@ private:
// uORB topic handle for MAVLink parameter responses
orb_advert_t _param_response_pub = nullptr;
orb_advert_t _command_ack_pub = nullptr;
uORB::PublicationQueued<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
typedef uavcan::MethodBinder<UavcanServers *,
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;

24
src/lib/FlightTasks/FlightTasks.cpp
View File

@ -137,22 +137,16 @@ void FlightTasks::reActivate()
void FlightTasks::_updateCommand()
{
// lazy subscription to command topic
if (_sub_vehicle_command < 0) {
_sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
}
// check if there's any new command
bool updated = false;
orb_check(_sub_vehicle_command, &updated);
bool updated = _sub_vehicle_command.updated();
if (!updated) {
return;
}
// get command
struct vehicle_command_s command;
orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
vehicle_command_s command{};
_sub_vehicle_command.copy(&command);
// check what command it is
FlightTaskIndex desired_task = switchVehicleCommand(command.command);
@ -183,19 +177,13 @@ void FlightTasks::_updateCommand()
}
// send back acknowledgment
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = command.command;
command_ack.result = cmd_result;
command_ack.result_param1 = switch_result;
command_ack.target_system = command.source_system;
command_ack.target_component = command.source_component;
if (_pub_vehicle_command_ack == nullptr) {
_pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack);
}
_pub_vehicle_command_ack.publish(command_ack);
}

10
src/lib/FlightTasks/FlightTasks.hpp
View File

@ -45,6 +45,10 @@
#include "SubscriptionArray.hpp"
#include "FlightTasks_generated.hpp"
#include <lib/WeatherVane/WeatherVane.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_command.h>
#include <new>
@ -180,8 +184,10 @@ private:
*/
void _updateCommand();
FlightTaskIndex switchVehicleCommand(const int command);
int _sub_vehicle_command = -1; /**< topic handle on which commands are received */
orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */
uORB::Subscription _sub_vehicle_command{ORB_ID(vehicle_command)}; /**< topic handle on which commands are received */
uORB::PublicationQueued<vehicle_command_ack_s> _pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)};
int _initTask(FlightTaskIndex task_index);
};

45
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
View File

@ -63,19 +63,6 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
}
ObstacleAvoidance::~ObstacleAvoidance()
{
//unadvertise publishers
if (_pub_traj_wp_avoidance_desired != nullptr) {
orb_unadvertise(_pub_traj_wp_avoidance_desired);
}
if (_pub_pos_control_status != nullptr) {
orb_unadvertise(_pub_pos_control_status);
}
}
bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array)
{
if (!subscription_array.get(ORB_ID(vehicle_trajectory_waypoint), _sub_vehicle_trajectory_waypoint)) {
@ -170,7 +157,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp,
vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
_publishAvoidanceDesiredWaypoint();
_pub_traj_wp_avoidance_desired.publish(_desired_waypoint);
_desired_waypoint = empty_trajectory_waypoint;
}
@ -208,27 +195,7 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector
// do not check for waypoints yaw acceptance in navigator
pos_control_status.yaw_acceptance = NAN;
if (_pub_pos_control_status == nullptr) {
_pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status);
} else {
orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status);
}
}
void
ObstacleAvoidance::_publishAvoidanceDesiredWaypoint()
{
// publish desired waypoint
if (_pub_traj_wp_avoidance_desired != nullptr) {
orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _pub_traj_wp_avoidance_desired, &_desired_waypoint);
} else {
_pub_traj_wp_avoidance_desired = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
&_desired_waypoint);
}
_pub_pos_control_status.publish(pos_control_status);
}
void ObstacleAvoidance::_publishVehicleCmdDoLoiter()
@ -248,11 +215,5 @@ void ObstacleAvoidance::_publishVehicleCmdDoLoiter()
command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
// publish the vehicle command
if (_pub_vehicle_command == nullptr) {
_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
}
_pub_vehicle_command.publish(command);
}

18
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
View File

@ -46,6 +46,8 @@
#include <commander/px4_custom_mode.h>
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
@ -62,7 +64,7 @@ class ObstacleAvoidance : public ModuleParams
{
public:
ObstacleAvoidance(ModuleParams *parent);
~ObstacleAvoidance();
~ObstacleAvoidance() = default;
bool initializeSubscriptions(SubscriptionArray &subscription_array);
@ -113,10 +115,11 @@ private:
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */
);
vehicle_trajectory_waypoint_s _desired_waypoint = {}; /**< desired vehicle trajectory waypoint to be sent to OA */
orb_advert_t _pub_traj_wp_avoidance_desired{nullptr}; /**< trajectory waypoint desired publication */
orb_advert_t _pub_pos_control_status{nullptr}; /**< position controller status publication */
orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */
vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */
uORB::Publication<vehicle_trajectory_waypoint_s> _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; /**< trajectory waypoint desired publication */
uORB::Publication<position_controller_status_s> _pub_pos_control_status{ORB_ID(position_controller_status)}; /**< position controller status publication */
uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command do publication */
matrix::Vector3f _curr_wp = {}; /**< current position triplet */
matrix::Vector3f _position = {}; /**< current vehicle position */
@ -126,11 +129,6 @@ private:
bool _ext_yaw_active = false; /**< true, if external yaw handling is active */
/**
* Publishes vehicle trajectory waypoint desired.
*/
void _publishAvoidanceDesiredWaypoint();
/**
* Publishes vehicle command.
*/

2
src/lib/systemlib/CMakeLists.txt
View File

@ -35,7 +35,7 @@ set(SRCS
conversions.c
cpuload.c
crc.c
mavlink_log.c
mavlink_log.cpp
otp.c
)

19
src/lib/systemlib/mavlink_log.c → src/lib/systemlib/mavlink_log.cpp
View File

@ -48,9 +48,6 @@
#include <uORB/topics/mavlink_log.h>
#include "mavlink_log.h"
#define MAVLINK_LOG_QUEUE_SIZE 5
__EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, const char *fmt, ...)
{
// TODO: add compile check for maxlen
@ -59,31 +56,23 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
return;
}
if (mavlink_log_pub == NULL) {
if (mavlink_log_pub == nullptr) {
return;
}
struct mavlink_log_s log_msg;
mavlink_log_s log_msg;
log_msg.severity = severity;
log_msg.timestamp = hrt_absolute_time();
va_list ap;
va_start(ap, fmt);
vsnprintf((char *)log_msg.text, sizeof(log_msg.text), fmt, ap);
va_end(ap);
if (*mavlink_log_pub != NULL) {
if (*mavlink_log_pub != nullptr) {
orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg);
} else {
*mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log),
&log_msg,
MAVLINK_LOG_QUEUE_SIZE);
*mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH);
}
}

36
src/modules/commander/Commander.cpp
View File

@ -93,7 +93,6 @@
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vtol_vehicle_status.h>
@ -205,7 +204,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const ch
*/
void *commander_low_prio_loop(void *arg);
static void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub);
static void answer_command(const vehicle_command_s &cmd, unsigned result,
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub);
static int power_button_state_notification_cb(board_power_button_state_notification_e request)
{
@ -249,7 +249,8 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN)
{
vehicle_command_s vcmd = {};
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = param1;
vcmd.param2 = param2;
@ -262,9 +263,9 @@ static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2
vcmd.target_system = status.system_id;
vcmd.target_component = status.component_id;
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
return (h != nullptr);
return vcmd_pub.publish(vcmd);
}
int commander_main(int argc, char *argv[])
@ -577,7 +578,7 @@ Commander::~Commander()
bool
Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local,
orb_advert_t *command_ack_pub, bool *changed)
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed)
{
/* only handle commands that are meant to be handled by this system and component */
if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id)
@ -1078,13 +1079,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
default:
/* Warn about unsupported commands, this makes sense because only commands
* to this component ID (or all) are passed by mavlink. */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
break;
}
if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(cmd, cmd_result, *command_ack_pub);
answer_command(cmd, cmd_result, command_ack_pub);
}
return true;
@ -1239,7 +1240,7 @@ Commander::run()
hrt_abstime last_disarmed_timestamp = 0;
/* command ack */
orb_advert_t command_ack_pub = nullptr;
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
mission_init();
@ -2152,7 +2153,7 @@ Commander::run()
cmd_sub.copy(&cmd);
/* handle it */
if (handle_command(&status, cmd, &armed, &command_ack_pub, &status_changed)) {
if (handle_command(&status, cmd, &armed, command_ack_pub, &status_changed)) {
status_changed = true;
}
}
@ -3260,7 +3261,8 @@ print_reject_arm(const char *msg)
}
}
void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub)
void answer_command(const vehicle_command_s &cmd, unsigned result,
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub)
{
switch (result) {
case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
@ -3288,20 +3290,16 @@ void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t
}
/* publish ACK */
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)result;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
if (command_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
} else {
command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
}
command_ack_pub.publish(command_ack);
}
void *commander_low_prio_loop(void *arg)
@ -3313,7 +3311,7 @@ void *commander_low_prio_loop(void *arg)
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
/* command ack */
orb_advert_t command_ack_pub = nullptr;
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
/* wakeup source(s) */
px4_pollfd_struct_t fds[1];

7
src/modules/commander/Commander.hpp
View File

@ -45,8 +45,10 @@
// publications
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
@ -180,7 +182,7 @@ private:
bool _flight_termination_triggered{false};
bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
orb_advert_t *command_ack_pub, bool *changed);
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed);
bool set_home_position();
bool set_home_position_alt_only();
@ -258,12 +260,13 @@ private:
// Publications
uORB::Publication<vehicle_control_mode_s> _control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};
uORB::Publication<vehicle_status_s> _status_pub{ORB_ID(vehicle_status)};
uORB::Publication<actuator_armed_s> _armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<commander_state_s> _commander_state_pub{ORB_ID(commander_state)};
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};
};
#endif /* COMMANDER_HPP_ */

12
src/modules/commander/arm_auth.cpp
View File

@ -39,10 +39,10 @@
#include <px4_config.h>
#include <systemlib/mavlink_log.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
static orb_advert_t handle_vehicle_command_pub;
static orb_advert_t *mavlink_log_pub;
static int command_ack_sub = -1;
@ -78,17 +78,13 @@ static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
static void arm_auth_request_msg_send()
{
vehicle_command_s vcmd = {};
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST;
vcmd.target_system = arm_parameters.authorizer_system_id;
if (handle_vehicle_command_pub == nullptr) {
handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &vcmd);
}
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
}
static uint8_t _auth_method_arm_req_check()

2
src/modules/commander/commander_helper.cpp
View File

@ -290,7 +290,7 @@ int led_init()
led_control.mode = led_control_s::MODE_OFF;
led_control.priority = 0;
led_control.timestamp = hrt_absolute_time();
led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH);
led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH);
/* first open normal LEDs */
DevMgr::getHandle(LED0_DEVICE_PATH, h_leds);

16
src/modules/events/send_event.cpp
View File

@ -195,20 +195,15 @@ void SendEvent::process_commands()
void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
{
/* publish ACK */
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)result;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
if (_command_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
} else {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
}
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
command_ack_pub.publish(command_ack);
}
int SendEvent::print_usage(const char *reason)
@ -275,7 +270,7 @@ int SendEvent::custom_command(int argc, char *argv[])
}
}
vehicle_command_s vcmd = {};
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = (float)((gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
vcmd.param2 = NAN;
@ -286,7 +281,8 @@ int SendEvent::custom_command(int argc, char *argv[])
vcmd.param7 = (float)((baro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION;
orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
} else {
print_usage("unrecognized command");

4
src/modules/events/send_event.h
View File

@ -40,6 +40,7 @@
#include <px4_workqueue.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
@ -132,9 +133,6 @@ private:
/** @var _rc_loss_alarm Pointer to the RC loss alarm object. */
rc_loss::RC_Loss_Alarm *_rc_loss_alarm = nullptr;
/** @var _command_ack_pub The command ackowledgement topic. */
orb_advert_t _command_ack_pub = nullptr;
/** @note Declare local parameters using defined parameters. */
DEFINE_PARAMETERS(
/** @var _param_status_display Parameter to enable/disable the LED status display. */

7
src/modules/events/status_display.cpp
View File

@ -102,12 +102,7 @@ void StatusDisplay::publish()
{
_led_control.timestamp = hrt_absolute_time();
if (_led_control_pub != nullptr) {
orb_publish(ORB_ID(led_control), _led_control_pub, &_led_control);
} else {
_led_control_pub = orb_advertise_queue(ORB_ID(led_control), &_led_control, LED_UORB_QUEUE_LENGTH);
}
_led_control_pub.publish(_led_control);
}
} /* namespace status */

6
src/modules/events/status_display.h
View File

@ -44,7 +44,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/led_control.h>
@ -96,7 +96,9 @@ private:
bool _critical_battery = false;
int _old_nav_state = -1;
int _old_battery_status_warning = -1;
orb_advert_t _led_control_pub = nullptr;
uORB::PublicationQueued<led_control_s> _led_control_pub{ORB_ID(led_control)};
const events::SubscriberHandler &_subscriber_handler;
};

11
src/modules/events/temperature_calibration/task.cpp
View File

@ -39,6 +39,7 @@
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/sensor_gyro.h>
#include <mathlib/mathlib.h>
#include <px4_log.h>
@ -92,7 +93,7 @@ public:
private:
void publish_led_control(led_control_s &led_control);
orb_advert_t _led_control_pub = nullptr;
uORB::PublicationQueued<led_control_s> _led_control_pub{ORB_ID(led_control)};
bool _force_task_exit = false;
int _control_task = -1; // task handle for task
@ -349,13 +350,7 @@ int TemperatureCalibration::start()
void TemperatureCalibration::publish_led_control(led_control_s &led_control)
{
led_control.timestamp = hrt_absolute_time();
if (_led_control_pub == nullptr) {
_led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(led_control), _led_control_pub, &led_control);
}
_led_control_pub.publish(led_control);
}
int run_temperature_calibration(bool accel, bool baro, bool gyro)

34
src/modules/logger/logger.cpp
View File

@ -44,7 +44,7 @@
#include <stdlib.h>
#include <time.h>
#include <uORB/uORB.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/uORBTopics.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/log_message.h>
@ -935,7 +935,6 @@ void Logger::run()
}
int vehicle_command_sub = -1;
orb_advert_t vehicle_command_ack_pub = nullptr;
if (_writer.backend() & LogWriter::BackendMavlink) {
vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
@ -1042,7 +1041,7 @@ void Logger::run()
/* check for logging command from MAVLink (start/stop streaming) */
if (vehicle_command_sub >= 0) {
handle_vehicle_command_update(vehicle_command_sub, vehicle_command_ack_pub);
handle_vehicle_command_update(vehicle_command_sub);
}
@ -1293,10 +1292,6 @@ void Logger::run()
_mavlink_log_pub = nullptr;
}
if (vehicle_command_ack_pub) {
orb_unadvertise(vehicle_command_ack_pub);
}
if (vehicle_command_sub != -1) {
orb_unsubscribe(vehicle_command_sub);
}
@ -1402,7 +1397,7 @@ bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_su
return bret;
}
void Logger::handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t &vehicle_command_ack_pub)
void Logger::handle_vehicle_command_update(int vehicle_command_sub)
{
bool command_updated = false;
int ret = orb_check(vehicle_command_sub, &command_updated);
@ -1413,23 +1408,19 @@ void Logger::handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t
if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
if ((int)(command.param1 + 0.5f) != 0) {
ack_vehicle_command(vehicle_command_ack_pub, &command,
vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
} else if (can_start_mavlink_log()) {
ack_vehicle_command(vehicle_command_ack_pub, &command,
vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
start_log_mavlink();
} else {
ack_vehicle_command(vehicle_command_ack_pub, &command,
vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
}
} else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) {
stop_log_mavlink();
ack_vehicle_command(vehicle_command_ack_pub, &command,
vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
}
}
}
@ -2339,7 +2330,7 @@ void Logger::write_changed_parameters(LogType type)
_writer.notify();
}
void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result)
void Logger::ack_vehicle_command(vehicle_command_s *cmd, uint32_t result)
{
vehicle_command_ack_s vehicle_command_ack = {};
vehicle_command_ack.timestamp = hrt_absolute_time();
@ -2348,13 +2339,8 @@ void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_
vehicle_command_ack.target_system = cmd->source_system;
vehicle_command_ack.target_component = cmd->source_component;
if (vehicle_command_ack_pub == nullptr) {
vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
}
uORB::PublicationQueued<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
cmd_ack_pub.publish(vehicle_command_ack);
}
int Logger::print_usage(const char *reason)

4
src/modules/logger/logger.h
View File

@ -357,8 +357,8 @@ private:
*/
bool start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type);
void handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t &vehicle_command_ack_pub);
void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result);
void handle_vehicle_command_update(int vehicle_command_sub);
void ack_vehicle_command(vehicle_command_s *cmd, uint32_t result);
/**
* initialize the output for the process load, so that ~1 second later it will be written to the log

24
src/modules/mavlink/mavlink_main.cpp
View File

@ -49,8 +49,9 @@
#endif
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <version/version.h>
#include <lib/mathlib/mathlib.h>
#include <lib/version/version.h>
#include <uORB/PublicationQueued.hpp>
#include "mavlink_receiver.h"
#include "mavlink_main.h"
@ -2126,7 +2127,7 @@ Mavlink::task_main(int argc, char *argv[])
cmd_sub->subscribe_from_beginning(true);
/* command ack */
orb_advert_t command_ack_pub = nullptr;
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
@ -2263,7 +2264,7 @@ Mavlink::task_main(int argc, char *argv[])
}
// send positive command ack
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = vehicle_cmd.timestamp;
command_ack.command = vehicle_cmd.command;
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
@ -2271,13 +2272,7 @@ Mavlink::task_main(int argc, char *argv[])
command_ack.target_system = vehicle_cmd.source_system;
command_ack.target_component = vehicle_cmd.source_component;
if (command_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
} else {
command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
}
command_ack_pub.publish(command_ack);
}
}
@ -2542,12 +2537,7 @@ void Mavlink::publish_telemetry_status()
_tstatus.timestamp = hrt_absolute_time();
if (_telem_status_pub == nullptr) {
_telem_status_pub = orb_advertise_queue(ORB_ID(telemetry_status), &_tstatus, 3);
} else {
orb_publish(ORB_ID(telemetry_status), _telem_status_pub, &_tstatus);
}
_telem_status_pub.publish(_tstatus);
}
void Mavlink::check_radio_config()

5
src/modules/mavlink/mavlink_main.h
View File

@ -71,11 +71,11 @@
#include <px4_posix.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/uthash/utlist.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/radio_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/uORB.h>
#include "mavlink_command_sender.h"
#include "mavlink_messages.h"
@ -540,7 +540,8 @@ private:
bool _first_heartbeat_sent{false};
orb_advert_t _mavlink_log_pub{nullptr};
orb_advert_t _telem_status_pub{nullptr};
uORB::PublicationQueued<telemetry_status_s> _telem_status_pub{ORB_ID(telemetry_status)};
bool _task_running{false};
static bool _boot_complete;

1
src/modules/mavlink/mavlink_mission.cpp
View File

@ -270,7 +270,6 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
PX4_DEBUG("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system);
}
void
MavlinkMissionManager::send_mission_current(uint16_t seq)
{

28
src/modules/mavlink/mavlink_parameters.cpp
View File

@ -50,13 +50,6 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) :
{
}
MavlinkParametersManager::~MavlinkParametersManager()
{
if (_uavcan_parameter_request_pub) {
orb_unadvertise(_uavcan_parameter_request_pub);
}
}
unsigned
MavlinkParametersManager::get_size()
{
@ -91,12 +84,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
req.node_id = req_list.target_component;
req.param_index = 0;
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
_uavcan_parameter_request_pub.publish(req);
}
break;
@ -163,12 +151,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
req.int_value = val;
}
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
_uavcan_parameter_request_pub.publish(req);
}
break;
@ -577,12 +560,7 @@ void MavlinkParametersManager::request_next_uavcan_parameter()
if (!_uavcan_waiting_for_request_response && _uavcan_open_request_list != nullptr) {
uavcan_parameter_request_s req = _uavcan_open_request_list->req;
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise_queue(ORB_ID(uavcan_parameter_request), &req, 5);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
_uavcan_parameter_request_pub.publish(req);
_uavcan_waiting_for_request_response = true;
}

7
src/modules/mavlink/mavlink_parameters.h
View File

@ -45,7 +45,7 @@
#include <parameters/param.h>
#include "mavlink_bridge_header.h"
#include <uORB/uORB.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rc_parameter_map.h>
#include <uORB/topics/uavcan_parameter_request.h>
@ -59,7 +59,7 @@ class MavlinkParametersManager
{
public:
explicit MavlinkParametersManager(Mavlink *mavlink);
~MavlinkParametersManager();
~MavlinkParametersManager() = default;
/**
* Handle sending of messages. Call this regularly at a fixed frequency.
@ -128,7 +128,8 @@ protected:
orb_advert_t _rc_param_map_pub{nullptr};
rc_parameter_map_s _rc_param_map{};
orb_advert_t _uavcan_parameter_request_pub{nullptr};
uORB::PublicationQueued<uavcan_parameter_request_s> _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)};
uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};
uORB::Subscription _mavlink_parameter_sub{ORB_ID(parameter_update)};

60
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -98,20 +98,14 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
void
MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
{
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = command;
command_ack.result = result;
command_ack.target_system = sysid;
command_ack.target_component = compid;
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
}
_cmd_ack_pub.publish(command_ack);
}
void
@ -515,12 +509,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
}
if (!send_ack) {
if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vehicle_command);
}
_cmd_pub.publish(vehicle_command);
}
}
@ -547,13 +536,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
command_ack.target_system = ack.target_system;
command_ack.target_component = ack.target_component;
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
}
_cmd_ack_pub.publish(command_ack);
// TODO: move it to the same place that sent the command
if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) {
@ -687,7 +670,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
union px4_custom_mode custom_mode;
custom_mode.data = new_mode.custom_mode;
vehicle_command_s vcmd = {};
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
@ -703,12 +686,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
vcmd.confirmation = true;
vcmd.from_external = true;
if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
_cmd_pub.publish(vcmd);
}
void
@ -2274,12 +2252,7 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
//PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc);
if (_transponder_report_pub == nullptr) {
_transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t);
}
_transponder_report_pub.publish(t);
}
void
@ -2343,13 +2316,7 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg)
// Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not
// provide these.
if (_transponder_report_pub == nullptr) {
_transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t);
}
_transponder_report_pub.publish(t);
_last_utm_global_pos_com = t.timestamp;
}
@ -2392,16 +2359,7 @@ MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data,
math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_topic.len));
orb_advert_t &pub = _gps_inject_data_pub;
if (pub == nullptr) {
pub = orb_advertise_queue(ORB_ID(gps_inject_data), &gps_inject_data_topic,
_gps_inject_data_queue_size);
} else {
orb_publish(ORB_ID(gps_inject_data), pub, &gps_inject_data_topic);
}
_gps_inject_data_pub.publish(gps_inject_data_topic);
}
void

13
src/modules/mavlink/mavlink_receiver.h
View File

@ -43,6 +43,7 @@
#include <px4_module_params.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
@ -71,6 +72,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
@ -212,6 +214,11 @@ private:
vehicle_land_detected_s _hil_land_detector {};
vehicle_control_mode_s _control_mode {};
uORB::PublicationQueued<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
uORB::PublicationQueued<transponder_report_s> _transponder_report_pub{ORB_ID(transponder_report)};
uORB::PublicationQueued<vehicle_command_s> _cmd_pub{ORB_ID(vehicle_command)};
uORB::PublicationQueued<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
orb_advert_t _accel_pub{nullptr};
orb_advert_t _actuator_controls_pubs[4] {nullptr, nullptr, nullptr, nullptr};
orb_advert_t _airspeed_pub{nullptr};
@ -219,9 +226,7 @@ private:
orb_advert_t _attitude_pub{nullptr};
orb_advert_t _baro_pub{nullptr};
orb_advert_t _battery_pub{nullptr};
orb_advert_t _cmd_pub{nullptr};
orb_advert_t _collision_report_pub{nullptr};
orb_advert_t _command_ack_pub{nullptr};
orb_advert_t _debug_array_pub{nullptr};
orb_advert_t _debug_key_value_pub{nullptr};
orb_advert_t _debug_value_pub{nullptr};
@ -231,7 +236,6 @@ private:
orb_advert_t _flow_pub{nullptr};
orb_advert_t _follow_target_pub{nullptr};
orb_advert_t _global_pos_pub{nullptr};
orb_advert_t _gps_inject_data_pub{nullptr};
orb_advert_t _gps_pub{nullptr};
orb_advert_t _gyro_pub{nullptr};
orb_advert_t _hil_distance_sensor_pub{nullptr};
@ -249,15 +253,12 @@ private:
orb_advert_t _rates_sp_pub{nullptr};
orb_advert_t _rc_pub{nullptr};
orb_advert_t _trajectory_waypoint_pub{nullptr};
orb_advert_t _transponder_report_pub{nullptr};
orb_advert_t _visual_odometry_pub{nullptr};
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
static constexpr int _gps_inject_data_queue_size{6};
static constexpr unsigned int MOM_SWITCH_COUNT{8};
int _orb_class_instance{-1};

11
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -47,6 +47,7 @@
#include <commander/px4_custom_mode.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/parameter_update.h>
@ -108,7 +109,7 @@ private:
Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */
orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */
orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command publication */
uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */
orb_advert_t _mavlink_log_pub{nullptr};
orb_id_t _attitude_setpoint_id{nullptr};
@ -1038,13 +1039,7 @@ void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state)
}
// publish the vehicle command
if (_pub_vehicle_command == nullptr) {
_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
}
_pub_vehicle_command.publish(command);
}
int MulticopterPositionControl::task_spawn(int argc, char *argv[])

108
src/modules/muorb/test/muorb_test_example.cpp
View File

@ -43,6 +43,9 @@
#include <unistd.h>
#include <stdio.h>
#include <px4_defines.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
px4::AppState MuorbTestExample::appState;
@ -50,7 +53,7 @@ int MuorbTestExample::main()
{
int rc;
appState.setRunning(true);
rc = PingPongTest();
rc = PingPongTest();
appState.setRunning(false);
return rc;
}
@ -58,63 +61,37 @@ int MuorbTestExample::main()
int MuorbTestExample::DefaultTest()
{
int i = 0;
orb_advert_t pub_id = orb_advertise(ORB_ID(esc_status), & m_esc_status);
if (pub_id == nullptr) {
PX4_ERR("error publishing esc_status");
return -1;
}
orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc);
if (pub_id_vc == nullptr) {
PX4_ERR("error publishing vehicle_command");
return -1;
}
if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
PX4_ERR("[%d]Error publishing the vechile command message", i);
return -1;
}
int sub_vc = orb_subscribe(ORB_ID(vehicle_command));
if (sub_vc == PX4_ERROR) {
PX4_ERR("Error subscribing to vehicle_command topic");
return -1;
}
uORB::Subscription sub_vc{ORB_ID(vehicle_command)};
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
uORB::Publication<esc_status_s> pub_id{ORB_ID(esc_status)};
pub_id.publish(m_esc_status);
while (!appState.exitRequested() && i < 100) {
PX4_DEBUG("[%d] Doing work...", i);
if (orb_publish(ORB_ID(esc_status), pub_id, &m_esc_status) == PX4_ERROR) {
if (!pub_id.publish(m_esc_status)) {
PX4_ERR("[%d]Error publishing the esc status message for iter", i);
break;
}
bool updated = false;
if (sub_vc.updated()) {
PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i);
if (orb_check(sub_vc, &updated) == 0) {
if (updated) {
PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i);
if (!sub_vc.copy(&m_vc)) {
PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i);
break;
}
if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) {
PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i);
break;
}
if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
PX4_ERR("[%d]Error publishing the vechile command message", i);
break;
}
} else {
PX4_DEBUG("[%d] VC topic is not updated", i);
if (!vcmd_pub.publish(m_vc)) {
PX4_ERR("[%d]Error publishing the vechile command message", i);
break;
}
} else {
PX4_ERR("[%d]Error checking the updated status for vechile command... ", i);
PX4_DEBUG("[%d] VC topic is not updated", i);
break;
}
@ -127,51 +104,28 @@ int MuorbTestExample::DefaultTest()
int MuorbTestExample::PingPongTest()
{
int i = 0;
orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc);
if (pub_id_vc == nullptr) {
PX4_ERR("error publishing vehicle_command");
return -1;
}
if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
PX4_ERR("[%d]Error publishing the vechile command message", i);
return -1;
}
int sub_esc_status = orb_subscribe(ORB_ID(esc_status));
if (sub_esc_status == PX4_ERROR) {
PX4_ERR("Error subscribing to esc_status topic");
return -1;
}
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
uORB::Subscription sub_esc_status{ORB_ID(esc_status)};
while (!appState.exitRequested()) {
PX4_INFO("[%d] Doing work...", i);
bool updated = false;
if (orb_check(sub_esc_status, &updated) == 0) {
if (updated) {
PX4_INFO("[%d]ESC status is updated... reading new value", i);
if (sub_esc_status.updated()) {
PX4_INFO("[%d]ESC status is updated... reading new value", i);
if (orb_copy(ORB_ID(esc_status), sub_esc_status, &m_esc_status) != 0) {
PX4_ERR("[%d]Error calling orb copy for esc status... ", i);
break;
}
if (!sub_esc_status.copy(&m_esc_status)) {
PX4_ERR("[%d]Error calling orb copy for esc status... ", i);
break;
}
if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
PX4_ERR("[%d]Error publishing the vechile command message", i);
break;
}
} else {
PX4_INFO("[%d] esc status topic is not updated", i);
if (!vcmd_pub.publish(m_vc)) {
PX4_ERR("[%d]Error publishing the vechile command message", i);
break;
}
} else {
PX4_ERR("[%d]Error checking the updated status for esc status... ", i);
break;
PX4_INFO("[%d] esc status topic is not updated", i);
}
// sleep for 1 sec.

6
src/modules/navigator/navigator.h
View File

@ -60,6 +60,7 @@
#include <lib/perf/perf_counter.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/home_position.h>
@ -330,8 +331,9 @@ private:
uORB::Publication<vehicle_roi_s> _vehicle_roi_pub{ORB_ID(vehicle_roi)};
orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */
orb_advert_t _vehicle_cmd_ack_pub{nullptr};
orb_advert_t _vehicle_cmd_pub{nullptr};
uORB::PublicationQueued<vehicle_command_ack_s> _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::PublicationQueued<vehicle_command_s> _vehicle_cmd_pub{ORB_ID(vehicle_command)};
// Subscriptions
home_position_s _home_pos{}; /**< home position for RTL */

21
src/modules/navigator/navigator_main.cpp
View File

@ -903,7 +903,7 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi
// float vel_e = get_global_position()->vel_e;
// float vel_d = get_global_position()->vel_d;
transponder_report_s tr = {};
transponder_report_s tr{};
tr.timestamp = hrt_absolute_time();
tr.icao_address = 1234;
tr.lat = lat; // Latitude, expressed as degrees
@ -923,8 +923,8 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi
transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields
tr.squawk = 6667;
orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h);
uORB::PublicationQueued<transponder_report_s> tr_pub{ORB_ID(transponder_report)};
tr_pub.publish(tr);
}
void Navigator::check_traffic()
@ -1142,12 +1142,7 @@ Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
break;
}
if (_vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, vcmd);
} else {
_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
_vehicle_cmd_pub.publish(*vcmd);
}
void
@ -1165,13 +1160,7 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res
command_ack.result_param1 = 0;
command_ack.result_param2 = 0;
if (_vehicle_cmd_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), _vehicle_cmd_ack_pub, &command_ack);
} else {
_vehicle_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
}
_vehicle_cmd_ack_pub.publish(command_ack);
}
int Navigator::print_usage(const char *reason)

7
src/modules/sensors/voted_sensors_update.cpp
View File

@ -951,12 +951,7 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n
_info.enabled = true;
_info.ok = false;
if (_info_pub == nullptr) {
_info_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(subsystem_info), _info_pub, &_info);
}
_info_pub.publish(_info);
}
}
}

5
src/modules/sensors/voted_sensors_update.h
View File

@ -52,6 +52,7 @@
#include <lib/ecl/validation/data_validator.h>
#include <lib/ecl/validation/data_validator_group.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_correction.h>
@ -274,8 +275,8 @@ private:
bool _selection_changed = false; /**< true when a sensor selection has changed and not been published */
/* subsystem info publication */
struct subsystem_info_s _info;
orb_advert_t _info_pub = nullptr;
subsystem_info_s _info{};
uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)};
uint32_t _accel_device_id[SENSOR_COUNT_MAX] = {}; /**< accel driver device id for each uorb instance */
uint32_t _baro_device_id[SENSOR_COUNT_MAX] = {};

17
src/modules/uORB/Publication.hpp
View File

@ -45,8 +45,7 @@ namespace uORB
{
/**
* Base publication wrapper class, used in list traversal
* of various publications.
* Base publication wrapper class
*/
template<typename T>
class Publication
@ -61,7 +60,7 @@ public:
* @param priority The priority for multi pub/sub, 0-based, -1 means
* don't publish as multi
*/
Publication(const orb_metadata *meta, int priority = -1) : _meta(meta), _priority(priority) {}
Publication(const orb_metadata *meta, uint8_t priority = 0) : _meta(meta), _priority(priority) {}
~Publication() { orb_unadvertise(_handle); }
@ -74,12 +73,7 @@ public:
bool updated = false;
if (_handle != nullptr) {
if (orb_publish(_meta, _handle, &data) != PX4_OK) {
PX4_ERR("%s publish fail", _meta->o_name);
} else {
updated = true;
}
updated = (orb_publish(_meta, _handle, &data) == PX4_OK);
} else {
orb_advert_t handle = nullptr;
@ -106,13 +100,14 @@ public:
protected:
const orb_metadata *_meta;
const int _priority;
orb_advert_t _handle{nullptr};
const uint8_t _priority;
};
/**
* The publication base class as a list node.
* The publication class with data.
*/
template<typename T>
class PublicationData : public Publication<T>

97
src/modules/uORB/PublicationQueued.hpp Normal file
View File

@ -0,0 +1,97 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
/**
* @file PublicationQueued.hpp
*
*/
#pragma once
#include <uORB/uORB.h>
#include <px4_defines.h>
namespace uORB
{
/**
* Queued publication with queue length set as a message constant (ORB_QUEUE_LENGTH)
*/
template<typename T>
class PublicationQueued
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
*/
PublicationQueued(const orb_metadata *meta) : _meta(meta) {}
~PublicationQueued() { orb_unadvertise(_handle); }
/**
* Publish the struct
* @param data The uORB message struct we are updating.
*/
bool publish(const T &data)
{
bool updated = false;
if (_handle != nullptr) {
updated = (orb_publish(_meta, _handle, &data) == PX4_OK);
} else {
orb_advert_t handle = orb_advertise_queue(_meta, &data, T::ORB_QUEUE_LENGTH);
if (handle != nullptr) {
_handle = handle;
updated = true;
} else {
PX4_ERR("%s advertise failed", _meta->o_name);
}
}
return updated;
}
protected:
const orb_metadata *_meta;
orb_advert_t _handle{nullptr};
};
} // namespace uORB

13
src/modules/vmount/input_mavlink.cpp
View File

@ -39,7 +39,7 @@
*/
#include "input_mavlink.h"
#include <uORB/uORB.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/position_setpoint_triplet.h>
@ -333,7 +333,7 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd)
{
vehicle_command_ack_s vehicle_command_ack = {};
vehicle_command_ack_s vehicle_command_ack{};
vehicle_command_ack.timestamp = hrt_absolute_time();
vehicle_command_ack.command = cmd->command;
@ -341,13 +341,8 @@ void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd)
vehicle_command_ack.target_system = cmd->source_system;
vehicle_command_ack.target_component = cmd->source_component;
if (_vehicle_command_ack_pub == nullptr) {
_vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _vehicle_command_ack_pub, &vehicle_command_ack);
}
uORB::PublicationQueued<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
cmd_ack_pub.publish(vehicle_command_ack);
}
void InputMavlinkCmdMount::print_status()

5
src/modules/vmount/input_mavlink.h
View File

@ -43,13 +43,11 @@
#include "input_rc.h"
#include <cstdint>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_roi.h>
namespace vmount
{
/**
** class InputMavlinkROI
** Input based on the vehicle_roi topic
@ -95,7 +93,6 @@ private:
void _ack_vehicle_command(vehicle_command_s *cmd);
int _vehicle_command_sub = -1;
orb_advert_t _vehicle_command_ack_pub = nullptr;
bool _stabilize[3] = { false, false, false };
int32_t _mav_sys_id{1}; ///< our mavlink system id

24
src/modules/vmount/output_mavlink.cpp
View File

@ -54,16 +54,9 @@ OutputMavlink::OutputMavlink(const OutputConfig &output_config)
{
}
OutputMavlink::~OutputMavlink()
{
if (_vehicle_command_pub) {
orb_unadvertise(_vehicle_command_pub);
}
}
int OutputMavlink::update(const ControlData *control_data)
{
vehicle_command_s vehicle_command = {};
vehicle_command_s vehicle_command{};
vehicle_command.timestamp = hrt_absolute_time();
vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id;
vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id;
@ -82,18 +75,7 @@ int OutputMavlink::update(const ControlData *control_data)
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
}
if (_vehicle_command_pub) {
orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
} else {
_vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command,
vehicle_command_s::ORB_QUEUE_LENGTH);
}
}
if (!_vehicle_command_pub) {
return 0;
_vehicle_command_pub.publish(vehicle_command);
}
_handle_position_update();
@ -110,7 +92,7 @@ int OutputMavlink::update(const ControlData *control_data)
vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F;
vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F;
orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
_vehicle_command_pub.publish(vehicle_command);
_last_update = t;

11
src/modules/vmount/output_mavlink.h
View File

@ -41,13 +41,11 @@
#include "output.h"
#include <uORB/uORB.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/vehicle_command.h>
namespace vmount
{
/**
** class OutputMavlink
* Output via vehicle_command topic
@ -56,14 +54,15 @@ class OutputMavlink : public OutputBase
{
public:
OutputMavlink(const OutputConfig &output_config);
virtual ~OutputMavlink();
virtual ~OutputMavlink() = default;
virtual int update(const ControlData *control_data);
virtual void print_status();
private:
orb_advert_t _vehicle_command_pub = nullptr;
uORB::PublicationQueued<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
};

12
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -49,6 +49,7 @@
#include "vtol_att_control_main.h"
#include <systemlib/mavlink_log.h>
#include <matrix/matrix/math.hpp>
#include <uORB/PublicationQueued.hpp>
using namespace matrix;
@ -193,20 +194,15 @@ VtolAttitudeControl::handle_command()
// This might not be optimal but is better than no response at all.
if (_vehicle_cmd.from_external) {
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = _vehicle_cmd.command;
command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
command_ack.target_system = _vehicle_cmd.source_system;
command_ack.target_component = _vehicle_cmd.source_component;
if (_v_cmd_ack_pub == nullptr) {
_v_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _v_cmd_ack_pub, &command_ack);
}
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
command_ack_pub.publish(command_ack);
}
}
}

1
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -144,7 +144,6 @@ private:
orb_advert_t _actuators_0_pub{nullptr}; //input for the mixer (roll,pitch,yaw,thrust)
orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle
orb_advert_t _v_att_sp_pub{nullptr};
orb_advert_t _v_cmd_ack_pub{nullptr};
orb_advert_t _vtol_vehicle_status_pub{nullptr};
orb_advert_t _actuators_1_pub{nullptr};

12
src/platforms/common/px4_work_queue/WorkQueueManager.hpp
View File

@ -50,12 +50,12 @@ namespace wq_configurations
{
static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority
static constexpr wq_config_t SPI1{"wq:SPI1", 1250, -1};
static constexpr wq_config_t SPI2{"wq:SPI2", 1250, -2};
static constexpr wq_config_t SPI3{"wq:SPI3", 1250, -3};
static constexpr wq_config_t SPI4{"wq:SPI4", 1250, -4};
static constexpr wq_config_t SPI5{"wq:SPI5", 1250, -5};
static constexpr wq_config_t SPI6{"wq:SPI6", 1250, -6};
static constexpr wq_config_t SPI1{"wq:SPI1", 1400, -1};
static constexpr wq_config_t SPI2{"wq:SPI2", 1400, -2};
static constexpr wq_config_t SPI3{"wq:SPI3", 1400, -3};
static constexpr wq_config_t SPI4{"wq:SPI4", 1400, -4};
static constexpr wq_config_t SPI5{"wq:SPI5", 1400, -5};
static constexpr wq_config_t SPI6{"wq:SPI6", 1400, -6};
static constexpr wq_config_t I2C1{"wq:I2C1", 1250, -7};
static constexpr wq_config_t I2C2{"wq:I2C2", 1250, -8};

13
src/systemcmds/led_control/led_control.cpp
View File

@ -45,9 +45,10 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_led.h>
static void usage();
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/led_control.h>
static orb_advert_t led_control_pub = nullptr;
static void usage();
extern "C" {
__EXPORT int led_control_main(int argc, char *argv[]);
@ -57,12 +58,8 @@ static void publish_led_control(led_control_s &led_control)
{
led_control.timestamp = hrt_absolute_time();
if (led_control_pub == nullptr) {
led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(led_control), led_control_pub, &led_control);
}
uORB::PublicationQueued<led_control_s> led_control_pub{ORB_ID(led_control)};
led_control_pub.publish(led_control);
}
static void run_led_test1()

2
src/systemcmds/motor_test/CMakeLists.txt
View File

@ -36,7 +36,7 @@ px4_add_module(
STACK_MAIN 4096
COMPILE_FLAGS
SRCS
motor_test.c
motor_test.cpp
DEPENDS
)

38
src/systemcmds/motor_test/motor_test.c → src/systemcmds/motor_test/motor_test.cpp
View File

@ -38,48 +38,30 @@
* Tool for drive testing
*/
#include <drivers/drv_hrt.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_log.h>
#include <px4_module.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/test_motor.h>
__EXPORT int motor_test_main(int argc, char *argv[]);
extern "C" __EXPORT int motor_test_main(int argc, char *argv[]);
static void motor_test(unsigned channel, float value);
static void usage(const char *reason);
static orb_advert_t _test_motor_pub = NULL;
void motor_test(unsigned channel, float value)
{
struct test_motor_s _test_motor;
test_motor_s test_motor{};
test_motor.timestamp = hrt_absolute_time();
test_motor.motor_number = channel;
test_motor.value = value;
_test_motor.motor_number = channel;
_test_motor.timestamp = hrt_absolute_time();
_test_motor.value = value;
uORB::PublicationQueued<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
test_motor_pub.publish(test_motor);
if (_test_motor_pub != NULL) {
/* publish test state */
orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
} else {
/* advertise and publish */
_test_motor_pub = orb_advertise_queue(ORB_ID(test_motor), &_test_motor, 4);
}
printf("motor %d set to %.2f\n", channel, (double)value);
PX4_INFO("motor %d set to %.2f", channel, (double)value);
}
static void usage(const char *reason)