diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3c49fee229..ae8c0dd690 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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 diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index f7073c6766..7c242a31a1 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -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 _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 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 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 vcmd_pub{ORB_ID(vehicle_command)}; - vcmd_pub.publish(vcmd); -} diff --git a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp index 3d2dd54177..0163729b15 100644 --- a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp +++ b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp @@ -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) { diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index b2faca0851..7b5fd39000 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -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 */