diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index bfbd770c8b..c4d829047e 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -89,21 +89,21 @@ public: * Display info. */ void info(); - + int pin; - + private: - + struct hrt_call _pollcall; struct hrt_call _firecall; - + int _gpio_fd; int _polarity; float _activation_time; float _integration_time; float _transfer_time; - uint32_t _trigger_seq; + uint32_t _trigger_seq; bool _trigger_enabled; int _sensor_sub; @@ -116,10 +116,10 @@ private: struct vehicle_command_s _command; param_t polarity ; - param_t activation_time ; + param_t activation_time ; param_t integration_time ; param_t transfer_time ; - + /** * Topic poller to check for fire info. */ @@ -162,13 +162,13 @@ CameraTrigger::CameraTrigger() : memset(&_trigger, 0, sizeof(_trigger)); memset(&_sensor, 0, sizeof(_sensor)); memset(&_command, 0, sizeof(_command)); - + memset(&_pollcall, 0, sizeof(_pollcall)); - memset(&_firecall, 0, sizeof(_firecall)); + memset(&_firecall, 0, sizeof(_firecall)); // Parameters polarity = param_find("TRIG_POLARITY"); - activation_time = param_find("TRIG_ACT_TIME"); + activation_time = param_find("TRIG_ACT_TIME"); integration_time = param_find("TRIG_INT_TIME"); transfer_time = param_find("TRIG_TRANS_TIME"); } @@ -184,37 +184,37 @@ CameraTrigger::start() _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); _vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); - - param_get(polarity, &_polarity); - param_get(activation_time, &_activation_time); - param_get(integration_time, &_integration_time); - param_get(transfer_time, &_transfer_time); - + + param_get(polarity, &_polarity); + param_get(activation_time, &_activation_time); + param_get(integration_time, &_integration_time); + param_get(transfer_time, &_transfer_time); + stm32_configgpio(GPIO_GPIO0_OUTPUT); - if(_polarity == 0) { + if (_polarity == 0) { stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); // GPIO pin pull high - } - else if(_polarity == 1) { + + } else if (_polarity == 1) { stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); // GPIO pin pull low - } - else { + + } else { warnx(" invalid trigger polarity setting. stopping."); stop(); } - + poll(this); // Trampoline call } void CameraTrigger::stop() -{ +{ hrt_cancel(&_firecall); hrt_cancel(&_pollcall); if (camera_trigger::g_camera_trigger != nullptr) { - delete (camera_trigger::g_camera_trigger); + delete(camera_trigger::g_camera_trigger); } } @@ -226,60 +226,55 @@ CameraTrigger::poll(void *arg) bool updated; orb_check(trig->_vcommand_sub, &updated); - + if (updated) { - + orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command); - - if(trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) - { - if(trig->_command.param1 < 1) - { - if(trig->_trigger_enabled) - { - trig->_trigger_enabled = false ; + + if (trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { + if (trig->_command.param1 < 1) { + if (trig->_trigger_enabled) { + trig->_trigger_enabled = false ; } - } - else if(trig->_command.param1 >= 1) - { - if(!trig->_trigger_enabled) - { + + } else if (trig->_command.param1 >= 1) { + if (!trig->_trigger_enabled) { trig->_trigger_enabled = true ; - } + } } // Set trigger rate from command - if(trig->_command.param2 > 0) - { + if (trig->_command.param2 > 0) { trig->_integration_time = trig->_command.param2; param_set(trig->integration_time, &(trig->_integration_time)); - } + } } } - if(!trig->_trigger_enabled) { - hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig); + if (!trig->_trigger_enabled) { + hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig); return; - } - else - { + + } else { engage(trig); - hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig); - + hrt_call_after(&trig->_firecall, trig->_activation_time * 1000, (hrt_callout)&CameraTrigger::disengage, trig); + orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); - + trig->_trigger.timestamp = trig->_sensor.timestamp; // get IMU timestamp trig->_trigger.seq = trig->_trigger_seq++; if (trig->_trigger_pub != nullptr) { orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trig->_trigger); + } else { trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } - hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000, (hrt_callout)&CameraTrigger::poll, trig); + hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time) * 1000, + (hrt_callout)&CameraTrigger::poll, trig); } - + } void @@ -289,17 +284,15 @@ CameraTrigger::engage(void *arg) CameraTrigger *trig = reinterpret_cast(arg); stm32_configgpio(GPIO_GPIO0_OUTPUT); - - if(trig->_polarity == 0) // ACTIVE_LOW - { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); + + if (trig->_polarity == 0) { // ACTIVE_LOW + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); + + } else if (trig->_polarity == 1) { // ACTIVE_HIGH + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); } - else if(trig->_polarity == 1) // ACTIVE_HIGH - { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); - } - - + + } void @@ -307,18 +300,16 @@ CameraTrigger::disengage(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - + stm32_configgpio(GPIO_GPIO0_OUTPUT); - - if(trig->_polarity == 0) // ACTIVE_LOW - { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); + + if (trig->_polarity == 0) { // ACTIVE_LOW + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); + + } else if (trig->_polarity == 1) { // ACTIVE_HIGH + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); } - else if(trig->_polarity == 1) // ACTIVE_HIGH - { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); - } - + } void @@ -333,8 +324,8 @@ CameraTrigger::info() static void usage() { errx(1, "usage: camera_trigger {start|stop|info} [-p ]\n" - "\t-p \tUse specified AUX OUT pin number (default: 1)" - ); + "\t-p \tUse specified AUX OUT pin number (default: 1)" + ); } int camera_trigger_main(int argc, char *argv[]) @@ -348,25 +339,26 @@ int camera_trigger_main(int argc, char *argv[]) if (camera_trigger::g_camera_trigger != nullptr) { errx(0, "already running"); } - + camera_trigger::g_camera_trigger = new CameraTrigger; if (camera_trigger::g_camera_trigger == nullptr) { errx(1, "alloc failed"); } - + if (argc > 3) { - + camera_trigger::g_camera_trigger->pin = (int)argv[3]; - if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) { - warnx("starting trigger on pin : %li ", atoi(argv[3])); + + if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) { + warnx("starting trigger on pin : %li ", atoi(argv[3])); camera_trigger::g_camera_trigger->pin = atoi(argv[3]); - } - else - { - usage(); + + } else { + usage(); } } + camera_trigger::g_camera_trigger->start(); return 0; @@ -377,11 +369,10 @@ int camera_trigger_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { - camera_trigger::g_camera_trigger->stop(); + camera_trigger::g_camera_trigger->stop(); - } - else if (!strcmp(argv[1], "info")) { - camera_trigger::g_camera_trigger->info(); + } else if (!strcmp(argv[1], "info")) { + camera_trigger::g_camera_trigger->info(); } else { usage();