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

camera_capture: use up_input_capture_set directly

Browse Source 
It reserves the channel and pwm_out will not use it
This commit is contained in:
Beat Küng 2021-09-08 12:14:36 +02:00 committed by Daniel Agar
parent 78b5cdae4c
commit 062dd28f4d
7 changed files with 55 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c
View File

@ -106,11 +106,11 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer,
uint16_t capture = _REG32(timer->base, chan->ccr_offset);
channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in);
if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) {
channel_stats[chan_index].latnecy = (isrs_rcnt - capture);
if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) {
channel_stats[chan_index].latency = (isrs_rcnt - capture);
}
channel_stats[chan_index].chan_in_edges_out++;
channel_stats[chan_index].edges++;
channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture);
uint32_t overflow = _REG32(timer->base, STM32_GTIM_SR_OFFSET) & chan->masks & GTIM_SR_CCOF;

2
src/drivers/camera_capture/CMakeLists.txt
View File

@ -36,5 +36,7 @@ px4_add_module(
COMPILE_FLAGS
SRCS
camera_capture.cpp
DEPENDS
arch_io_pins
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

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

@ -43,6 +43,8 @@
#define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
static constexpr int capture_channel = 5; ///< FMU output 6
namespace camera_capture
{
CameraCapture *g_camera_capture{nullptr};
@ -216,63 +218,27 @@ CameraCapture::set_capture_control(bool enabled)
#else
int fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0) {
PX4_ERR("open fail");
return;
}
input_capture_config_t conf;
conf.channel = 5; // FMU chan 6
conf.filter = 0;
if (_camera_capture_mode == 0) {
conf.edge = _camera_capture_edge ? Rising : Falling;
} else {
conf.edge = Both;
}
conf.callback = nullptr;
conf.context = nullptr;
capture_callback_t callback = nullptr;
void *context = nullptr;
if (enabled) {
conf.callback = &CameraCapture::capture_trampoline;
conf.context = this;
unsigned int capture_count = 0;
if (::ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) {
PX4_INFO("Not in a capture mode");
unsigned long mode = PWM_SERVO_MODE_4PWM2CAP;
if (::ioctl(fd, PWM_SERVO_SET_MODE, mode) == 0) {
PX4_INFO("Mode changed to 4PWM2CAP");
} else {
PX4_ERR("Mode NOT changed to 4PWM2CAP!");
goto err_out;
}
}
callback = &CameraCapture::capture_trampoline;
context = this;
}
if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) {
int ret = up_input_capture_set_callback(capture_channel, callback, context);
if (ret == 0) {
_capture_enabled = enabled;
_gpio_capture = false;
} else {
PX4_ERR("Unable to set capture callback for chan %" PRIu8 "\n", conf.channel);
PX4_ERR("Unable to set capture callback for chan %" PRIu8 " (%i)", capture_channel, ret);
_capture_enabled = false;
goto err_out;
}
reset_statistics(false);
err_out:
::close(fd);
#endif
}
@ -292,6 +258,22 @@ CameraCapture::reset_statistics(bool reset_seq)
int
CameraCapture::start()
{
#if !defined(BOARD_CAPTURE_GPIO)
input_capture_edge edge = Both;
if (_camera_capture_mode == 0) {
edge = _camera_capture_edge ? Rising : Falling;
}
int ret = up_input_capture_set(capture_channel, edge, 0, nullptr, nullptr);
if (ret != 0) {
PX4_ERR("up_input_capture_set failed (%i)", ret);
return ret;
}
#endif
// run every 100 ms (10 Hz)
ScheduleOnInterval(100000, 10000);
@ -329,6 +311,26 @@ CameraCapture::status()
}
PX4_INFO("Number of overflows : %" PRIu32, _capture_overflows);
#if !defined(BOARD_CAPTURE_GPIO)
input_capture_stats_t stats;
int ret = up_input_capture_get_stats(capture_channel, &stats, false);
if (ret != 0) {
PX4_ERR("Unable to get stats for chan %" PRIu8 " (%i)", capture_channel, ret);
} else {
PX4_INFO("Status chan: %" PRIu8 " edges: %" PRIu32 " last time: %" PRIu64 " last state: %" PRIu32
" overflows: %" PRIu32 " latency: %" PRIu16,
capture_channel,
stats.edges,
stats.last_time,
stats.last_edge,
stats.overflows,
stats.latency);
}
#endif
}
static int usage()

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

@ -54,9 +54,6 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#define PX4FMU_DEVICE_PATH "/dev/px4fmu"
class CameraCapture : public px4::ScheduledWorkItem
{
public:

2
src/drivers/camera_trigger/interfaces/src/gpio.cpp
View File

@ -56,7 +56,7 @@ CameraInterfaceGPIO::~CameraInterfaceGPIO()
unsigned channel = 0;
while (_allocated_channels != 0) {
if (((1 << channel) & _allocated_channels)) {
if (((1u << channel) & _allocated_channels)) {
io_timer_unallocate_channel(channel);
_allocated_channels &= ~(1u << channel);
}

4
src/drivers/drv_input_capture.h
View File

@ -83,11 +83,11 @@ typedef struct input_capture_element_t {
} input_capture_element_t;
typedef struct input_capture_stats_t {
uint32_t chan_in_edges_out;
uint32_t edges;
uint32_t overflows;
uint32_t last_edge;
hrt_abstime last_time;
uint16_t latnecy;
uint16_t latency;
} input_capture_stats_t;
/**

78
src/drivers/pwm_out/PWMOut.cpp
View File

@ -1850,16 +1850,10 @@ int PWMOut::test(const char *dev)
{
int fd;
unsigned servo_count = 0;
unsigned capture_count = 0;
unsigned pwm_value = 1000;
int direction = 1;
int ret;
int rv = -1;
uint32_t rate_limit = 0;
struct input_capture_t {
bool valid;
input_capture_config_t chan;
} capture_conf[INPUT_CAPTURE_MAX_CHANNELS];
fd = ::open(dev, O_RDWR);
@ -1883,39 +1877,7 @@ int PWMOut::test(const char *dev)
goto err_out;
}
if (::ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) {
PX4_INFO("Not in a capture mode");
}
PX4_INFO("Testing %u servos and %u input captures", servo_count, capture_count);
memset(capture_conf, 0, sizeof(capture_conf));
if (capture_count != 0) {
for (unsigned i = 0; i < capture_count; i++) {
// Map to channel number
capture_conf[i].chan.channel = i + servo_count;
/* Save handler */
if (::ioctl(fd, INPUT_CAP_GET_CALLBACK, (unsigned long)&capture_conf[i].chan.channel) != 0) {
PX4_ERR("Unable to get capture callback for chan %" PRIu8 "\n", capture_conf[i].chan.channel);
goto err_out;
} else {
input_capture_config_t conf = capture_conf[i].chan;
conf.callback = &PWMOut::capture_trampoline;
conf.context = _objects[0].load(); // TODO PWMOut::get_instance();
if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) {
capture_conf[i].valid = true;
} else {
PX4_ERR("Unable to set capture callback for chan %" PRIu8 "\n", capture_conf[i].chan.channel);
goto err_out;
}
}
}
}
PX4_INFO("Testing %u servos", servo_count);
struct pollfd fds;
@ -1972,31 +1934,6 @@ int PWMOut::test(const char *dev)
}
}
if (capture_count != 0 && (++rate_limit % 500 == 0)) {
for (unsigned i = 0; i < capture_count; i++) {
if (capture_conf[i].valid) {
input_capture_stats_t stats;
stats.chan_in_edges_out = capture_conf[i].chan.channel;
if (::ioctl(fd, INPUT_CAP_GET_STATS, (unsigned long)&stats) != 0) {
PX4_ERR("Unable to get stats for chan %" PRIu8 "\n", capture_conf[i].chan.channel);
goto err_out;
} else {
fprintf(stdout, "FMU: Status chan:%" PRIu8 " edges: %" PRIu32 " last time:%" PRIu64 " last state:%" PRIu32
" overflows:%" PRIu32 " lantency:%" PRIu16 "\n",
capture_conf[i].chan.channel,
stats.chan_in_edges_out,
stats.last_time,
stats.last_edge,
stats.overflows,
stats.latnecy);
}
}
}
}
/* Check if user wants to quit */
char c;
ret = ::poll(&fds, 1, 0);
@ -2012,19 +1949,6 @@ int PWMOut::test(const char *dev)
}
}
if (capture_count != 0) {
for (unsigned i = 0; i < capture_count; i++) {
// Map to channel number
if (capture_conf[i].valid) {
/* Save handler */
if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&capture_conf[i].chan) != 0) {
PX4_ERR("Unable to set capture callback for chan %" PRIu8 "\n", capture_conf[i].chan.channel);
goto err_out;
}
}
}
}
rv = 0;
err_out: