fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 08:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

camera_trigger: cleanup

Browse Source 
- add copyright headers and update year
 - move initializers to header
 - add PX4_DEBUG messages
This commit is contained in:
Daniel Agar
2021-03-15 12:35:41 -04:00
committed by GitHub
parent 517a1d0116
commit 6cdc034f08
11 changed files with 391 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/camera_trigger/camera_trigger.cpp
+93 -131
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -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,13 @@ CameraTrigger::update_intervalometer()
// control_intervalometer once on enabling/disabling trigger to schedule the calls.
if (_trigger_enabled && !_trigger_paused) {
// schedule trigger on and off calls
hrt_call_every(&_engagecall, 0, (_interval * 1000),
(hrt_callout)&CameraTrigger::engage, this);
PX4_DEBUG("update intervalometer, trigger enabled: %d, trigger paused: %d", _trigger_enabled, _trigger_paused);
// schedule trigger on and off calls
hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000),
(hrt_callout)&CameraTrigger::disengage, 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), &CameraTrigger::disengage, this);
}
}
@@ -392,7 +359,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 +392,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_DEBUG("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_DEBUG("keep alive disable");
// cancel all calls
hrt_cancel(&_keepalivecall_up);
hrt_cancel(&_keepalivecall_down);
@@ -443,23 +410,24 @@ 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_DEBUG("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_DEBUG("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);
hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000), &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_DEBUG("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_DEBUG("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_DEBUG("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_DEBUG("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,45 @@ unknown_cmd:
// Command ACK handling
if (updated && need_ack) {
PX4_DEBUG("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 +831,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 +922,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 +972,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);
}