Add camera_capture driver

This commit is contained in:
Mohammed Kabir
2018-04-15 13:32:12 -04:00
committed by Daniel Agar
parent 2fa70fca80
commit 94bb02a9c7
6 changed files with 518 additions and 1 deletions
+7
View File
@@ -376,6 +376,13 @@ else
camera_feedback start
fi
# Camera capture driver
if camera_capture start
then
set FMU_MODE pwm4
set AUX_MODE pwm4
fi
#
# Check if UAVCAN is enabled, default to it for ESCs.
#
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_module(
MODULE drivers__camera_capture
MAIN camera_capture
COMPILE_FLAGS
SRCS
camera_capture.cpp
DEPENDS
platforms__common
EXTERNAL
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
@@ -0,0 +1,286 @@
/****************************************************************************
*
* 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_capture.cpp
*
* Online and offline geotagging from camera feedback
*
* @author Mohammed Kabir <kabir@uasys.io>
*/
#include "camera_capture.hpp"
#define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
namespace camera_capture
{
CameraCapture *g_camera_capture;
}
CameraCapture::CameraCapture() :
_capture_enabled(false),
_trigger_pub(nullptr),
_command_ack_pub(nullptr),
_command_sub(-1),
_capture_seq(0),
_last_fall_time(0),
_last_exposure_time(0),
_capture_overflows(0)
{
memset(&_work, 0, sizeof(_work));
// Parameters
_p_strobe_delay = param_find("CAM_CAP_DELAY");
param_get(_p_strobe_delay, &_strobe_delay);
struct camera_trigger_s trigger = {};
_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
}
CameraCapture::~CameraCapture()
{
camera_capture::g_camera_capture = nullptr;
}
void
CameraCapture::capture_callback(uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
{
if (edge_state == 0) { // Falling edge
// Timestamp and compensate for strobe delay
_last_fall_time = edge_time - uint64_t(1000 * _strobe_delay);
} else if (edge_state == 1 && _last_fall_time > 0) { // Falling edge and got rising before
struct camera_trigger_s trigger {};
trigger.timestamp = edge_time - ((edge_time - _last_fall_time) / 2); // Get timestamp of mid-exposure
trigger.seq = _capture_seq++;
orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger);
_last_exposure_time = edge_time - _last_fall_time;
}
_capture_overflows = overflow;
}
void
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 *cap = reinterpret_cast<CameraCapture *>(arg);
if (cap->_command_sub < 0) {
cap->_command_sub = orb_subscribe(ORB_ID(vehicle_command));
}
bool updated = false;
orb_check(cap->_command_sub, &updated);
// Command handling
if (updated) {
vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), cap->_command_sub, &cmd);
// TODO : this should eventuallly be a capture control command
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
// Enable/disable signal capture
if (commandParamToInt(cmd.param1) == 1) {
cap->set_capture_control(true);
} else if (commandParamToInt(cmd.param1) == 0) {
cap->set_capture_control(false);
}
// Reset capture sequence
if (commandParamToInt(cmd.param2) == 1) {
cap->reset_statistics(true);
}
// 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
};
if (cap->_command_ack_pub == nullptr) {
cap->_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), cap->_command_ack_pub, &command_ack);
}
}
}
work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, camera_capture::g_camera_capture,
USEC2TICK(100000)); // 100ms
}
void
CameraCapture::set_capture_control(bool enabled)
{
if (enabled) {
// register callbacks
//up_input_capture_set(4, Both, 0, &CameraCapture::capture_trampoline, this);
up_input_capture_set(5, Both, 0, &CameraCapture::capture_trampoline, this);
_capture_enabled = true;
} else {
//up_input_capture_set(4, Disabled, 0, NULL, NULL);
up_input_capture_set(5, Disabled, 0, NULL, NULL);
_capture_enabled = false;
}
reset_statistics(false);
}
void
CameraCapture::reset_statistics(bool reset_seq)
{
if (reset_seq) { _capture_seq = 0; }
_last_fall_time = 0;
_last_exposure_time = 0;
_capture_overflows = 0;
}
void
CameraCapture::start()
{
// start to monitor at low rates for capture control commands
work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, this, USEC2TICK(1)); // TODO : is this low rate??!
}
void
CameraCapture::stop()
{
work_cancel(LPWORK, &_work);
if (camera_capture::g_camera_capture != nullptr) {
delete (camera_capture::g_camera_capture);
}
}
void
CameraCapture::status()
{
PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO");
PX4_INFO("Frame sequence : %u", _capture_seq);
PX4_INFO("Last fall timestamp : %llu", _last_fall_time);
PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0);
PX4_INFO("Number of overflows : %u", _capture_overflows);
}
static int usage()
{
PX4_INFO("usage: camera_capture {start|stop|on|off|reset}\n");
return 1;
}
extern "C" __EXPORT int camera_capture_main(int argc, char *argv[]);
int camera_capture_main(int argc, char *argv[])
{
if (argc < 2) {
return usage();
}
if (!strcmp(argv[1], "start")) {
if (camera_capture::g_camera_capture != nullptr) {
PX4_WARN("already running");
return 0;
}
camera_capture::g_camera_capture = new CameraCapture();
if (camera_capture::g_camera_capture == nullptr) {
PX4_WARN("alloc failed");
return 1;
}
camera_capture::g_camera_capture->start();
return 0;
}
if (camera_capture::g_camera_capture == nullptr) {
PX4_WARN("not running");
return 1;
} else if (!strcmp(argv[1], "stop")) {
camera_capture::g_camera_capture->stop();
} else if (!strcmp(argv[1], "status")) {
camera_capture::g_camera_capture->status();
} else if (!strcmp(argv[1], "on")) {
camera_capture::g_camera_capture->set_capture_control(true);
} else if (!strcmp(argv[1], "off")) {
camera_capture::g_camera_capture->set_capture_control(false);
} else if (!strcmp(argv[1], "reset")) {
camera_capture::g_camera_capture->set_capture_control(false);
camera_capture::g_camera_capture->reset_statistics(true);
} else {
return usage();
}
return 0;
}
@@ -0,0 +1,130 @@
/****************************************************************************
*
* 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_capture.hpp
*
*/
#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 <px4_config.h>
#include <px4_defines.h>
#include <px4_module.h>
#include <px4_tasks.h>
#include <px4_workqueue.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_input_capture.h>
#include <uORB/uORB.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
class CameraCapture
{
public:
/**
* Constructor
*/
CameraCapture();
/**
* Destructor, also kills task.
*/
~CameraCapture();
/**
* Start the task.
*/
void start();
/**
* Stop the task.
*/
void stop();
void status();
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);
void reset_statistics(bool reset_seq);
static struct work_s _work;
private:
bool _capture_enabled;
// Publishers
orb_advert_t _trigger_pub;
orb_advert_t _command_ack_pub;
// Subscribers
int _command_sub;
// Parameters
param_t _p_strobe_delay;
float _strobe_delay;
// Signal capture statistics
uint32_t _capture_seq;
hrt_abstime _last_fall_time;
hrt_abstime _last_exposure_time;
uint32_t _capture_overflows;
// Signal capture callback
void capture_callback(uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
// Low-rate command handling loop
static void cycle_trampoline(void *arg);
};
struct work_s CameraCapture::_work;
@@ -0,0 +1,51 @@
/****************************************************************************
*
* 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_capture_params.c
* Camera capture parameters
*
* @author Mohammed Kabir <kabir@uasys.io>
*/
/**
* Camera strobe delay
*
* This parameter sets the delay between image integration start and strobe firing
*
* @unit ms
* @min 0.0
* @max 100.0
* @decimal 1
* @group Camera Capture
*/
PARAM_DEFINE_FLOAT(CAM_CAP_DELAY, 1.0f);
+1 -1
View File
@@ -126,7 +126,7 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer,
if (channel_handlers[chan_index].callback) {
channel_handlers[chan_index].callback(channel_handlers[chan_index].context, chan_index,
channel_stats[chan_index].last_time,
channel_stats[chan_index].last_edge, overflow);
channel_stats[chan_index].last_edge, channel_stats[chan_index].overflows);
}
}