forked from Archive/PX4-Autopilot
[DO NOT MERGE] Nora PWM12 mode + camera_trigger hacks
This commit is contained in:
parent
e2563388eb
commit
64b5277c5d
|
@ -382,23 +382,25 @@ else
|
|||
|
||||
if param greater -s TRIG_MODE 0
|
||||
then
|
||||
# We ONLY support trigger on pins 5+6 or 7+8 when simultanously using AUX for actuator output.
|
||||
if param compare TRIG_PINS 56
|
||||
then
|
||||
# clear pins 5 and 6
|
||||
set FMU_MODE pwm4
|
||||
set AUX_MODE pwm4
|
||||
else
|
||||
if param compare TRIG_PINS 78
|
||||
then
|
||||
# clear pins 7 and 8
|
||||
set FMU_MODE pwm6
|
||||
set AUX_MODE pwm6
|
||||
else
|
||||
set FMU_MODE none
|
||||
set AUX_MODE none
|
||||
fi
|
||||
fi
|
||||
# # We ONLY support trigger on pins 5+6 or 7+8 when simultanously using AUX for actuator output.
|
||||
# if param compare TRIG_PINS 56
|
||||
# then
|
||||
# # clear pins 5 and 6
|
||||
# set FMU_MODE pwm4
|
||||
# set AUX_MODE pwm4
|
||||
# else
|
||||
# if param compare TRIG_PINS 78
|
||||
# then
|
||||
# # clear pins 7 and 8
|
||||
# set FMU_MODE pwm6
|
||||
# set AUX_MODE pwm6
|
||||
# else
|
||||
# set FMU_MODE none
|
||||
# set AUX_MODE none
|
||||
# fi
|
||||
# fi
|
||||
|
||||
set FMU_MODE pwm12
|
||||
|
||||
camera_trigger start
|
||||
camera_feedback start
|
||||
|
|
|
@ -69,8 +69,6 @@
|
|||
#include "interfaces/src/pwm.h"
|
||||
#include "interfaces/src/seagull_map2.h"
|
||||
|
||||
extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);
|
||||
|
||||
typedef enum : int32_t {
|
||||
CAMERA_INTERFACE_MODE_NONE = 0,
|
||||
CAMERA_INTERFACE_MODE_GPIO,
|
||||
|
@ -154,41 +152,41 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
struct hrt_call _engagecall;
|
||||
struct hrt_call _disengagecall;
|
||||
struct hrt_call _engage_turn_on_off_call;
|
||||
struct hrt_call _disengage_turn_on_off_call;
|
||||
struct hrt_call _keepalivecall_up;
|
||||
struct hrt_call _keepalivecall_down;
|
||||
struct hrt_call _engagecall {};
|
||||
struct hrt_call _disengagecall {};
|
||||
struct hrt_call _engage_turn_on_off_call {};
|
||||
struct hrt_call _disengage_turn_on_off_call {};
|
||||
struct hrt_call _keepalivecall_up {};
|
||||
struct hrt_call _keepalivecall_down {};
|
||||
|
||||
float _activation_time;
|
||||
float _interval;
|
||||
float _min_interval;
|
||||
float _distance;
|
||||
uint32_t _trigger_seq;
|
||||
hrt_abstime _last_trigger_timestamp;
|
||||
bool _trigger_enabled;
|
||||
bool _trigger_paused;
|
||||
bool _one_shot;
|
||||
bool _test_shot;
|
||||
bool _turning_on;
|
||||
matrix::Vector2f _last_shoot_position;
|
||||
bool _valid_position;
|
||||
float _activation_time{0.5f};
|
||||
float _interval{100.f};
|
||||
float _min_interval{1.f};
|
||||
float _distance{25.f};
|
||||
uint32_t _trigger_seq{0};
|
||||
hrt_abstime _last_trigger_timestamp{0};
|
||||
bool _trigger_enabled{false};
|
||||
bool _trigger_paused{false};
|
||||
bool _one_shot{false};
|
||||
bool _test_shot{false};
|
||||
bool _turning_on{false};
|
||||
matrix::Vector2f _last_shoot_position{0.f, 0.f};
|
||||
bool _valid_position{false};
|
||||
|
||||
//Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
|
||||
uint32_t _CAMPOS_num_poses;
|
||||
uint32_t _CAMPOS_pose_counter;
|
||||
float _CAMPOS_roll_angle;
|
||||
float _CAMPOS_angle_interval;
|
||||
float _CAMPOS_pitch_angle;
|
||||
bool _CAMPOS_updated_roll_angle;
|
||||
uint32_t _target_system;
|
||||
uint32_t _target_component;
|
||||
uint32_t _CAMPOS_num_poses{0};
|
||||
uint32_t _CAMPOS_pose_counter{0};
|
||||
float _CAMPOS_roll_angle{0.f};
|
||||
float _CAMPOS_angle_interval{0.f};
|
||||
float _CAMPOS_pitch_angle{-90.f};
|
||||
bool _CAMPOS_updated_roll_angle{false};
|
||||
uint32_t _target_system{0};
|
||||
uint32_t _target_component{0};
|
||||
|
||||
uORB::Subscription _command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)};
|
||||
|
||||
orb_advert_t _trigger_pub;
|
||||
orb_advert_t _trigger_pub{nullptr};
|
||||
|
||||
uORB::Publication<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
|
||||
|
@ -200,11 +198,11 @@ private:
|
|||
param_t _p_interface;
|
||||
param_t _p_cam_cap_fback;
|
||||
|
||||
trigger_mode_t _trigger_mode;
|
||||
int32_t _cam_cap_fback;
|
||||
trigger_mode_t _trigger_mode{TRIGGER_MODE_NONE};
|
||||
int32_t _cam_cap_fback{0};
|
||||
|
||||
camera_interface_mode_t _camera_interface_mode;
|
||||
CameraInterface *_camera_interface; ///< instance of camera interface
|
||||
camera_interface_mode_t _camera_interface_mode{CAMERA_INTERFACE_MODE_GPIO};
|
||||
CameraInterface *_camera_interface{nullptr}; ///< instance of camera interface
|
||||
|
||||
/**
|
||||
* Vehicle command handler
|
||||
|
@ -248,39 +246,7 @@ CameraTrigger *g_camera_trigger;
|
|||
}
|
||||
|
||||
CameraTrigger::CameraTrigger() :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
|
||||
_engagecall {},
|
||||
_disengagecall {},
|
||||
_engage_turn_on_off_call {},
|
||||
_disengage_turn_on_off_call {},
|
||||
_keepalivecall_up {},
|
||||
_keepalivecall_down {},
|
||||
_activation_time(0.5f /* ms */),
|
||||
_interval(100.0f /* ms */),
|
||||
_min_interval(1.0f /* ms */),
|
||||
_distance(25.0f /* m */),
|
||||
_trigger_seq(0),
|
||||
_last_trigger_timestamp(0),
|
||||
_trigger_enabled(false),
|
||||
_trigger_paused(false),
|
||||
_one_shot(false),
|
||||
_test_shot(false),
|
||||
_turning_on(false),
|
||||
_last_shoot_position(0.0f, 0.0f),
|
||||
_valid_position(false),
|
||||
_CAMPOS_num_poses(0),
|
||||
_CAMPOS_pose_counter(0),
|
||||
_CAMPOS_roll_angle(0.0f),
|
||||
_CAMPOS_angle_interval(0.0f),
|
||||
_CAMPOS_pitch_angle(-90),
|
||||
_CAMPOS_updated_roll_angle(false),
|
||||
_target_system(0),
|
||||
_target_component(0),
|
||||
_trigger_pub(nullptr),
|
||||
_trigger_mode(TRIGGER_MODE_NONE),
|
||||
_cam_cap_fback(0),
|
||||
_camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO),
|
||||
_camera_interface(nullptr)
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
|
||||
{
|
||||
// Initiate camera interface based on camera_interface_mode
|
||||
if (_camera_interface != nullptr) {
|
||||
|
@ -338,13 +304,14 @@ CameraTrigger::CameraTrigger() :
|
|||
if ((_activation_time < 40.0f) &&
|
||||
(_camera_interface_mode == CAMERA_INTERFACE_MODE_GENERIC_PWM ||
|
||||
_camera_interface_mode == CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM)) {
|
||||
|
||||
_activation_time = 40.0f;
|
||||
PX4_WARN("Trigger interval too low for PWM interface, setting to 40 ms");
|
||||
param_set_no_notification(_p_activation_time, &(_activation_time));
|
||||
}
|
||||
|
||||
// Advertise critical publishers here, because we cannot advertise in interrupt context
|
||||
struct camera_trigger_s trigger = {};
|
||||
camera_trigger_s trigger{};
|
||||
|
||||
if (!_cam_cap_fback) {
|
||||
_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
|
||||
|
@ -370,13 +337,12 @@ CameraTrigger::update_intervalometer()
|
|||
// control_intervalometer once on enabling/disabling trigger to schedule the calls.
|
||||
|
||||
if (_trigger_enabled && !_trigger_paused) {
|
||||
PX4_INFO("update intervalometer, trigger enabled: %d, trigger paused: %d", _trigger_enabled, _trigger_paused);
|
||||
// schedule trigger on and off calls
|
||||
hrt_call_every(&_engagecall, 0, (_interval * 1000),
|
||||
(hrt_callout)&CameraTrigger::engage, this);
|
||||
hrt_call_every(&_engagecall, 0, (_interval * 1000), &CameraTrigger::engage, this);
|
||||
|
||||
// schedule trigger on and off calls
|
||||
hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000),
|
||||
(hrt_callout)&CameraTrigger::disengage, this);
|
||||
hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000), &CameraTrigger::disengage, this);
|
||||
|
||||
}
|
||||
}
|
||||
|
@ -392,7 +358,6 @@ CameraTrigger::update_distance()
|
|||
_lpos_sub.copy(&local);
|
||||
|
||||
if (local.xy_valid) {
|
||||
|
||||
// Initialize position if not done yet
|
||||
matrix::Vector2f current_position(local.x, local.y);
|
||||
|
||||
|
@ -426,14 +391,15 @@ void
|
|||
CameraTrigger::enable_keep_alive(bool on)
|
||||
{
|
||||
if (on) {
|
||||
// schedule keep-alive up and down calls
|
||||
hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000),
|
||||
(hrt_callout)&CameraTrigger::keep_alive_up, this);
|
||||
PX4_INFO("keep alive enable");
|
||||
|
||||
hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000),
|
||||
(hrt_callout)&CameraTrigger::keep_alive_down, this);
|
||||
// schedule keep-alive up and down calls
|
||||
hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000), &CameraTrigger::keep_alive_up, this);
|
||||
|
||||
hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000), &CameraTrigger::keep_alive_down, this);
|
||||
|
||||
} else {
|
||||
PX4_INFO("keep alive disable");
|
||||
// cancel all calls
|
||||
hrt_cancel(&_keepalivecall_up);
|
||||
hrt_cancel(&_keepalivecall_down);
|
||||
|
@ -443,23 +409,25 @@ CameraTrigger::enable_keep_alive(bool on)
|
|||
void
|
||||
CameraTrigger::toggle_power()
|
||||
{
|
||||
// schedule power toggle calls
|
||||
hrt_call_after(&_engage_turn_on_off_call, 0,
|
||||
(hrt_callout)&CameraTrigger::engange_turn_on_off, this);
|
||||
PX4_INFO("toggle power");
|
||||
|
||||
hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000),
|
||||
(hrt_callout)&CameraTrigger::disengage_turn_on_off, this);
|
||||
// schedule power toggle calls
|
||||
hrt_call_after(&_engage_turn_on_off_call, 0, &CameraTrigger::engange_turn_on_off, this);
|
||||
|
||||
hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000), &CameraTrigger::disengage_turn_on_off, this);
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::shoot_once()
|
||||
{
|
||||
PX4_INFO("shoot once");
|
||||
|
||||
// schedule trigger on and off calls
|
||||
hrt_call_after(&_engagecall, 0,
|
||||
(hrt_callout)&CameraTrigger::engage, this);
|
||||
|
||||
hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000),
|
||||
(hrt_callout)&CameraTrigger::disengage, this);
|
||||
&CameraTrigger::disengage, this);
|
||||
}
|
||||
|
||||
bool
|
||||
|
@ -527,13 +495,13 @@ void
|
|||
CameraTrigger::test()
|
||||
{
|
||||
vehicle_command_s vcmd{};
|
||||
vcmd.timestamp = hrt_absolute_time();
|
||||
vcmd.param5 = 1.0;
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
|
||||
vcmd.target_system = 1;
|
||||
vcmd.target_component = 1;
|
||||
|
||||
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
|
||||
vcmd.timestamp = hrt_absolute_time();
|
||||
vcmd_pub.publish(vcmd);
|
||||
}
|
||||
|
||||
|
@ -559,6 +527,7 @@ CameraTrigger::Run()
|
|||
// Command handling
|
||||
if (updated) {
|
||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
|
||||
PX4_INFO("received DO_DIGICAM_CONTROL");
|
||||
|
||||
need_ack = true;
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
@ -571,20 +540,18 @@ CameraTrigger::Run()
|
|||
if (commandParamToInt(cmd.param7) == 1) {
|
||||
// test shots are not logged or forwarded to GCS for geotagging
|
||||
_test_shot = true;
|
||||
|
||||
}
|
||||
|
||||
if (commandParamToInt((float)cmd.param5) == 1) {
|
||||
// Schedule shot
|
||||
_one_shot = true;
|
||||
|
||||
}
|
||||
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
}
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
|
||||
|
||||
PX4_INFO("received DO_TRIGGER_CONTROL");
|
||||
need_ack = true;
|
||||
|
||||
if (commandParamToInt(cmd.param3) == 1) {
|
||||
|
@ -610,13 +577,12 @@ CameraTrigger::Run()
|
|||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
|
||||
|
||||
PX4_INFO("received DO_SET_CAM_TRIGG_DIST");
|
||||
need_ack = true;
|
||||
|
||||
/*
|
||||
* TRANSITIONAL SUPPORT ADDED AS OF 11th MAY 2017 (v1.6 RELEASE)
|
||||
*/
|
||||
|
||||
if (cmd.param1 > 0.0f) {
|
||||
_distance = cmd.param1;
|
||||
param_set_no_notification(_p_distance, &_distance);
|
||||
|
@ -649,7 +615,7 @@ CameraTrigger::Run()
|
|||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
|
||||
|
||||
PX4_INFO("received DO_SET_CAM_TRIGG_INTERVAL");
|
||||
need_ack = true;
|
||||
|
||||
if (cmd.param1 > 0.0f) {
|
||||
|
@ -668,7 +634,7 @@ CameraTrigger::Run()
|
|||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY) {
|
||||
|
||||
PX4_INFO("received OBLIQUE_SURVEY");
|
||||
// Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
|
||||
|
||||
need_ack = true;
|
||||
|
@ -792,7 +758,6 @@ unknown_cmd:
|
|||
|
||||
// One shot command-based capture handling
|
||||
if (_one_shot && !_turning_on) {
|
||||
|
||||
// One-shot trigger
|
||||
shoot_once();
|
||||
_one_shot = false;
|
||||
|
@ -804,20 +769,44 @@ unknown_cmd:
|
|||
|
||||
// Command ACK handling
|
||||
if (updated && need_ack) {
|
||||
PX4_INFO("acknowledging command %d, result=%d", cmd.command, cmd_result);
|
||||
vehicle_command_ack_s command_ack{};
|
||||
|
||||
command_ack.timestamp = hrt_absolute_time();
|
||||
command_ack.command = cmd.command;
|
||||
command_ack.result = (uint8_t)cmd_result;
|
||||
command_ack.target_system = cmd.source_system;
|
||||
command_ack.target_component = cmd.source_component;
|
||||
|
||||
command_ack.timestamp = hrt_absolute_time();
|
||||
_cmd_ack_pub.publish(command_ack);
|
||||
}
|
||||
|
||||
ScheduleDelayed(poll_interval_usec);
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::adjust_roll()
|
||||
{
|
||||
vehicle_command_s vcmd{};
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
|
||||
vcmd.target_system = _target_system;
|
||||
vcmd.target_component = _target_component;
|
||||
|
||||
//param1 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is pitch
|
||||
vcmd.param1 = _CAMPOS_pitch_angle;
|
||||
|
||||
//param2 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is roll
|
||||
if (++_CAMPOS_pose_counter == _CAMPOS_num_poses) {
|
||||
_CAMPOS_pose_counter = 0;
|
||||
}
|
||||
|
||||
vcmd.param2 = _CAMPOS_angle_interval * _CAMPOS_pose_counter - _CAMPOS_roll_angle;
|
||||
|
||||
vcmd.param7 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
|
||||
|
||||
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
|
||||
vcmd.timestamp = hrt_absolute_time();
|
||||
vcmd_pub.publish(vcmd);
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::engage(void *arg)
|
||||
{
|
||||
|
@ -841,18 +830,15 @@ CameraTrigger::engage(void *arg)
|
|||
|
||||
// Send camera trigger message. This messages indicates that we sent
|
||||
// the camera trigger request. Does not guarantee capture.
|
||||
camera_trigger_s trigger{};
|
||||
|
||||
struct camera_trigger_s trigger = {};
|
||||
|
||||
// Set timestamp the instant after the trigger goes off
|
||||
trigger.timestamp = now;
|
||||
|
||||
timespec tv = {};
|
||||
timespec tv{};
|
||||
px4_clock_gettime(CLOCK_REALTIME, &tv);
|
||||
trigger.timestamp_utc = (uint64_t) tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
|
||||
|
||||
trigger.seq = trig->_trigger_seq;
|
||||
trigger.feedback = false;
|
||||
trigger.timestamp = hrt_absolute_time();
|
||||
|
||||
if (!trig->_cam_cap_fback) {
|
||||
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
|
||||
|
@ -935,7 +921,7 @@ static int usage()
|
|||
return 1;
|
||||
}
|
||||
|
||||
int camera_trigger_main(int argc, char *argv[])
|
||||
extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
return usage();
|
||||
|
@ -985,28 +971,3 @@ int camera_trigger_main(int argc, char *argv[])
|
|||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::adjust_roll()
|
||||
{
|
||||
vehicle_command_s vcmd{};
|
||||
vcmd.timestamp = hrt_absolute_time();
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
|
||||
vcmd.target_system = _target_system;
|
||||
vcmd.target_component = _target_component;
|
||||
|
||||
//param1 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is pitch
|
||||
vcmd.param1 = _CAMPOS_pitch_angle;
|
||||
|
||||
//param2 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is roll
|
||||
if (++_CAMPOS_pose_counter == _CAMPOS_num_poses) {
|
||||
_CAMPOS_pose_counter = 0;
|
||||
}
|
||||
|
||||
vcmd.param2 = _CAMPOS_angle_interval * _CAMPOS_pose_counter - _CAMPOS_roll_angle;
|
||||
|
||||
vcmd.param7 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
|
||||
|
||||
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
|
||||
vcmd_pub.publish(vcmd);
|
||||
}
|
||||
|
|
|
@ -21,7 +21,17 @@ CameraInterfaceSeagull::CameraInterfaceSeagull():
|
|||
CameraInterface(),
|
||||
_camera_is_on(false)
|
||||
{
|
||||
get_pins();
|
||||
//get_pins();
|
||||
|
||||
// NORA hacks
|
||||
// Set all pins as invalid
|
||||
for (unsigned i = 0; i < arraySize(_pins); i++) {
|
||||
_pins[i] = -1;
|
||||
}
|
||||
|
||||
_pins[0] = 14 - 1;
|
||||
_pins[1] = 13 - 1;
|
||||
|
||||
setup();
|
||||
}
|
||||
|
||||
|
@ -41,10 +51,12 @@ void CameraInterfaceSeagull::setup()
|
|||
up_pwm_trigger_init(pin_bitmask);
|
||||
|
||||
// Set both interface pins to disarmed
|
||||
up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_DISARMED);
|
||||
up_pwm_trigger_set(_pins[i], PWM_CAMERA_DISARMED);
|
||||
int ret1 = up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_DISARMED);
|
||||
PX4_INFO("pwm trigger set %d %d=%d, ret=%d", i + 1, _pins[i + 1], PWM_CAMERA_DISARMED, ret1);
|
||||
|
||||
int ret2 = up_pwm_trigger_set(_pins[i], PWM_CAMERA_DISARMED);
|
||||
PX4_INFO("pwm trigger set %d %d=%d, ret=%d", i, _pins[i], PWM_CAMERA_DISARMED, ret2);
|
||||
|
||||
// We only support 2 consecutive pins while using the Seagull MAP2
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
@ -54,7 +66,6 @@ void CameraInterfaceSeagull::setup()
|
|||
|
||||
void CameraInterfaceSeagull::trigger(bool trigger_on_true)
|
||||
{
|
||||
|
||||
if (!_camera_is_on) {
|
||||
return;
|
||||
}
|
||||
|
@ -69,6 +80,7 @@ void CameraInterfaceSeagull::trigger(bool trigger_on_true)
|
|||
|
||||
void CameraInterfaceSeagull::send_keep_alive(bool enable)
|
||||
{
|
||||
fprintf(stderr, "seagull keep alive %d (camera is on: %d)\n", enable, _camera_is_on);
|
||||
// This should alternate between enable and !enable to keep the camera alive
|
||||
|
||||
if (!_camera_is_on) {
|
||||
|
@ -78,6 +90,7 @@ void CameraInterfaceSeagull::send_keep_alive(bool enable)
|
|||
for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
|
||||
if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
|
||||
// Set channel 2 pin to keep_alive or netural signal
|
||||
fprintf(stderr, "seagull keep alive pin %d %d\n", i, _pins[i]);
|
||||
up_pwm_trigger_set(_pins[i], enable ? PWM_2_CAMERA_KEEP_ALIVE : PWM_CAMERA_NEUTRAL);
|
||||
}
|
||||
}
|
||||
|
@ -85,7 +98,7 @@ void CameraInterfaceSeagull::send_keep_alive(bool enable)
|
|||
|
||||
void CameraInterfaceSeagull::send_toggle_power(bool enable)
|
||||
{
|
||||
|
||||
fprintf(stderr, "seagull toggle power %d\n", enable);
|
||||
// This should alternate between enable and !enable to toggle camera power
|
||||
|
||||
for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
|
||||
|
|
|
@ -1113,16 +1113,26 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
|
||||
case PWM_SERVO_GET(13):
|
||||
case PWM_SERVO_GET(12):
|
||||
case PWM_SERVO_GET(11):
|
||||
case PWM_SERVO_GET(10):
|
||||
case PWM_SERVO_GET(9):
|
||||
case PWM_SERVO_GET(8):
|
||||
if (_mode < MODE_14PWM) {
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12
|
||||
|
||||
// FALLTHROUGH
|
||||
case PWM_SERVO_GET(11):
|
||||
case PWM_SERVO_GET(10):
|
||||
case PWM_SERVO_GET(9):
|
||||
case PWM_SERVO_GET(8):
|
||||
if (_mode < MODE_12PWM) {
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
|
||||
// FALLTHROUGH
|
||||
#endif
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
|
||||
|
||||
/* FALLTHROUGH */
|
||||
|
|
Loading…
Reference in New Issue