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

manual_control: enable sending camera commands

This commit is contained in:
Julian Oes 2021-10-05 14:31:51 +02:00 committed by Daniel Agar
parent 1754e25920
commit 9fe7a40673
5 changed files with 87 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/manual_control_switches.msg
View File

@ -26,4 +26,7 @@ uint8 kill_switch # throttle kill: _NORMAL_, KILL
uint8 gear_switch # landing gear switch: _DOWN_, UP
uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT
uint8 photo_switch # Photo trigger switch
uint8 video_switch # Photo trigger switch
uint32 switch_changes # number of switch changes

3
msg/vehicle_command.msg
View File

@ -80,7 +80,10 @@ uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom
uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw
uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control
uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000 # Start image capture sequence.
uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system
uint16 VEHICLE_CMD_VIDEO_START_CAPTURE = 2500 # Start a video capture.
uint16 VEHICLE_CMD_VIDEO_STOP_CAPTURE = 2501 # Stop the current video capture.
uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data
uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data
uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # control starting/stopping transmitting data over the high latency link

64
src/modules/manual_control/ManualControl.cpp
View File

@ -33,6 +33,7 @@
#include "ManualControl.hpp"
#include <uORB/topics/commander_state.h>
#include <uORB/topics/vehicle_command.h>
ManualControl::ManualControl() :
ModuleParams(nullptr),
@ -200,6 +201,20 @@ void ManualControl::Run()
}
}
if (switches.photo_switch != _previous_switches.photo_switch) {
if (switches.photo_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_camera_mode_command(CameraMode::Image);
send_photo_command();
}
}
if (switches.video_switch != _previous_switches.video_switch) {
if (switches.video_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_camera_mode_command(CameraMode::Video);
send_video_command();
}
}
} else {
// Send an initial request to switch to the mode requested by RC
evaluateModeSlot(switches.mode_slot);
@ -341,6 +356,55 @@ void ManualControl::publishLandingGear(int8_t action)
_landing_gear_pub.publish(landing_gear);
}
void ManualControl::send_camera_mode_command(CameraMode camera_mode)
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE;
command.param2 = static_cast<float>(camera_mode);
command.target_system = _param_mav_sys_id.get();
command.target_component = 100; // any camera
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
command.timestamp = hrt_absolute_time();
command_pub.publish(command);
}
void ManualControl::send_photo_command()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_IMAGE_START_CAPTURE;
command.param3 = 1; // one picture
command.param4 = _image_sequence++;
command.target_system = _param_mav_sys_id.get();
command.target_component = 100; // any camera
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
command.timestamp = hrt_absolute_time();
command_pub.publish(command);
}
void ManualControl::send_video_command()
{
vehicle_command_s command{};
if (_video_recording) {
command.command = vehicle_command_s::VEHICLE_CMD_VIDEO_STOP_CAPTURE;
command.param2 = 1; // status at 1 Hz
} else {
command.command = vehicle_command_s::VEHICLE_CMD_VIDEO_START_CAPTURE;
}
command.target_system = _param_mav_sys_id.get();
command.target_component = 100; // any camera
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
command.timestamp = hrt_absolute_time();
command_pub.publish(command);
_video_recording = !_video_recording;
}
int ManualControl::task_spawn(int argc, char *argv[])
{
ManualControl *instance = new ManualControl();

15
src/modules/manual_control/ManualControl.hpp
View File

@ -85,6 +85,15 @@ private:
uORB::Publication<action_request_s> _action_request_pub{ORB_ID(action_request)};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
enum class CameraMode {
Image = 0,
Video = 1
};
void send_camera_mode_command(CameraMode camera_mode);
void send_photo_command();
void send_video_command();
uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@ -128,6 +137,10 @@ private:
(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,
(ParamInt<px4::params::COM_FLTMODE4>) _param_fltmode_4,
(ParamInt<px4::params::COM_FLTMODE5>) _param_fltmode_5,
(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6
(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6,
(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id
)
unsigned _image_sequence {0};
bool _video_recording {false}; // TODO: hopefully there is a command soon to toggle without keeping state
};

4
src/modules/rc_update/rc_update.cpp
View File

@ -54,7 +54,9 @@ static bool operator ==(const manual_control_switches_s &a, const manual_control
a.kill_switch == b.kill_switch &&
a.arm_switch == b.arm_switch &&
a.transition_switch == b.transition_switch &&
a.gear_switch == b.gear_switch);
a.gear_switch == b.gear_switch &&
a.photo_switch == b.photo_switch &&
a.video_switch == b.video_switch);
}
static bool operator !=(const manual_control_switches_s &a, const manual_control_switches_s &b) { return !(a == b); }