From ba89883fb012d4685b948a9afcb07a4fa5536e76 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 16 Jun 2015 15:44:58 +0530 Subject: [PATCH] camera trigger: minor cleanup --- src/modules/camera_trigger/camera_trigger.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 2a1632d1e4..a81607e950 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -58,7 +58,6 @@ #include #include #include - #include extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); @@ -152,7 +151,7 @@ CameraTrigger::CameraTrigger() : _integration_time(0.0f), _transfer_time(0.0f), _trigger_seq(0), - _trigger_enabled(true), + _trigger_enabled(false), _sensor_sub(-1), _vcommand_sub(-1), _trigger_pub(nullptr), @@ -191,16 +190,15 @@ CameraTrigger::start() param_get(integration_time, &_integration_time); param_get(transfer_time, &_transfer_time); - if(_polarity == 0) - { - //px4_ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ + stm32_configgpio(GPIO_GPIO0_OUTPUT); + + if(_polarity == 0) { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); /* GPIO pin pull high */ } - else if(_polarity == 1) - { - //px4_ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ + else if(_polarity == 1) { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); /* GPIO pin pull low */ } - else - { + else { warnx(" invalid trigger polarity setting. stopping."); stop(); }