forked from Archive/PX4-Autopilot
Merge pull request #754 from julianoes/beta_mavlink2_camera
Beta mavlink2: onboard camera capture
This commit is contained in:
commit
461557a689
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue