mirror of https://github.com/ArduPilot/ardupilot
Sub: move common mavlink camera handling up
This commit is contained in:
parent
b4688bd3d4
commit
077f577e5c
|
@ -1133,33 +1133,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
|
||||||
result = MAV_RESULT_ACCEPTED;
|
result = MAV_RESULT_ACCEPTED;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
#if CAMERA == ENABLED
|
|
||||||
case MAV_CMD_DO_DIGICAM_CONFIGURE:
|
|
||||||
sub.camera.configure(packet.param1,
|
|
||||||
packet.param2,
|
|
||||||
packet.param3,
|
|
||||||
packet.param4,
|
|
||||||
packet.param5,
|
|
||||||
packet.param6,
|
|
||||||
packet.param7);
|
|
||||||
|
|
||||||
result = MAV_RESULT_ACCEPTED;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_DO_DIGICAM_CONTROL:
|
|
||||||
sub.camera.control(packet.param1,
|
|
||||||
packet.param2,
|
|
||||||
packet.param3,
|
|
||||||
packet.param4,
|
|
||||||
packet.param5,
|
|
||||||
packet.param6);
|
|
||||||
result = MAV_RESULT_ACCEPTED;
|
|
||||||
break;
|
|
||||||
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
|
|
||||||
sub.camera.set_trigger_distance(packet.param1);
|
|
||||||
result = MAV_RESULT_ACCEPTED;
|
|
||||||
break;
|
|
||||||
#endif // CAMERA == ENABLED
|
|
||||||
case MAV_CMD_DO_MOUNT_CONTROL:
|
case MAV_CMD_DO_MOUNT_CONTROL:
|
||||||
#if MOUNT == ENABLED
|
#if MOUNT == ENABLED
|
||||||
sub.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
|
sub.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
|
||||||
|
@ -1556,17 +1529,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
break;
|
||||||
#endif // AC_FENCE == ENABLED
|
#endif // AC_FENCE == ENABLED
|
||||||
|
|
||||||
#if CAMERA == ENABLED
|
|
||||||
//deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
|
|
||||||
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: // MAV ID: 202
|
|
||||||
break;
|
|
||||||
|
|
||||||
//deprecated. Use MAV_CMD_DO_DIGICAM_CONTROL
|
|
||||||
case MAVLINK_MSG_ID_DIGICAM_CONTROL:
|
|
||||||
sub.camera.control_msg(msg);
|
|
||||||
break;
|
|
||||||
#endif // CAMERA == ENABLED
|
|
||||||
|
|
||||||
#if MOUNT == ENABLED
|
#if MOUNT == ENABLED
|
||||||
//deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
|
//deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
|
||||||
case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // MAV ID: 204
|
case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // MAV ID: 204
|
||||||
|
@ -1704,6 +1666,15 @@ AP_GPS *GCS_MAVLINK_Sub::get_gps() const
|
||||||
return &sub.gps;
|
return &sub.gps;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
AP_Camera *GCS_MAVLINK_Sub::get_camera() const
|
||||||
|
{
|
||||||
|
#if CAMERA == ENABLED
|
||||||
|
return &sub.camera;
|
||||||
|
#else
|
||||||
|
return nullptr;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
AP_ServoRelayEvents *GCS_MAVLINK_Sub::get_servorelayevents() const
|
AP_ServoRelayEvents *GCS_MAVLINK_Sub::get_servorelayevents() const
|
||||||
{
|
{
|
||||||
return &sub.ServoRelayEvents;
|
return &sub.ServoRelayEvents;
|
||||||
|
|
|
@ -17,6 +17,7 @@ protected:
|
||||||
Compass *get_compass() const override;
|
Compass *get_compass() const override;
|
||||||
AP_Mission *get_mission() override;
|
AP_Mission *get_mission() override;
|
||||||
AP_Rally *get_rally() const override;
|
AP_Rally *get_rally() const override;
|
||||||
|
AP_Camera *get_camera() const override;
|
||||||
AP_ServoRelayEvents *get_servorelayevents() const override;
|
AP_ServoRelayEvents *get_servorelayevents() const override;
|
||||||
AP_GPS *get_gps() const override;
|
AP_GPS *get_gps() const override;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue