mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-07 01:04:19 +08:00
camera_trigger : add default mode for generic PWM triggering
This commit is contained in:
committed by
Lorenz Meier
parent
9be7ad5805
commit
f9862ec5a8
@@ -65,8 +65,9 @@
|
||||
#include <board_config.h>
|
||||
|
||||
#include "interfaces/src/camera_interface.h"
|
||||
#include "interfaces/src/seagull_map2.h"
|
||||
#include "interfaces/src/gpio.h"
|
||||
#include "interfaces/src/pwm.h"
|
||||
#include "interfaces/src/seagull_map2.h"
|
||||
|
||||
#define TRIGGER_PIN_DEFAULT 1
|
||||
|
||||
@@ -76,7 +77,8 @@ typedef enum : int32_t {
|
||||
CAMERA_INTERFACE_MODE_NONE = 0,
|
||||
CAMERA_INTERFACE_MODE_GPIO,
|
||||
CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM,
|
||||
CAMERA_INTERFACE_MODE_MAVLINK
|
||||
CAMERA_INTERFACE_MODE_MAVLINK,
|
||||
CAMERA_INTERFACE_MODE_GENERIC_PWM
|
||||
} camera_interface_mode_t;
|
||||
|
||||
class CameraTrigger
|
||||
@@ -255,6 +257,10 @@ CameraTrigger::CameraTrigger() :
|
||||
_camera_interface = new CameraInterfaceGPIO();
|
||||
break;
|
||||
|
||||
case CAMERA_INTERFACE_MODE_GENERIC_PWM:
|
||||
_camera_interface = new CameraInterfacePWM();
|
||||
break;
|
||||
|
||||
case CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM:
|
||||
_camera_interface = new CameraInterfaceSeagull();
|
||||
break;
|
||||
@@ -555,7 +561,6 @@ CameraTrigger::disengage(void *arg)
|
||||
void
|
||||
CameraTrigger::engange_turn_on_off(void *arg)
|
||||
{
|
||||
|
||||
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
|
||||
|
||||
trig->_camera_interface->turn_on_off(true);
|
||||
@@ -598,7 +603,7 @@ CameraTrigger::status()
|
||||
|
||||
static int usage()
|
||||
{
|
||||
PX4_INFO("usage: camera_trigger {start|stop|status|test}\n");
|
||||
PX4_INFO("usage: camera_trigger {start|stop|enable|disable|status|test}\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
@@ -45,14 +45,15 @@
|
||||
* Selects the trigger interface
|
||||
*
|
||||
* @value 1 GPIO
|
||||
* @value 2 Seagull MAP2 (PWM)
|
||||
* @value 2 Seagull MAP2 (over PWM)
|
||||
* @value 3 MAVLink (forward via MAV_CMD_IMAGE_START_CAPTURE)
|
||||
* @value 4 Generic PWM (IR trigger, servo)
|
||||
*
|
||||
* @reboot_required true
|
||||
*
|
||||
* @group Camera trigger
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TRIG_INTERFACE, 2);
|
||||
PARAM_DEFINE_INT32(TRIG_INTERFACE, 4);
|
||||
|
||||
/**
|
||||
* Camera trigger interval
|
||||
|
||||
@@ -17,8 +17,6 @@
|
||||
#define PWM_2_CAMERA_KEEP_ALIVE 1700
|
||||
#define PWM_2_CAMERA_ON_OFF 1900
|
||||
|
||||
// TODO : cleanup using arraySize() macro
|
||||
|
||||
CameraInterfaceSeagull::CameraInterfaceSeagull():
|
||||
CameraInterface(),
|
||||
_camera_is_on(false)
|
||||
@@ -53,7 +51,7 @@ CameraInterfaceSeagull::CameraInterfaceSeagull():
|
||||
|
||||
CameraInterfaceSeagull::~CameraInterfaceSeagull()
|
||||
{
|
||||
// Deinitialise pwm channels
|
||||
// Deinitialise trigger channels
|
||||
up_pwm_trigger_deinit();
|
||||
}
|
||||
|
||||
@@ -65,6 +63,9 @@ void CameraInterfaceSeagull::setup()
|
||||
up_pwm_trigger_init(pin_bitmask);
|
||||
up_pwm_trigger_set(_pins[i + 1], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
|
||||
up_pwm_trigger_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
|
||||
|
||||
// We only support 2 consecutive pins while using the Seagull MAP2
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user