dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

[DO NOT MERGE] Nora PWM12 mode + camera_trigger hacks

This commit is contained in:
Daniel Agar 2021-01-28 10:02:40 -05:00
parent e2563388eb
commit 64b5277c5d
4 changed files with 141 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
ROMFS/px4fmu_common/init.d/rcS
View File

@ -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

217
src/drivers/camera_trigger/camera_trigger.cpp
View File

@ -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);
}

25
src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp
View File

@ -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) {

18
src/drivers/pwm_out/PWMOut.cpp
View File

@ -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 */