mavlink: add onboard function for camera capture commands

This commit is contained in:
Julian Oes 2014-03-17 20:12:12 +01:00
parent 295f87f22c
commit 8fe3475b41
3 changed files with 58 additions and 2 deletions

View File

@ -1653,6 +1653,8 @@ Mavlink::task_main(int argc, char *argv[])
case 'm': case 'm':
if (strcmp(optarg, "custom") == 0) { if (strcmp(optarg, "custom") == 0) {
_mode = MAVLINK_MODE_CUSTOM; _mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
_mode = MAVLINK_MODE_CAMERA;
} }
break; break;
@ -1696,6 +1698,10 @@ Mavlink::task_main(int argc, char *argv[])
warnx("mode: CUSTOM"); warnx("mode: CUSTOM");
break; break;
case MAVLINK_MODE_CAMERA:
warnx("mode: CAMERA");
break;
default: default:
warnx("ERROR: Unknown mode"); warnx("ERROR: Unknown mode");
break; break;
@ -1765,6 +1771,13 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
break; break;
case MAVLINK_MODE_CAMERA:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 20.0f);
configure_stream("GLOBAL_POSITION_INT", 20.0f);
configure_stream("CAMERA_CAPTURE", 1.0f);
break;
default: default:
break; break;
} }

View File

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

View File

@ -230,7 +230,6 @@ protected:
mavlink_base_mode, mavlink_base_mode,
mavlink_custom_mode, mavlink_custom_mode,
mavlink_state); mavlink_state);
} }
}; };
@ -1127,6 +1126,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[] = { MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(), new MavlinkStreamHeartbeat(),
@ -1151,6 +1192,7 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamRCChannelsRaw(), new MavlinkStreamRCChannelsRaw(),
new MavlinkStreamManualControl(), new MavlinkStreamManualControl(),
new MavlinkStreamOpticalFlow(), new MavlinkStreamOpticalFlow(),
new MavlinkStreamCameraCapture(),
nullptr nullptr
}; };