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

Merge pull request #754 from julianoes/beta_mavlink2_camera 

Beta mavlink2: onboard camera capture
This commit is contained in:
Julian Oes 2014-03-21 11:57:25 +01:00
parent ca2514b846 3d47ba14f3
commit 461557a689
3 changed files with 59 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/mavlink/mavlink_main.cpp
View File

@ -1652,6 +1652,8 @@ Mavlink::task_main(int argc, char *argv[])
case 'm':
if (strcmp(optarg, "custom") == 0) {
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
_mode = MAVLINK_MODE_CAMERA;
}
break;
@ -1695,6 +1697,10 @@ Mavlink::task_main(int argc, char *argv[])
warnx("mode: CUSTOM");
break;
case MAVLINK_MODE_CAMERA:
warnx("mode: CAMERA");
break;
default:
warnx("ERROR: Unknown mode");
break;
@ -1702,7 +1708,7 @@ Mavlink::task_main(int argc, char *argv[])
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate);
warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
@ -1765,6 +1771,13 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
break;
case MAVLINK_MODE_CAMERA:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 15.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
configure_stream("CAMERA_CAPTURE", 1.0f);
break;
default:
break;
}

3
src/modules/mavlink/mavlink_main.h
View File

@ -146,7 +146,8 @@ public:
enum MAVLINK_MODE {
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM
MAVLINK_MODE_CUSTOM,
MAVLINK_MODE_CAMERA
};
void set_mode(enum MAVLINK_MODE);

43
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1215,6 +1215,48 @@ protected:
}
};
class MavlinkStreamCameraCapture : public MavlinkStream
{
public:
const char *get_name()
{
return "CAMERA_CAPTURE";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamCameraCapture();
}
private:
MavlinkOrbSubscription *status_sub;
struct vehicle_status_s *status;
protected:
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
status = (struct vehicle_status_s *)status_sub->get_data();
}
void send(const hrt_abstime t)
{
(void)status_sub->update(t);
if (status->arming_state == ARMING_STATE_ARMED
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
/* send camera capture on */
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
} else {
/* send camera capture off */
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
}
}
};
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
@ -1240,5 +1282,6 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamOpticalFlow(),
new MavlinkStreamAttitudeControls(),
new MavlinkStreamNamedValueFloat(),
new MavlinkStreamCameraCapture(),
nullptr
};