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

introduce uORB::PublicationQueued and transition most orb_advertise_queue usage

Browse Source
This commit is contained in:
Daniel Agar
2019-08-04 10:08:09 -04:00
parent 839787568c
commit e69398c09f
62 changed files with 421 additions and 674 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/camera_capture/camera_capture.cpp
+24 -81
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) {
src/drivers/camera_capture/camera_capture.hpp
+35 -41
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;