diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 2cac457366..7cf285de8d 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -392,7 +392,6 @@ CameraTrigger::test() { struct vehicle_command_s cmd = {}; cmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; - cmd.param5 = 1.0f; orb_advert_t pub; @@ -478,7 +477,8 @@ CameraTrigger::cycle_trampoline(void *arg) if (cmd.param1 > 0.0f && !trig->_trigger_enabled) { trig->turnOnOff(); trig->keepAlive(true); - poll_interval_usec = 2000000; + // Give the camera time to turn on, before starting to send trigger signals + poll_interval_usec = 5000000; turning_on = true; } else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) { diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 421beb8ae9..2da8c6bda4 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -67,7 +67,7 @@ PARAM_DEFINE_INT32(TRIG_INTERFACE, 2); * @decimal 1 * @group Camera trigger */ -PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 3000.0f); +PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 40.0f); /** * Camera trigger polarity @@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0); * @decimal 1 * @group Camera trigger */ -PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 500.0f); +PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f); /** * Camera trigger mode @@ -115,7 +115,7 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0); * * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6 on px4fmu-v2 and the rail * pins on px4fmu-v4). The PWM interface takes two pins per camera, while relay - * triggers on every pin individually. Example: Value 34 would trigger on pins 3 and 4. + * triggers on every pin individually. Example: Value 56 would trigger on pins 5 and 6. * * @min 1 * @max 123456 diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp index 984968af47..15942946c7 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -2,7 +2,6 @@ #include #include -#include #include "drivers/drv_pwm_output.h" #include "pwm.h" @@ -19,7 +18,6 @@ CameraInterfacePWM::CameraInterfacePWM(): CameraInterface(), - _pwm_pub {nullptr, nullptr}, _camera_is_on(false) { _p_pin = param_find("TRIG_PINS"); @@ -36,24 +34,17 @@ CameraInterfacePWM::CameraInterfacePWM(): int single_pin; while ((single_pin = pin_list % 10)) { + _pins[i] = single_pin - 1; if (_pins[i] < 0) { _pins[i] = -1; } - // Wingtra safety check, pins can only be on 5 or 6!!! - if (_pins[i] != 4 && _pins[i] != 5) { - _pins[i] = -1; - } - pin_list /= 10; i++; } - _orb_id[0] = 0; - _orb_id[1] = 1; - setup(); } @@ -72,13 +63,6 @@ void CameraInterfacePWM::setup() up_pwm_servo_init(pin_bitmask); up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); - - trigger_pwm_s pwm; - pwm.timestamp = hrt_absolute_time(); - pwm.pwm = PWM_CAMERA_DISARMED; - orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT); - pwm.pwm = PWM_CAMERA_DISARMED; - orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT); } } } @@ -91,18 +75,11 @@ void CameraInterfacePWM::trigger(bool enable) return; } - - trigger_pwm_s pwm; - pwm.timestamp = hrt_absolute_time(); - if (enable) { // Set all valid pins to shoot level for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000)); - - pwm.pwm = PWM_CAMERA_INSTANT_SHOOT; - orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT); } } @@ -111,9 +88,6 @@ void CameraInterfacePWM::trigger(bool enable) for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); - - pwm.pwm = PWM_CAMERA_NEUTRAL; - orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT); } } } @@ -127,17 +101,11 @@ void CameraInterfacePWM::keep_alive(bool signal_on) return; } - trigger_pwm_s pwm; - pwm.timestamp = hrt_absolute_time(); - if (signal_on) { // Set channel 2 pin to keep_alive signal for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_KEEP_ALIVE, 1000, 2000)); - - pwm.pwm = PWM_2_CAMERA_KEEP_ALIVE; - orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT); } } @@ -146,9 +114,6 @@ void CameraInterfacePWM::keep_alive(bool signal_on) for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); - - pwm.pwm = PWM_CAMERA_NEUTRAL; - orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT); } } } @@ -156,21 +121,12 @@ void CameraInterfacePWM::keep_alive(bool signal_on) void CameraInterfacePWM::turn_on_off(bool enable) { - - trigger_pwm_s pwm; - pwm.timestamp = hrt_absolute_time(); - if (enable) { // For now, set channel one on neutral upon startup. for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000)); - - pwm.pwm = PWM_CAMERA_NEUTRAL; - orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT); - pwm.pwm = PWM_2_CAMERA_ON_OFF; - orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT); } } @@ -180,11 +136,6 @@ void CameraInterfacePWM::turn_on_off(bool enable) if (_pins[i] >= 0 && _pins[i + 1] >= 0) { up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); - - pwm.pwm = PWM_CAMERA_NEUTRAL; - orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT); - pwm.pwm = PWM_CAMERA_NEUTRAL; - orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT); } } diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h index 62ed1838cf..1a61a2a791 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.h +++ b/src/drivers/camera_trigger/interfaces/src/pwm.h @@ -29,10 +29,7 @@ public: private: void setup(); - param_t _p_pin; - orb_advert_t _pwm_pub[2]; - int _orb_id[2]; - - bool _camera_is_on; + param_t _p_pin; + bool _camera_is_on; };