yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

camera_feedback: move to WQ callback and uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-09-28 16:19:40 -04:00
parent 0420a4386d
commit e951531b12
20 changed files with 234 additions and 358 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
boards/aerotenna/ocpoc/default.cmake
View File

@@ -11,6 +11,7 @@ px4_add_board(
#barometer # all available barometer drivers
barometer/ms5611
batt_smbus
camera_capture
camera_trigger
differential_pressure # all available differential pressure drivers
distance_sensor # all available distance sensor drivers

2
boards/bitcraze/crazyflie/default.cmake
View File

@@ -15,7 +15,7 @@ px4_add_board(
px4fmu
MODULES
attitude_estimator_q
camera_feedback
#camera_feedback
commander
dataman
ekf2

1
boards/emlid/navio2/cross.cmake
View File

@@ -11,6 +11,7 @@ px4_add_board(
#barometer # all available barometer drivers
barometer/ms5611
batt_smbus
camera_capture
camera_trigger
differential_pressure # all available differential pressure drivers
distance_sensor # all available distance sensor drivers

1
boards/emlid/navio2/native.cmake
View File

@@ -9,6 +9,7 @@ px4_add_board(
#barometer # all available barometer drivers
barometer/ms5611
batt_smbus
camera_capture
camera_trigger
differential_pressure # all available differential pressure drivers
distance_sensor # all available distance sensor drivers

1
boards/nxp/fmuk66-v3/default.cmake
View File

@@ -18,6 +18,7 @@ px4_add_board(
barometer # all available barometer drivers
barometer/mpl3115a2
batt_smbus
camera_capture
camera_trigger
differential_pressure # all available differential pressure drivers
distance_sensor # all available distance sensor drivers

2
boards/parrot/bebop/default.cmake
View File

@@ -21,7 +21,7 @@ px4_add_board(
MODULES
attitude_estimator_q
camera_feedback
#camera_feedback
commander
dataman
ekf2

2
boards/px4/fmu-v2/default.cmake
View File

@@ -66,7 +66,7 @@ px4_add_board(
airspeed_selector
#attitude_estimator_q
battery_status
camera_feedback
#camera_feedback
commander
dataman
ekf2

4
boards/px4/fmu-v2/fixedwing.cmake
View File

@@ -21,8 +21,8 @@ px4_add_board(
#barometer # all available barometer drivers
barometer/ms5611
#batt_smbus
#camera_capture
#camera_trigger
camera_capture
camera_trigger
differential_pressure # all available differential pressure drivers
distance_sensor # all available distance sensor drivers
gps

4
boards/px4/fmu-v2/multicopter.cmake
View File

@@ -19,8 +19,8 @@ px4_add_board(
adc
barometer/ms5611
#batt_smbus
#camera_capture
#camera_trigger
camera_capture
camera_trigger
distance_sensor # all available distance sensor drivers
gps
imu/l3gd20

2
boards/px4/fmu-v2/test.cmake
View File

@@ -65,7 +65,7 @@ px4_add_board(
#airspeed_selector
#attitude_estimator_q
battery_status
camera_feedback
#camera_feedback
commander
dataman
#ekf2

2
boards/px4/fmu-v5/stackcheck.cmake
View File

@@ -60,7 +60,7 @@ px4_add_board(
airspeed_selector
attitude_estimator_q
battery_status
camera_feedback
#camera_feedback
commander
dataman
ekf2

1
boards/px4/sitl/default.cmake
View File

@@ -8,6 +8,7 @@ px4_add_board(
DRIVERS
#barometer # all available barometer drivers
#batt_smbus
camera_capture
camera_trigger
#differential_pressure # all available differential pressure drivers
#distance_sensor # all available distance sensor drivers

1
boards/px4/sitl/rtps.cmake
View File

@@ -8,6 +8,7 @@ px4_add_board(
DRIVERS
#barometer # all available barometer drivers
#batt_smbus
camera_capture
camera_trigger
#differential_pressure # all available differential pressure drivers
#distance_sensor # all available distance sensor drivers

1
boards/px4/sitl/test.cmake
View File

@@ -9,6 +9,7 @@ px4_add_board(
DRIVERS
#barometer # all available barometer drivers
#batt_smbus
camera_capture
camera_trigger
#differential_pressure # all available differential pressure drivers
#distance_sensor # all available distance sensor drivers

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

@@ -48,6 +48,8 @@ namespace camera_capture
CameraCapture *g_camera_capture{nullptr};
}
struct work_s CameraCapture::_work_publisher;
CameraCapture::CameraCapture() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
@@ -67,9 +69,7 @@ CameraCapture::CameraCapture() :
CameraCapture::~CameraCapture()
{
/* free any existing reports */
if (_trig_buffer != nullptr) {
delete _trig_buffer;
}
delete _trig_buffer;
camera_capture::g_camera_capture = nullptr;
}
@@ -229,8 +229,8 @@ CameraCapture::set_capture_control(bool enabled)
conf.edge = Both;
}
conf.callback = NULL;
conf.context = NULL;
conf.callback = nullptr;
conf.context = nullptr;
if (enabled) {
@@ -278,8 +278,6 @@ CameraCapture::set_capture_control(bool enabled)
err_out:
::close(fd);
#endif
return;
}
void
@@ -328,7 +326,7 @@ CameraCapture::status()
{
PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO");
PX4_INFO("Frame sequence : %u", _capture_seq);
PX4_INFO("Last trigger timestamp : %llu", _last_trig_time);
PX4_INFO("Last trigger timestamp : %" PRIu64 "", _last_trig_time);
if (_camera_capture_mode != 0) {
PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0);

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

@@ -149,5 +149,3 @@ private:
static void publish_trigger_trampoline(void *arg);
};
struct work_s CameraCapture::_work_publisher;

8
src/modules/camera_feedback/CMakeLists.txt
View File

@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2017 PX4 Development Team. All rights reserved.
# Copyright (c) 2017-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
@@ -33,9 +33,9 @@
px4_add_module(
MODULE modules__camera_feedback
MAIN camera_feedback
COMPILE_FLAGS
SRCS
camera_feedback.cpp
CameraFeedback.cpp
CameraFeedback.hpp
DEPENDS
px4_work_queue
)

176
src/modules/camera_feedback/CameraFeedback.cpp Normal file
View File

@@ -0,0 +1,176 @@
/****************************************************************************
*
* Copyright (c) 2017-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.
*
****************************************************************************/
#include "CameraFeedback.hpp"
CameraFeedback::CameraFeedback() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
}
bool
CameraFeedback::init()
{
if (!_trigger_sub.registerCallback()) {
PX4_ERR("camera_trigger callback registration failed!");
return false;
}
return true;
}
void
CameraFeedback::Run()
{
if (should_exit()) {
_trigger_sub.unregisterCallback();
exit_and_cleanup();
return;
}
camera_trigger_s trig{};
if (_trigger_sub.update(&trig)) {
// update geotagging subscriptions
vehicle_global_position_s gpos{};
_gpos_sub.copy(&gpos);
vehicle_attitude_s att{};
_att_sub.copy(&att);
if (trig.timestamp == 0 ||
gpos.timestamp == 0 ||
att.timestamp == 0) {
// reject until we have valid data
return;
}
camera_capture_s capture{};
// Fill timestamps
capture.timestamp = trig.timestamp;
capture.timestamp_utc = trig.timestamp_utc;
// Fill image sequence
capture.seq = trig.seq;
// Fill position data
capture.lat = gpos.lat;
capture.lon = gpos.lon;
capture.alt = gpos.alt;
if (gpos.terrain_alt_valid) {
capture.ground_distance = gpos.alt - gpos.terrain_alt;
} else {
capture.ground_distance = -1.0f;
}
// Fill attitude data
// TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
capture.q[0] = att.q[0];
capture.q[1] = att.q[1];
capture.q[2] = att.q[2];
capture.q[3] = att.q[3];
// Indicate whether capture feedback from camera is available
// What is case 0 for capture.result?
if (!_param_camera_capture_feedback.get()) {
capture.result = -1;
} else {
capture.result = 1;
}
_capture_pub.publish(capture);
}
}
int
CameraFeedback::task_spawn(int argc, char *argv[])
{
CameraFeedback *instance = new CameraFeedback();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int
CameraFeedback::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int
CameraFeedback::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("camera_feedback", "system");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int camera_feedback_main(int argc, char *argv[])
{
return CameraFeedback::main(argc, argv);
}

85
src/modules/camera_feedback/camera_feedback.hpp → src/modules/camera_feedback/CameraFeedback.hpp
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2017-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
@@ -32,81 +32,60 @@
****************************************************************************/
/**
* @file camera_feedback.hpp
*
* Online and offline geotagging from camera feedback
*
* @author Mohammed Kabir <kabir@uasys.io>
*/
#pragma once
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <stdbool.h>
#include <poll.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/camera_trigger.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/camera_capture.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
class CameraFeedback
class CameraFeedback : public ModuleBase<CameraFeedback>, public ModuleParams, public px4::WorkItem
{
public:
/**
* Constructor
*/
CameraFeedback();
~CameraFeedback() override = default;
/**
* Destructor, also kills task.
*/
~CameraFeedback();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/**
* Start the task.
*
* @return OK on success.
*/
int start();
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/**
* Stop the task.
*/
void stop();
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
void Run() override;
bool init();
private:
bool _task_should_exit; /**< if true, task should exit */
int _main_task; /**< handle for task */
uORB::SubscriptionCallbackWorkItem _trigger_sub{this, ORB_ID(camera_trigger)};
int _trigger_sub;
int _gpos_sub;
int _att_sub;
uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
orb_advert_t _capture_pub;
uORB::Publication<camera_capture_s> _capture_pub{ORB_ID(camera_capture)};
param_t _p_camera_capture_feedback;
int32_t _camera_capture_feedback;
void task_main();
/**
* Shim for calling task_main from task_create.
*/
static int task_main_trampoline(int argc, char *argv[]);
DEFINE_PARAMETERS(
(ParamBool<px4::params::CAM_CAP_FBACK>) _param_camera_capture_feedback
)
};

282
src/modules/camera_feedback/camera_feedback.cpp
View File

@@ -1,282 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 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 camera_feedback.cpp
*
* Online and offline geotagging from camera feedback
*
* @author Mohammed Kabir <kabir@uasys.io>
*/
#include "camera_feedback.hpp"
namespace camera_feedback
{
CameraFeedback *g_camera_feedback;
}
CameraFeedback::CameraFeedback() :
_task_should_exit(false),
_main_task(-1),
_trigger_sub(-1),
_gpos_sub(-1),
_att_sub(-1),
_capture_pub(nullptr),
_camera_capture_feedback(false)
{
// Parameters
_p_camera_capture_feedback = param_find("CAM_CAP_FBACK");
param_get(_p_camera_capture_feedback, (int32_t *)&_camera_capture_feedback);
}
CameraFeedback::~CameraFeedback()
{
if (_main_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
px4_usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
px4_task_delete(_main_task);
break;
}
} while (_main_task != -1);
}
camera_feedback::g_camera_feedback = nullptr;
}
int
CameraFeedback::start()
{
/* start the task */
_main_task = px4_task_spawn_cmd("camera_feedback",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 15,
1600,
(px4_main_t)&CameraFeedback::task_main_trampoline,
nullptr);
if (_main_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
void
CameraFeedback::stop()
{
if (camera_feedback::g_camera_feedback != nullptr) {
delete (camera_feedback::g_camera_feedback);
}
}
void
CameraFeedback::task_main()
{
_trigger_sub = orb_subscribe(ORB_ID(camera_trigger));
// Polling sources
struct camera_trigger_s trig = {};
px4_pollfd_struct_t fds[1] = {};
fds[0].fd = _trigger_sub;
fds[0].events = POLLIN;
// Geotagging subscriptions
_gpos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_global_position_s gpos = {};
struct vehicle_attitude_s att = {};
bool updated = false;
while (!_task_should_exit) {
/* wait for up to 20ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20);
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
continue;
}
/* trigger subscription updated */
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(camera_trigger), _trigger_sub, &trig);
/* update geotagging subscriptions */
orb_check(_gpos_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position), _gpos_sub, &gpos);
}
orb_check(_att_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
}
if (trig.timestamp == 0 ||
gpos.timestamp == 0 ||
att.timestamp == 0) {
// reject until we have valid data
continue;
}
struct camera_capture_s capture = {};
// Fill timestamps
capture.timestamp = trig.timestamp;
capture.timestamp_utc = trig.timestamp_utc;
// Fill image sequence
capture.seq = trig.seq;
// Fill position data
capture.lat = gpos.lat;
capture.lon = gpos.lon;
capture.alt = gpos.alt;
capture.ground_distance = gpos.terrain_alt_valid ? (gpos.alt - gpos.terrain_alt) : -1.0f;
// Fill attitude data
// TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
capture.q[0] = att.q[0];
capture.q[1] = att.q[1];
capture.q[2] = att.q[2];
capture.q[3] = att.q[3];
// Indicate whether capture feedback from camera is available
// What is case 0 for capture.result?
if (!_camera_capture_feedback) {
capture.result = -1;
} else {
capture.result = 1;
}
int instance_id;
orb_publish_auto(ORB_ID(camera_capture), &_capture_pub, &capture, &instance_id, ORB_PRIO_DEFAULT);
}
}
orb_unsubscribe(_trigger_sub);
orb_unsubscribe(_gpos_sub);
orb_unsubscribe(_att_sub);
_main_task = -1;
}
int
CameraFeedback::task_main_trampoline(int argc, char *argv[])
{
camera_feedback::g_camera_feedback->task_main();
return 0;
}
static int usage()
{
PX4_INFO("usage: camera_feedback {start|stop}\n");
return 1;
}
extern "C" __EXPORT int camera_feedback_main(int argc, char *argv[]);
int camera_feedback_main(int argc, char *argv[])
{
if (argc < 2) {
return usage();
}
if (!strcmp(argv[1], "start")) {
if (camera_feedback::g_camera_feedback != nullptr) {
PX4_WARN("already running");
return 0;
}
camera_feedback::g_camera_feedback = new CameraFeedback();
if (camera_feedback::g_camera_feedback == nullptr) {
PX4_WARN("alloc failed");
return 1;
}
camera_feedback::g_camera_feedback->start();
return 0;
}
if (camera_feedback::g_camera_feedback == nullptr) {
PX4_WARN("not running");
return 1;
} else if (!strcmp(argv[1], "stop")) {
camera_feedback::g_camera_feedback->stop();
} else {
return usage();
}
return 0;
}