Browse Source

camera_capture: remove board-specific ifdef

sbg
Beat Küng 5 years ago
parent
commit
316085c154
  1. 2
      boards/av/x-v1/src/board_config.h
  2. 28
      src/drivers/camera_capture/camera_capture.cpp
  3. 5
      src/drivers/camera_capture/camera_capture.hpp

2
boards/av/x-v1/src/board_config.h

@ -103,6 +103,8 @@
#define DIRECT_PWM_OUTPUT_CHANNELS 9 #define DIRECT_PWM_OUTPUT_CHANNELS 9
#define DIRECT_INPUT_TIMER_CHANNELS 9 #define DIRECT_INPUT_TIMER_CHANNELS 9
#define BOARD_CAPTURE_GPIO /* PD14 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN14)
/* High-resolution timer */ /* High-resolution timer */
#define HRT_TIMER 5 /* use timer5 for the HRT */ #define HRT_TIMER 5 /* use timer5 for the HRT */
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 3 */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 3 */

28
src/drivers/camera_capture/camera_capture.cpp

@ -209,7 +209,15 @@ CameraCapture::Run()
void void
CameraCapture::set_capture_control(bool enabled) CameraCapture::set_capture_control(bool enabled)
{ {
#if !defined CONFIG_ARCH_BOARD_AV_X_V1 // a board can define BOARD_CAPTURE_GPIO to use a separate capture pin
#if defined(BOARD_CAPTURE_GPIO)
px4_arch_gpiosetevent(BOARD_CAPTURE_GPIO, true, false, true, &CameraCapture::gpio_interrupt_routine, this);
_capture_enabled = enabled;
_gpio_capture = true;
reset_statistics(false);
#else
int fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR); int fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
@ -264,17 +272,8 @@ CameraCapture::set_capture_control(bool enabled)
goto err_out; goto err_out;
} }
#else
px4_arch_gpiosetevent(GPIO_TRIG_AVX, true, false, true, &CameraCapture::gpio_interrupt_routine, this);
_capture_enabled = enabled;
_gpio_capture = true;
#endif
reset_statistics(false); reset_statistics(false);
#if !defined CONFIG_ARCH_BOARD_AV_X_V1
err_out: err_out:
::close(fd); ::close(fd);
#endif #endif
@ -326,7 +325,14 @@ CameraCapture::status()
{ {
PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO"); PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO");
PX4_INFO("Frame sequence : %u", _capture_seq); PX4_INFO("Frame sequence : %u", _capture_seq);
PX4_INFO("Last trigger timestamp : %" PRIu64 "", _last_trig_time);
if (_last_trig_time != 0) {
PX4_INFO("Last trigger timestamp : %" PRIu64 " (%i ms ago)", _last_trig_time,
(int)(hrt_elapsed_time(&_last_trig_time) / 1000));
} else {
PX4_INFO("No trigger yet");
}
if (_camera_capture_mode != 0) { if (_camera_capture_mode != 0) {
PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0); PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0);

5
src/drivers/camera_capture/camera_capture.hpp

@ -57,9 +57,6 @@
#define PX4FMU_DEVICE_PATH "/dev/px4fmu" #define PX4FMU_DEVICE_PATH "/dev/px4fmu"
// For AV-X board
#define GPIO_TRIG_AVX /* PD14 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN14)
class CameraCapture : public px4::ScheduledWorkItem class CameraCapture : public px4::ScheduledWorkItem
{ {
@ -141,7 +138,7 @@ private:
// Signal capture callback // 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) // GPIO interrupt routine
static int gpio_interrupt_routine(int irq, void *context, void *arg); static int gpio_interrupt_routine(int irq, void *context, void *arg);
// Signal capture publish // Signal capture publish

Loading…
Cancel
Save