diff --git a/.gitignore b/.gitignore
index 72ba949..5a7684c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,4 @@
*.o
*.so
*.sublime*
+/.vscode/
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9478576..4b58496 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,6 +4,12 @@ project(raspicam_node)
set(CMAKE_CXX_STANDARD 14) # use C++14
+# Enable compile optimizations
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
+set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv8-a+crc -mtune=cortex-a53 -mfpu=crypto-neon-fp-armv8 -mfloat-abi=hard -ftree-vectorize -funsafe-math-optimizations -O3 -pipe")
+
+# add_definitions(-D_DEBUG)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
@@ -15,8 +21,9 @@ find_package(catkin REQUIRED COMPONENTS
dynamic_reconfigure
diagnostic_updater
message_generation
+ cv_bridge
)
-
+find_package(OpenCV 4.2 REQUIRED COMPONENTS imgproc highgui)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -43,7 +50,7 @@ if(${CMAKE_SYSTEM_PROCESSOR} MATCHES arm*)
IF( (NOT vchostif_LIBS ))
MESSAGE(FATAL_ERROR "Could not find vchostif library")
ENDIF()
-
+
set(RPI_LIBRARIES ${RPI_LIBRARIES}
${bcmhost_LIBS}
${vcos_LIBS}
@@ -117,22 +124,45 @@ include_directories(include
/opt/vc/include
/opt/vc/include/interface/vcos/pthreads
/opt/vc/include/interface/vmcs_host/linux
+ ${OpenCV_INCLUDE_DIRS}
)
-add_compile_options(-Wall -Wuseless-cast -Wformat-nonliteral)
+add_compile_options(-Wall -Wuseless-cast -Wformat-nonliteral -Wparentheses)
+
+## Declare a cpp library
+add_library(raspicli STATIC
+ src/RaspiCLI.c
+)
+
+add_library(raspicamcontrol STATIC
+ src/RaspiCamControl.c
+)
+
+## Declare a cpp executables
+add_executable(raspicam_node src/raspicam_node.cpp)
+add_executable(stereo_node src/stereo_node.cpp)
-## Declare a cpp executable
-add_executable(raspicam_node src/raspicam_node.cpp src/RaspiCamControl.cpp)
## Add cmake target dependencies of the executable/library
add_dependencies(raspicam_node raspicam_node_generate_messages_cpp)
add_dependencies(raspicam_node raspicam_node_gencfg)
+add_dependencies(stereo_node raspicam_node_generate_messages_cpp)
+add_dependencies(stereo_node raspicam_node_gencfg)
## Specify libraries to link a library or executable target against
target_link_libraries(raspicam_node
+ raspicamcontrol
+ raspicli
${catkin_LIBRARIES}
${RPI_LIBRARIES}
)
+ target_link_libraries(stereo_node
+ raspicamcontrol
+ raspicli
+ ${catkin_LIBRARIES}
+ ${RPI_LIBRARIES}
+ ${OpenCV_LIBS}
+)
#############
## Install ##
@@ -149,7 +179,7 @@ target_link_libraries(raspicam_node
# )
## Mark executables and/or libraries for installation
-install(TARGETS raspicam_node
+install(TARGETS raspicam_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
diff --git a/README.md b/README.md
index f18254a..cdcbcf3 100644
--- a/README.md
+++ b/README.md
@@ -35,7 +35,7 @@ rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y
Compile the code with `catkin_make`.
-## Running the Node
+## Running the monocamera Node
Once you have the node built, you can run it using a launch file.
For a V2.x camera, run `roslaunch raspicam_node camerav2_1280x960.launch`
@@ -44,6 +44,13 @@ For a V1.x camera, run `roslaunch raspicam_node camerav1_1280x720.launch`
Use `rqt_image_view` on a connected computer to view the published image.
+## Running the stereocamera Node
+Once you have the node built, you can run it using a launch file.
+
+For a V2.x camera, run `roslaunch raspicam_node stereo_1280x720.launch`
+
+Use `rqt_image_view` on a connected computer to view the published image.
+
## Configuring the node with dynamic reconfigure
The `raspicam_node` supports dynamically reconfiguring the camera parameters.
@@ -68,6 +75,7 @@ make sure that the camera cable is properly seated on both ends, and that the ca
## Node Information
+# Raspicam_node
Topics:
* `~/image/compressed`:
@@ -110,6 +118,50 @@ This parameter is mainly present in order to keep backward compatibility.
* `~enable_imv` (bool): Publish inline motion vectors computed by the GPU
+* `~camera_id` (int): The camera id (only supported on Compute Module)
+# Stereo_node
+
+Topics:
+
+* `~/image_left`:
+ Publishes `sensor_msgs/Image` from the camera module.
+
+* `~/image_right`:
+ Publishes `sensor_msgs/Image` from the camera module.
+
+* `~/left_camera_info`:
+ Publishes `sensor_msgs/CameraInfo` left camera info for each frame.
+
+* `~/left_camera_info`:
+ Publishes `sensor_msgs/CameraInfo` right camera info for each frame.
+
+Services:
+
+* `~/set_camera_info`: Used to update calibration info for the camera.
+
+Parameters:
+
+* `~private_topics` (bool): By default the topics are private, meaning the node name will be added in front of every topic name.
+If you don't want the topics to be private, you can set this parameter to "true".
+This parameter is mainly present in order to keep backward compatibility.
+
+* `~camera_frame_id` (tf frame): The frame identifier to associate the camera.
+
+* `~left_camera_info_url`: The URL of the camera calibration `.yaml` file for left camera.
+* `~left_camera_info_url`: The URL of the camera calibration `.yaml` file for right camera.
+
+* `~left_camera_name` (string): The name of the left camera, should match with name in camera_info file.
+
+* `~right_camera_name` (string): The name of the right camera, should match with name in camera_info file.
+
+* `~framerate` (fps): Framerate to capture at. Maximum 90fps
+
+* `~height` (pixels): Height to capture images at.
+
+* `~width` (pixels): Width to capture images at.
+
+* `~quality` (0-100): Quality of the captured images.
+
* `~camera_id` (int): The camera id (only supported on Compute Module)
## Calibration
diff --git a/camera_info/left/left.yaml b/camera_info/left/left.yaml
new file mode 100644
index 0000000..e2ed20d
--- /dev/null
+++ b/camera_info/left/left.yaml
@@ -0,0 +1,26 @@
+image_width: 640
+image_height: 480
+camera_name: left_camera_optical_frame
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [ 383.37648, 0. , 318.72515,
+ 0. , 386.12691, 251.37268,
+ 0. , 0. , 1. ]
+camera_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.294786, 0.071835, -0.004425, 0.000128, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [ 0.99380801, -0.0319161 , 0.10642835,
+ 0.03129902, 0.9994822 , 0.00746371,
+ -0.10661146, -0.00408639, 0.99429236]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [ 357.83988, 0. , 263.50254, 0. ,
+ 0. , 357.83988, 249.2244 , 0. ,
+ 0. , 0. , 1. , 0. ]
\ No newline at end of file
diff --git a/camera_info/left/ost.yaml b/camera_info/left/ost.yaml
new file mode 100644
index 0000000..6db1244
--- /dev/null
+++ b/camera_info/left/ost.yaml
@@ -0,0 +1,26 @@
+image_width: 640
+image_height: 480
+camera_name: left_camera_optical_frame
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [ 377.34204, 0. , 317.17444,
+ 0. , 377.98404, 242.82206,
+ 0. , 0. , 1. ]
+camera_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.317285, 0.096353, -0.000721, 0.000562, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [ 1., 0., 0.,
+ 0., 1., 0.,
+ 0., 0., 1.]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [ 284.43878, 0. , 317.7113 , 0. ,
+ 0. , 322.88959, 243.25744, 0. ,
+ 0. , 0. , 1. , 0. ]
diff --git a/camera_info/right/ost.yaml b/camera_info/right/ost.yaml
new file mode 100644
index 0000000..481dc06
--- /dev/null
+++ b/camera_info/right/ost.yaml
@@ -0,0 +1,26 @@
+image_width: 640
+image_height: 480
+camera_name: right_camera_optical_frame
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [ 370.55914, 0. , 329.09807,
+ 0. , 371.35797, 257.59864,
+ 0. , 0. , 1. ]
+camera_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.324288, 0.107501, -0.001059, 0.000672, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [ 1., 0., 0.,
+ 0., 1., 0.,
+ 0., 0., 1.]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [ 289.42285, 0. , 328.57791, 0. ,
+ 0. , 314.12253, 263.15155, 0. ,
+ 0. , 0. , 1. , 0. ]
diff --git a/camera_info/right/right.yaml b/camera_info/right/right.yaml
new file mode 100644
index 0000000..8d35279
--- /dev/null
+++ b/camera_info/right/right.yaml
@@ -0,0 +1,26 @@
+image_width: 640
+image_height: 480
+camera_name: right_camera_optical_frame
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [ 378.37387, 0. , 329.03434,
+ 0. , 380.28319, 260.9376 ,
+ 0. , 0. , 1. ]
+camera_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.305682, 0.078253, -0.001969, 0.000841, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [ 0.99584801, -0.02643552, 0.08710852,
+ 0.02693943, 0.99962642, -0.00461425,
+ -0.086954 , 0.00694175, 0.99618814]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [ 357.83988, 0. , 263.50254, -23.39691,
+ 0. , 357.83988, 249.2244 , 0. ,
+ 0. , 0. , 1. , 0. ]
\ No newline at end of file
diff --git a/include/RaspiCLI.h b/include/RaspiCLI.h
new file mode 100644
index 0000000..26b1ce3
--- /dev/null
+++ b/include/RaspiCLI.h
@@ -0,0 +1,56 @@
+/*
+Copyright (c) 2013, Broadcom Europe Ltd
+Copyright (c) 2013, James Hughes
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of the copyright holder nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RASPICLI_H_
+#define RASPICLI_H_
+
+typedef struct
+{
+ int id;
+ char *command;
+ char *abbrev;
+ char *help;
+ int num_parameters;
+} COMMAND_LIST;
+
+/// Cross reference structure, mode string against mode id
+typedef struct xref_t
+{
+ char *mode;
+ int mmal_mode;
+} XREF_T;
+
+
+void raspicli_display_help(const COMMAND_LIST *commands, const int num_commands);
+int raspicli_get_command_id(const COMMAND_LIST *commands, const int num_commands, const char *arg, int *num_parameters);
+
+int raspicli_map_xref(const char *str, const XREF_T *map, int num_refs);
+const char *raspicli_unmap_xref(const int en, XREF_T *map, int num_refs);
+
+
+#endif
diff --git a/include/RaspiCamControl.h b/include/RaspiCamControl.h
index 7b6543b..535c678 100644
--- a/include/RaspiCamControl.h
+++ b/include/RaspiCamControl.h
@@ -1,184 +1,226 @@
-/*
-Copyright (c) 2013, Broadcom Europe Ltd
-Copyright (c) 2013, James Hughes
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
- * Redistributions of source code must retain the above copyright
- notice, this list of conditions and the following disclaimer.
- * Redistributions in binary form must reproduce the above copyright
- notice, this list of conditions and the following disclaimer in the
- documentation and/or other materials provided with the distribution.
- * Neither the name of the copyright holder nor the
- names of its contributors may be used to endorse or promote products
- derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef RASPICAMCONTROL_H_
-#define RASPICAMCONTROL_H_
-
-#include "interface/mmal/mmal.h"
-#include "interface/mmal/mmal_logging.h"
-#include "interface/mmal/util/mmal_default_components.h"
-#include "interface/mmal/util/mmal_util.h"
-#include "interface/mmal/util/mmal_util_params.h"
-#include "interface/vmcs_host/vc_vchi_gencmd.h"
-
-/* Various parameters
- *
- * Exposure Mode
- * MMAL_PARAM_EXPOSUREMODE_OFF,
- MMAL_PARAM_EXPOSUREMODE_AUTO,
- MMAL_PARAM_EXPOSUREMODE_NIGHT,
- MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW,
- MMAL_PARAM_EXPOSUREMODE_BACKLIGHT,
- MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT,
- MMAL_PARAM_EXPOSUREMODE_SPORTS,
- MMAL_PARAM_EXPOSUREMODE_SNOW,
- MMAL_PARAM_EXPOSUREMODE_BEACH,
- MMAL_PARAM_EXPOSUREMODE_VERYLONG,
- MMAL_PARAM_EXPOSUREMODE_FIXEDFPS,
- MMAL_PARAM_EXPOSUREMODE_ANTISHAKE,
- MMAL_PARAM_EXPOSUREMODE_FIREWORKS,
- *
- * AWB Mode
- * MMAL_PARAM_AWBMODE_OFF,
- MMAL_PARAM_AWBMODE_AUTO,
- MMAL_PARAM_AWBMODE_SUNLIGHT,
- MMAL_PARAM_AWBMODE_CLOUDY,
- MMAL_PARAM_AWBMODE_SHADE,
- MMAL_PARAM_AWBMODE_TUNGSTEN,
- MMAL_PARAM_AWBMODE_FLUORESCENT,
- MMAL_PARAM_AWBMODE_INCANDESCENT,
- MMAL_PARAM_AWBMODE_FLASH,
- MMAL_PARAM_AWBMODE_HORIZON,
- *
- * Image FX
- MMAL_PARAM_IMAGEFX_NONE,
- MMAL_PARAM_IMAGEFX_NEGATIVE,
- MMAL_PARAM_IMAGEFX_SOLARIZE,
- MMAL_PARAM_IMAGEFX_POSTERIZE,
- MMAL_PARAM_IMAGEFX_WHITEBOARD,
- MMAL_PARAM_IMAGEFX_BLACKBOARD,
- MMAL_PARAM_IMAGEFX_SKETCH,
- MMAL_PARAM_IMAGEFX_DENOISE,
- MMAL_PARAM_IMAGEFX_EMBOSS,
- MMAL_PARAM_IMAGEFX_OILPAINT,
- MMAL_PARAM_IMAGEFX_HATCH,
- MMAL_PARAM_IMAGEFX_GPEN,
- MMAL_PARAM_IMAGEFX_PASTEL,
- MMAL_PARAM_IMAGEFX_WATERCOLOUR,
- MMAL_PARAM_IMAGEFX_FILM,
- MMAL_PARAM_IMAGEFX_BLUR,
- MMAL_PARAM_IMAGEFX_SATURATION,
- MMAL_PARAM_IMAGEFX_COLOURSWAP,
- MMAL_PARAM_IMAGEFX_WASHEDOUT,
- MMAL_PARAM_IMAGEFX_POSTERISE,
- MMAL_PARAM_IMAGEFX_COLOURPOINT,
- MMAL_PARAM_IMAGEFX_COLOURBALANCE,
- MMAL_PARAM_IMAGEFX_CARTOON,
-
- */
-
-// There isn't actually a MMAL structure for the following, so make one
-typedef struct {
- int enable; /// Turn colourFX on or off
- int u, v; /// U and V to use
-} MMAL_PARAM_COLOURFX_T;
-
-typedef struct {
- int enable;
- int width, height;
- int quality;
-} MMAL_PARAM_THUMBNAIL_CONFIG_T;
-
-typedef struct {
- double x;
- double y;
- double w;
- double h;
-} PARAM_FLOAT_RECT_T;
-
-/// struct contain camera settings
-typedef struct {
- int sharpness; /// -100 to 100
- int contrast; /// -100 to 100
- int brightness; /// 0 to 100
- int saturation; /// -100 to 100
- int ISO; /// TODO : what range?
- int videoStabilisation; /// 0 or 1 (false or true)
- int exposureCompensation; /// -10 to +10 ?
- MMAL_PARAM_EXPOSUREMODE_T exposureMode;
- MMAL_PARAM_EXPOSUREMETERINGMODE_T exposureMeterMode;
- MMAL_PARAM_AWBMODE_T awbMode;
- MMAL_PARAM_IMAGEFX_T imageEffect;
- MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imageEffectsParameters;
- MMAL_PARAM_COLOURFX_T colourEffects;
- int rotation; /// 0-359
- int hflip; /// 0 or 1
- int vflip; /// 0 or 1
- PARAM_FLOAT_RECT_T roi; /// region of interest to use on the sensor.
- /// Normalised [0,1] values in the rect
- int shutter_speed; /// TODO : Range?
-} RASPICAM_CAMERA_PARAMETERS;
-
-MMAL_PARAM_EXPOSUREMODE_T exposure_mode_from_string(const char* str);
-MMAL_PARAM_AWBMODE_T awb_mode_from_string(const char* str);
-
-void raspicamcontrol_check_configuration(int min_gpu_mem);
-
-int raspicamcontrol_cycle_test(MMAL_COMPONENT_T& camera);
-
-int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T& camera, const RASPICAM_CAMERA_PARAMETERS& params);
-void raspicamcontrol_dump_parameters(const RASPICAM_CAMERA_PARAMETERS& params);
-
-void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS& params);
-
-void raspicamcontrol_check_configuration(int min_gpu_mem);
-
-// Individual setting functions
-int raspicamcontrol_set_saturation(MMAL_COMPONENT_T& camera, int saturation);
-int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T& camera, int sharpness);
-int raspicamcontrol_set_contrast(MMAL_COMPONENT_T& camera, int contrast);
-int raspicamcontrol_set_brightness(MMAL_COMPONENT_T& camera, int brightness);
-int raspicamcontrol_set_ISO(MMAL_COMPONENT_T& camera, int ISO);
-int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T mode);
-int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T& camera, int vstabilisation);
-int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T& camera, int exp_comp);
-int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_EXPOSUREMODE_T mode);
-int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_AWBMODE_T awb_mode);
-int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T& camera, MMAL_PARAM_IMAGEFX_T imageFX);
-int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T& camera, const MMAL_PARAM_COLOURFX_T* colourFX);
-int raspicamcontrol_set_rotation(MMAL_COMPONENT_T& camera, int rotation);
-int raspicamcontrol_set_flips(MMAL_COMPONENT_T& camera, int hflip, int vflip);
-int raspicamcontrol_set_ROI(MMAL_COMPONENT_T& camera, PARAM_FLOAT_RECT_T rect);
-int raspicamcontrol_set_shutter_speed(MMAL_COMPONENT_T& camera, int shutter_speed);
-
-// Individual getting functions
-int raspicamcontrol_get_saturation(MMAL_COMPONENT_T& camera);
-int raspicamcontrol_get_sharpness(MMAL_COMPONENT_T& camera);
-int raspicamcontrol_get_contrast(MMAL_COMPONENT_T& camera);
-int raspicamcontrol_get_brightness(MMAL_COMPONENT_T& camera);
-int raspicamcontrol_get_ISO(MMAL_COMPONENT_T& camera);
-MMAL_PARAM_EXPOSUREMETERINGMODE_T raspicamcontrol_get_metering_mode(MMAL_COMPONENT_T& camera);
-int raspicamcontrol_get_video_stabilisation(MMAL_COMPONENT_T& camera);
-int raspicamcontrol_get_exposure_compensation(MMAL_COMPONENT_T& camera);
-MMAL_PARAM_THUMBNAIL_CONFIG_T raspicamcontrol_get_thumbnail_parameters(MMAL_COMPONENT_T& camera);
-MMAL_PARAM_EXPOSUREMODE_T raspicamcontrol_get_exposure_mode(MMAL_COMPONENT_T& camera);
-MMAL_PARAM_AWBMODE_T raspicamcontrol_get_awb_mode(MMAL_COMPONENT_T& camera);
-MMAL_PARAM_IMAGEFX_T raspicamcontrol_get_imageFX(MMAL_COMPONENT_T& camera);
-MMAL_PARAM_COLOURFX_T raspicamcontrol_get_colourFX(MMAL_COMPONENT_T& camera);
-
-#endif /* RASPICAMCONTROL_H_ */
+/*
+Copyright (c) 2013, Broadcom Europe Ltd
+Copyright (c) 2013, James Hughes
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of the copyright holder nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RASPICAMCONTROL_H_
+#define RASPICAMCONTROL_H_
+
+/* Various parameters
+ *
+ * Exposure Mode
+ * MMAL_PARAM_EXPOSUREMODE_OFF,
+ MMAL_PARAM_EXPOSUREMODE_AUTO,
+ MMAL_PARAM_EXPOSUREMODE_NIGHT,
+ MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW,
+ MMAL_PARAM_EXPOSUREMODE_BACKLIGHT,
+ MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT,
+ MMAL_PARAM_EXPOSUREMODE_SPORTS,
+ MMAL_PARAM_EXPOSUREMODE_SNOW,
+ MMAL_PARAM_EXPOSUREMODE_BEACH,
+ MMAL_PARAM_EXPOSUREMODE_VERYLONG,
+ MMAL_PARAM_EXPOSUREMODE_FIXEDFPS,
+ MMAL_PARAM_EXPOSUREMODE_ANTISHAKE,
+ MMAL_PARAM_EXPOSUREMODE_FIREWORKS,
+ *
+ * AWB Mode
+ * MMAL_PARAM_AWBMODE_OFF,
+ MMAL_PARAM_AWBMODE_AUTO,
+ MMAL_PARAM_AWBMODE_SUNLIGHT,
+ MMAL_PARAM_AWBMODE_CLOUDY,
+ MMAL_PARAM_AWBMODE_SHADE,
+ MMAL_PARAM_AWBMODE_TUNGSTEN,
+ MMAL_PARAM_AWBMODE_FLUORESCENT,
+ MMAL_PARAM_AWBMODE_INCANDESCENT,
+ MMAL_PARAM_AWBMODE_FLASH,
+ MMAL_PARAM_AWBMODE_HORIZON,
+ *
+ * Image FX
+ MMAL_PARAM_IMAGEFX_NONE,
+ MMAL_PARAM_IMAGEFX_NEGATIVE,
+ MMAL_PARAM_IMAGEFX_SOLARIZE,
+ MMAL_PARAM_IMAGEFX_POSTERIZE,
+ MMAL_PARAM_IMAGEFX_WHITEBOARD,
+ MMAL_PARAM_IMAGEFX_BLACKBOARD,
+ MMAL_PARAM_IMAGEFX_SKETCH,
+ MMAL_PARAM_IMAGEFX_DENOISE,
+ MMAL_PARAM_IMAGEFX_EMBOSS,
+ MMAL_PARAM_IMAGEFX_OILPAINT,
+ MMAL_PARAM_IMAGEFX_HATCH,
+ MMAL_PARAM_IMAGEFX_GPEN,
+ MMAL_PARAM_IMAGEFX_PASTEL,
+ MMAL_PARAM_IMAGEFX_WATERCOLOUR,
+ MMAL_PARAM_IMAGEFX_FILM,
+ MMAL_PARAM_IMAGEFX_BLUR,
+ MMAL_PARAM_IMAGEFX_SATURATION,
+ MMAL_PARAM_IMAGEFX_COLOURSWAP,
+ MMAL_PARAM_IMAGEFX_WASHEDOUT,
+ MMAL_PARAM_IMAGEFX_POSTERISE,
+ MMAL_PARAM_IMAGEFX_COLOURPOINT,
+ MMAL_PARAM_IMAGEFX_COLOURBALANCE,
+ MMAL_PARAM_IMAGEFX_CARTOON,
+
+ */
+
+/// Annotate bitmask options
+/// Supplied by user on command line
+#define ANNOTATE_USER_TEXT 1
+/// Supplied by app using this module
+#define ANNOTATE_APP_TEXT 2
+/// Insert current date
+#define ANNOTATE_DATE_TEXT 4
+// Insert current time
+#define ANNOTATE_TIME_TEXT 8
+
+#define ANNOTATE_SHUTTER_SETTINGS 16
+#define ANNOTATE_CAF_SETTINGS 32
+#define ANNOTATE_GAIN_SETTINGS 64
+#define ANNOTATE_LENS_SETTINGS 128
+#define ANNOTATE_MOTION_SETTINGS 256
+#define ANNOTATE_FRAME_NUMBER 512
+#define ANNOTATE_BLACK_BACKGROUND 1024
+
+
+// There isn't actually a MMAL structure for the following, so make one
+typedef struct mmal_param_colourfx_s
+{
+ int enable; /// Turn colourFX on or off
+ int u,v; /// U and V to use
+} MMAL_PARAM_COLOURFX_T;
+
+typedef struct mmal_param_thumbnail_config_s
+{
+ int enable;
+ int width,height;
+ int quality;
+} MMAL_PARAM_THUMBNAIL_CONFIG_T;
+
+typedef struct param_float_rect_s
+{
+ double x;
+ double y;
+ double w;
+ double h;
+} PARAM_FLOAT_RECT_T;
+
+/// struct contain camera settings
+typedef struct raspicam_camera_parameters_s
+{
+ int sharpness; /// -100 to 100
+ int contrast; /// -100 to 100
+ int brightness; /// 0 to 100
+ int saturation; /// -100 to 100
+ int ISO; /// TODO : what range?
+ int videoStabilisation; /// 0 or 1 (false or true)
+ int exposureCompensation; /// -10 to +10 ?
+ MMAL_PARAM_EXPOSUREMODE_T exposureMode;
+ MMAL_PARAM_EXPOSUREMETERINGMODE_T exposureMeterMode;
+ MMAL_PARAM_AWBMODE_T awbMode;
+ MMAL_PARAM_IMAGEFX_T imageEffect;
+ MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imageEffectsParameters;
+ MMAL_PARAM_COLOURFX_T colourEffects;
+ int rotation; /// 0-359
+ int hflip; /// 0 or 1
+ int vflip; /// 0 or 1
+ PARAM_FLOAT_RECT_T roi; /// region of interest to use on the sensor. Normalised [0,1] values in the rect
+ int shutter_speed; /// 0 = auto, otherwise the shutter speed in ms
+ float awb_gains_r; /// AWB red gain
+ float awb_gains_b; /// AWB blue gain
+ MMAL_PARAMETER_DRC_STRENGTH_T drc_level; // Strength of Dynamic Range compression to apply
+ MMAL_BOOL_T stats_pass; /// Stills capture statistics pass on/off
+ int enable_annotate; /// Flag to enable the annotate, 0 = disabled, otherwise a bitmask of what needs to be displayed
+ char annotate_string[MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V2]; /// String to use for annotate - overrides certain bitmask settings
+ int annotate_text_size; // Text size for annotation
+ int annotate_text_colour; // Text colour for annotation
+ int annotate_bg_colour; // Background colour for annotation
+ MMAL_PARAMETER_STEREOSCOPIC_MODE_T stereo_mode;
+} RASPICAM_CAMERA_PARAMETERS;
+
+typedef enum {
+ ZOOM_IN, ZOOM_OUT, ZOOM_RESET
+} ZOOM_COMMAND_T;
+
+MMAL_PARAM_EXPOSUREMODE_T exposure_mode_from_string(const char *str);
+MMAL_PARAM_AWBMODE_T awb_mode_from_string(const char *str);
+MMAL_STEREOSCOPIC_MODE_T stereo_mode_from_string(const char *str);
+MMAL_PARAM_IMAGEFX_T imagefx_mode_from_string(const char *str);
+void raspicamcontrol_check_configuration(int min_gpu_mem);
+
+int raspicamcontrol_parse_cmdline(RASPICAM_CAMERA_PARAMETERS *params, const char *arg1, const char *arg2);
+void raspicamcontrol_display_help();
+int raspicamcontrol_cycle_test(MMAL_COMPONENT_T *camera);
+
+int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T *camera, const RASPICAM_CAMERA_PARAMETERS *params);
+int raspicamcontrol_get_all_parameters(MMAL_COMPONENT_T *camera, RASPICAM_CAMERA_PARAMETERS *params);
+void raspicamcontrol_dump_parameters(const RASPICAM_CAMERA_PARAMETERS *params);
+
+void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS *params);
+
+void raspicamcontrol_check_configuration(int min_gpu_mem);
+
+// Individual setting functions
+int raspicamcontrol_set_saturation(MMAL_COMPONENT_T *camera, int saturation);
+int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T *camera, int sharpness);
+int raspicamcontrol_set_contrast(MMAL_COMPONENT_T *camera, int contrast);
+int raspicamcontrol_set_brightness(MMAL_COMPONENT_T *camera, int brightness);
+int raspicamcontrol_set_ISO(MMAL_COMPONENT_T *camera, int ISO);
+int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T mode);
+int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T *camera, int vstabilisation);
+int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T *camera, int exp_comp);
+int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMODE_T mode);
+int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_AWBMODE_T awb_mode);
+int raspicamcontrol_set_awb_gains(MMAL_COMPONENT_T *camera, float r_gain, float b_gain);
+int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T *camera, MMAL_PARAM_IMAGEFX_T imageFX);
+int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T *camera, const MMAL_PARAM_COLOURFX_T *colourFX);
+int raspicamcontrol_set_rotation(MMAL_COMPONENT_T *camera, int rotation);
+int raspicamcontrol_set_flips(MMAL_COMPONENT_T *camera, int hflip, int vflip);
+int raspicamcontrol_set_ROI(MMAL_COMPONENT_T *camera, PARAM_FLOAT_RECT_T rect);
+int raspicamcontrol_zoom_in_zoom_out(MMAL_COMPONENT_T *camera, ZOOM_COMMAND_T zoom_command, PARAM_FLOAT_RECT_T *roi);
+int raspicamcontrol_set_shutter_speed(MMAL_COMPONENT_T *camera, int speed_ms);
+int raspicamcontrol_set_DRC(MMAL_COMPONENT_T *camera, MMAL_PARAMETER_DRC_STRENGTH_T strength);
+int raspicamcontrol_set_stats_pass(MMAL_COMPONENT_T *camera, int stats_pass);
+int raspicamcontrol_set_annotate(MMAL_COMPONENT_T *camera, const int bitmask, const char *string,
+ const int text_size, const int text_colour, const int bg_colour);
+int raspicamcontrol_set_stereo_mode(MMAL_PORT_T *port, MMAL_PARAMETER_STEREOSCOPIC_MODE_T *stereo_mode);
+
+//Individual getting functions
+int raspicamcontrol_get_saturation(MMAL_COMPONENT_T *camera);
+int raspicamcontrol_get_sharpness(MMAL_COMPONENT_T *camera);
+int raspicamcontrol_get_contrast(MMAL_COMPONENT_T *camera);
+int raspicamcontrol_get_brightness(MMAL_COMPONENT_T *camera);
+int raspicamcontrol_get_ISO(MMAL_COMPONENT_T *camera);
+MMAL_PARAM_EXPOSUREMETERINGMODE_T raspicamcontrol_get_metering_mode(MMAL_COMPONENT_T *camera);
+int raspicamcontrol_get_video_stabilisation(MMAL_COMPONENT_T *camera);
+int raspicamcontrol_get_exposure_compensation(MMAL_COMPONENT_T *camera);
+MMAL_PARAM_THUMBNAIL_CONFIG_T raspicamcontrol_get_thumbnail_parameters(MMAL_COMPONENT_T *camera);
+MMAL_PARAM_EXPOSUREMODE_T raspicamcontrol_get_exposure_mode(MMAL_COMPONENT_T *camera);
+MMAL_PARAM_AWBMODE_T raspicamcontrol_get_awb_mode(MMAL_COMPONENT_T *camera);
+MMAL_PARAM_IMAGEFX_T raspicamcontrol_get_imageFX(MMAL_COMPONENT_T *camera);
+MMAL_PARAM_COLOURFX_T raspicamcontrol_get_colourFX(MMAL_COMPONENT_T *camera);
+
+
+
+#endif /* RASPICAMCONTROL_H_ */
diff --git a/launch/stereo_1280x720.launch b/launch/stereo_1280x720.launch
new file mode 100644
index 0000000..faeeba3
--- /dev/null
+++ b/launch/stereo_1280x720.launch
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/RaspiCLI.c b/src/RaspiCLI.c
new file mode 100644
index 0000000..9f858f4
--- /dev/null
+++ b/src/RaspiCLI.c
@@ -0,0 +1,155 @@
+/*
+Copyright (c) 2013, Broadcom Europe Ltd
+Copyright (c) 2013, James Hughes
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of the copyright holder nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/**
+ * \file RaspiCLI.c
+ * Code for handling command line parameters
+ *
+ * \date 4th March 2013
+ * \Author: James Hughes
+ *
+ * Description
+ *
+ * Some functions/structures for command line parameter parsing
+ *
+ */
+#include
+#include
+#include
+#include
+
+#include "interface/vcos/vcos.h"
+
+#include "RaspiCLI.h"
+
+
+/**
+ * Convert a string from command line to a comand_id from the list
+ *
+ * @param commands Array of command to check
+ * @param num_command Number of commands in the array
+ * @param arg String to search for in the list
+ * @param num_parameters Returns the number of parameters used by the command
+ *
+ * @return command ID if found, -1 if not found
+ *
+ */
+int raspicli_get_command_id(const COMMAND_LIST *commands, const int num_commands, const char *arg, int *num_parameters)
+{
+ int command_id = -1;
+ int j;
+
+ vcos_assert(commands);
+ vcos_assert(num_parameters);
+ vcos_assert(arg);
+
+ if (!commands || !num_parameters || !arg)
+ return -1;
+
+ for (j = 0; j < num_commands; j++)
+ {
+ if (!strcmp(arg, commands[j].command) ||
+ !strcmp(arg, commands[j].abbrev))
+ {
+ // match
+ command_id = commands[j].id;
+ *num_parameters = commands[j].num_parameters;
+ break;
+ }
+ }
+
+ return command_id;
+}
+
+
+/**
+ * Display the list of commands in help format
+ *
+ * @param commands Array of command to check
+ * @param num_command Number of commands in the arry
+ *
+ *
+ */
+void raspicli_display_help(const COMMAND_LIST *commands, const int num_commands)
+{
+ int i;
+
+ vcos_assert(commands);
+
+ if (!commands)
+ return;
+
+ for (i = 0; i < num_commands; i++)
+ {
+ fprintf(stdout, "-%s, -%s\t: %s\n", commands[i].abbrev,
+ commands[i].command, commands[i].help);
+ }
+}
+
+
+/**
+ * Function to take a string, a mapping, and return the int equivalent
+ * @param str Incoming string to match
+ * @param map Mapping data
+ * @param num_refs The number of items in the mapping data
+ * @return The integer match for the string, or -1 if no match
+ */
+int raspicli_map_xref(const char *str, const XREF_T *map, int num_refs)
+{
+ int i;
+
+ for (i=0;i
+#include
+#include
+
+#include "interface/vcos/vcos.h"
+
+#include "interface/vmcs_host/vc_vchi_gencmd.h"
+#include "interface/mmal/mmal.h"
+#include "interface/mmal/mmal_logging.h"
+#include "interface/mmal/util/mmal_util.h"
+#include "interface/mmal/util/mmal_util_params.h"
+#include "interface/mmal/util/mmal_default_components.h"
+#include "RaspiCamControl.h"
+#include "RaspiCLI.h"
+
+/// Structure to cross reference exposure strings against the MMAL parameter equivalent
+static XREF_T exposure_map[] =
+{
+ {"off", MMAL_PARAM_EXPOSUREMODE_OFF},
+ {"auto", MMAL_PARAM_EXPOSUREMODE_AUTO},
+ {"night", MMAL_PARAM_EXPOSUREMODE_NIGHT},
+ {"nightpreview", MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW},
+ {"backlight", MMAL_PARAM_EXPOSUREMODE_BACKLIGHT},
+ {"spotlight", MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT},
+ {"sports", MMAL_PARAM_EXPOSUREMODE_SPORTS},
+ {"snow", MMAL_PARAM_EXPOSUREMODE_SNOW},
+ {"beach", MMAL_PARAM_EXPOSUREMODE_BEACH},
+ {"verylong", MMAL_PARAM_EXPOSUREMODE_VERYLONG},
+ {"fixedfps", MMAL_PARAM_EXPOSUREMODE_FIXEDFPS},
+ {"antishake", MMAL_PARAM_EXPOSUREMODE_ANTISHAKE},
+ {"fireworks", MMAL_PARAM_EXPOSUREMODE_FIREWORKS}
+};
+
+static const int exposure_map_size = sizeof(exposure_map) / sizeof(exposure_map[0]);
+
+/// Structure to cross reference awb strings against the MMAL parameter equivalent
+static XREF_T awb_map[] =
+{
+ {"off", MMAL_PARAM_AWBMODE_OFF},
+ {"auto", MMAL_PARAM_AWBMODE_AUTO},
+ {"sun", MMAL_PARAM_AWBMODE_SUNLIGHT},
+ {"cloud", MMAL_PARAM_AWBMODE_CLOUDY},
+ {"shade", MMAL_PARAM_AWBMODE_SHADE},
+ {"tungsten", MMAL_PARAM_AWBMODE_TUNGSTEN},
+ {"fluorescent", MMAL_PARAM_AWBMODE_FLUORESCENT},
+ {"incandescent", MMAL_PARAM_AWBMODE_INCANDESCENT},
+ {"flash", MMAL_PARAM_AWBMODE_FLASH},
+ {"horizon", MMAL_PARAM_AWBMODE_HORIZON}
+};
+
+static const int awb_map_size = sizeof(awb_map) / sizeof(awb_map[0]);
+
+/// Structure to cross reference image effect against the MMAL parameter equivalent
+static XREF_T imagefx_map[] =
+{
+ {"none", MMAL_PARAM_IMAGEFX_NONE},
+ {"negative", MMAL_PARAM_IMAGEFX_NEGATIVE},
+ {"solarise", MMAL_PARAM_IMAGEFX_SOLARIZE},
+ {"sketch", MMAL_PARAM_IMAGEFX_SKETCH},
+ {"denoise", MMAL_PARAM_IMAGEFX_DENOISE},
+ {"emboss", MMAL_PARAM_IMAGEFX_EMBOSS},
+ {"oilpaint", MMAL_PARAM_IMAGEFX_OILPAINT},
+ {"hatch", MMAL_PARAM_IMAGEFX_HATCH},
+ {"gpen", MMAL_PARAM_IMAGEFX_GPEN},
+ {"pastel", MMAL_PARAM_IMAGEFX_PASTEL},
+ {"watercolour", MMAL_PARAM_IMAGEFX_WATERCOLOUR},
+ {"film", MMAL_PARAM_IMAGEFX_FILM},
+ {"blur", MMAL_PARAM_IMAGEFX_BLUR},
+ {"saturation", MMAL_PARAM_IMAGEFX_SATURATION},
+ {"colourswap", MMAL_PARAM_IMAGEFX_COLOURSWAP},
+ {"washedout", MMAL_PARAM_IMAGEFX_WASHEDOUT},
+ {"posterise", MMAL_PARAM_IMAGEFX_POSTERISE},
+ {"colourpoint", MMAL_PARAM_IMAGEFX_COLOURPOINT},
+ {"colourbalance", MMAL_PARAM_IMAGEFX_COLOURBALANCE},
+ {"cartoon", MMAL_PARAM_IMAGEFX_CARTOON}
+ };
+
+static const int imagefx_map_size = sizeof(imagefx_map) / sizeof(imagefx_map[0]);
+
+static XREF_T metering_mode_map[] =
+{
+ {"average", MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE},
+ {"spot", MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT},
+ {"backlit", MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT},
+ {"matrix", MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX}
+};
+
+static const int metering_mode_map_size = sizeof(metering_mode_map)/sizeof(metering_mode_map[0]);
+
+static XREF_T drc_mode_map[] =
+{
+ {"off", MMAL_PARAMETER_DRC_STRENGTH_OFF},
+ {"low", MMAL_PARAMETER_DRC_STRENGTH_LOW},
+ {"med", MMAL_PARAMETER_DRC_STRENGTH_MEDIUM},
+ {"high", MMAL_PARAMETER_DRC_STRENGTH_HIGH}
+};
+
+static const int drc_mode_map_size = sizeof(drc_mode_map)/sizeof(drc_mode_map[0]);
+
+static XREF_T stereo_mode_map[] =
+{
+ {"off", MMAL_STEREOSCOPIC_MODE_NONE},
+ {"sbs", MMAL_STEREOSCOPIC_MODE_SIDE_BY_SIDE},
+ {"tb", MMAL_STEREOSCOPIC_MODE_TOP_BOTTOM},
+};
+
+static const int stereo_mode_map_size = sizeof(stereo_mode_map)/sizeof(stereo_mode_map[0]);
+
+
+#define CommandSharpness 0
+#define CommandContrast 1
+#define CommandBrightness 2
+#define CommandSaturation 3
+#define CommandISO 4
+#define CommandVideoStab 5
+#define CommandEVComp 6
+#define CommandExposure 7
+#define CommandAWB 8
+#define CommandImageFX 9
+#define CommandColourFX 10
+#define CommandMeterMode 11
+#define CommandRotation 12
+#define CommandHFlip 13
+#define CommandVFlip 14
+#define CommandROI 15
+#define CommandShutterSpeed 16
+#define CommandAwbGains 17
+#define CommandDRCLevel 18
+#define CommandStatsPass 19
+#define CommandAnnotate 20
+#define CommandStereoMode 21
+#define CommandStereoDecimate 22
+#define CommandStereoSwap 23
+#define CommandAnnotateExtras 24
+
+static COMMAND_LIST cmdline_commands[] =
+{
+ {CommandSharpness, "-sharpness", "sh", "Set image sharpness (-100 to 100)", 1},
+ {CommandContrast, "-contrast", "co", "Set image contrast (-100 to 100)", 1},
+ {CommandBrightness, "-brightness","br", "Set image brightness (0 to 100)", 1},
+ {CommandSaturation, "-saturation","sa", "Set image saturation (-100 to 100)", 1},
+ {CommandISO, "-ISO", "ISO","Set capture ISO", 1},
+ {CommandVideoStab, "-vstab", "vs", "Turn on video stabilisation", 0},
+ {CommandEVComp, "-ev", "ev", "Set EV compensation - steps of 1/6 stop", 1},
+ {CommandExposure, "-exposure", "ex", "Set exposure mode (see Notes)", 1},
+ {CommandAWB, "-awb", "awb","Set AWB mode (see Notes)", 1},
+ {CommandImageFX, "-imxfx", "ifx","Set image effect (see Notes)", 1},
+ {CommandColourFX, "-colfx", "cfx","Set colour effect (U:V)", 1},
+ {CommandMeterMode, "-metering", "mm", "Set metering mode (see Notes)", 1},
+ {CommandRotation, "-rotation", "rot","Set image rotation (0-359)", 1},
+ {CommandHFlip, "-hflip", "hf", "Set horizontal flip", 0},
+ {CommandVFlip, "-vflip", "vf", "Set vertical flip", 0},
+ {CommandROI, "-roi", "roi","Set region of interest (x,y,w,d as normalised coordinates [0.0-1.0])", 1},
+ {CommandShutterSpeed,"-shutter", "ss", "Set shutter speed in microseconds", 1},
+ {CommandAwbGains, "-awbgains", "awbg", "Set AWB gains - AWB mode must be off", 1},
+ {CommandDRCLevel, "-drc", "drc", "Set DRC Level (see Notes)", 1},
+ {CommandStatsPass, "-stats", "st", "Force recomputation of statistics on stills capture pass"},
+ {CommandAnnotate, "-annotate", "a", "Enable/Set annotate flags or text", 1},
+ {CommandStereoMode, "-stereo", "3d", "Select stereoscopic mode", 1},
+ {CommandStereoDecimate,"-decimate","dec", "Half width/height of stereo image"},
+ {CommandStereoSwap, "-3dswap", "3dswap", "Swap camera order for stereoscopic"},
+ {CommandAnnotateExtras,"-annotateex","ae", "Set extra annotation parameters (text size, text colour(hex YUV), bg colour(hex YUV))", 2},
+};
+
+static int cmdline_commands_size = sizeof(cmdline_commands) / sizeof(cmdline_commands[0]);
+
+
+#define parameter_reset -99999
+
+#define zoom_full_16P16 ((unsigned int)(65536 * 0.15))
+#define zoom_increment_16P16 (65536UL / 10)
+
+/**
+ * Update the passed in parameter according to the rest of the parameters
+ * passed in.
+ *
+ *
+ * @return 0 if reached end of cycle for this parameter, !0 otherwise
+ */
+static int update_cycle_parameter(int *option, int min, int max, int increment)
+{
+ vcos_assert(option);
+ if (!option)
+ return 0;
+
+ if (*option == parameter_reset)
+ *option = min - increment;
+
+ *option += increment;
+
+ if (*option > max)
+ {
+ *option = parameter_reset;
+ return 0;
+ }
+ else
+ return 1;
+}
+
+
+/**
+ * Test/Demo code to cycle through a bunch of camera settings
+ * This code is pretty hacky so please don't complain!!
+ * It only does stuff that should have a visual impact (hence demo!)
+ * This will override any user supplied parameters
+ *
+ * Each call of this function will move on to the next setting
+ *
+ * @param camera Pointer to the camera to change settings on.
+ * @return 0 if reached end of complete sequence, !0 otherwise
+ */
+
+int raspicamcontrol_cycle_test(MMAL_COMPONENT_T *camera)
+{
+ static int parameter = 0;
+ static int parameter_option = parameter_reset; // which value the parameter currently has
+
+ vcos_assert(camera);
+
+ // We are going to cycle through all the relevant entries in the parameter block
+ // and send options to the camera.
+ if (parameter == 0)
+ {
+ // sharpness
+ if (update_cycle_parameter(¶meter_option, -100, 100, 10))
+ raspicamcontrol_set_sharpness(camera, parameter_option);
+ else
+ {
+ raspicamcontrol_set_sharpness(camera, 0);
+ parameter++;
+ }
+ }
+ else
+ if (parameter == 1)
+ {
+ // contrast
+ if (update_cycle_parameter(¶meter_option, -100, 100, 10))
+ raspicamcontrol_set_contrast(camera, parameter_option);
+ else
+ {
+ raspicamcontrol_set_contrast(camera, 0);
+ parameter++;
+ }
+ }
+ else
+ if (parameter == 2)
+ {
+ // brightness
+ if (update_cycle_parameter(¶meter_option, 0, 100, 10))
+ raspicamcontrol_set_brightness(camera, parameter_option);
+ else
+ {
+ raspicamcontrol_set_brightness(camera, 50);
+ parameter++;
+ }
+ }
+ else
+ if (parameter == 3)
+ {
+ // contrast
+ if (update_cycle_parameter(¶meter_option, -100, 100, 10))
+ raspicamcontrol_set_saturation(camera, parameter_option);
+ else
+ {
+ parameter++;
+ raspicamcontrol_set_saturation(camera, 0);
+ }
+ }
+ else
+ if (parameter == 4)
+ {
+ // EV
+ if (update_cycle_parameter(¶meter_option, -10, 10, 4))
+ raspicamcontrol_set_exposure_compensation(camera, parameter_option);
+ else
+ {
+ raspicamcontrol_set_exposure_compensation(camera, 0);
+ parameter++;
+ }
+ }
+ else
+ if (parameter == 5)
+ {
+ // MMAL_PARAM_EXPOSUREMODE_T
+ if (update_cycle_parameter(¶meter_option, 0, exposure_map_size, 1))
+ raspicamcontrol_set_exposure_mode(camera, exposure_map[parameter_option].mmal_mode);
+ else
+ {
+ raspicamcontrol_set_exposure_mode(camera, MMAL_PARAM_EXPOSUREMODE_AUTO);
+ parameter++;
+ }
+ }
+ else
+ if (parameter == 6)
+ {
+ // MMAL_PARAM_AWB_T
+ if (update_cycle_parameter(¶meter_option, 0, awb_map_size, 1))
+ raspicamcontrol_set_awb_mode(camera, awb_map[parameter_option].mmal_mode);
+ else
+ {
+ raspicamcontrol_set_awb_mode(camera, MMAL_PARAM_AWBMODE_AUTO);
+ parameter++;
+ }
+ }
+ if (parameter == 7)
+ {
+ // MMAL_PARAM_IMAGEFX_T
+ if (update_cycle_parameter(¶meter_option, 0, imagefx_map_size, 1))
+ raspicamcontrol_set_imageFX(camera, imagefx_map[parameter_option].mmal_mode);
+ else
+ {
+ raspicamcontrol_set_imageFX(camera, MMAL_PARAM_IMAGEFX_NONE);
+ parameter++;
+ }
+ }
+ if (parameter == 8)
+ {
+ MMAL_PARAM_COLOURFX_T colfx = {0,0,0};
+ switch (parameter_option)
+ {
+ case parameter_reset :
+ parameter_option = 1;
+ colfx.u = 128;
+ colfx.v = 128;
+ break;
+ case 1 :
+ parameter_option = 2;
+ colfx.u = 100;
+ colfx.v = 200;
+ break;
+ case 2 :
+ parameter_option = parameter_reset;
+ colfx.enable = 0;
+ parameter++;
+ break;
+ }
+ raspicamcontrol_set_colourFX(camera, &colfx);
+ }
+
+ // Orientation
+ if (parameter == 9)
+ {
+ switch (parameter_option)
+ {
+ case parameter_reset:
+ raspicamcontrol_set_rotation(camera, 90);
+ parameter_option = 1;
+ break;
+
+ case 1 :
+ raspicamcontrol_set_rotation(camera, 180);
+ parameter_option = 2;
+ break;
+
+ case 2 :
+ raspicamcontrol_set_rotation(camera, 270);
+ parameter_option = 3;
+ break;
+
+ case 3 :
+ {
+ raspicamcontrol_set_rotation(camera, 0);
+ raspicamcontrol_set_flips(camera, 1,0);
+ parameter_option = 4;
+ break;
+ }
+ case 4 :
+ {
+ raspicamcontrol_set_flips(camera, 0,1);
+ parameter_option = 5;
+ break;
+ }
+ case 5 :
+ {
+ raspicamcontrol_set_flips(camera, 1, 1);
+ parameter_option = 6;
+ break;
+ }
+ case 6 :
+ {
+ raspicamcontrol_set_flips(camera, 0, 0);
+ parameter_option = parameter_reset;
+ parameter++;
+ break;
+ }
+ }
+ }
+
+ if (parameter == 10)
+ {
+ parameter = 1;
+ return 0;
+ }
+
+ return 1;
+}
+
+
+
+/**
+ * Convert string to the MMAL parameter for exposure mode
+ * @param str Incoming string to match
+ * @return MMAL parameter matching the string, or the AUTO option if no match found
+ */
+/*static*/ MMAL_PARAM_EXPOSUREMODE_T exposure_mode_from_string(const char *str)
+{
+ int i = raspicli_map_xref(str, exposure_map, exposure_map_size);
+
+ if( i != -1)
+ return (MMAL_PARAM_EXPOSUREMODE_T)i;
+
+ vcos_log_error("Unknown exposure mode: %s", str);
+ return MMAL_PARAM_EXPOSUREMODE_AUTO;
+}
+
+/**
+ * Convert string to the MMAL parameter for AWB mode
+ * @param str Incoming string to match
+ * @return MMAL parameter matching the string, or the AUTO option if no match found
+ */
+/*static*/ MMAL_PARAM_AWBMODE_T awb_mode_from_string(const char *str)
+{
+ int i = raspicli_map_xref(str, awb_map, awb_map_size);
+
+ if( i != -1)
+ return (MMAL_PARAM_AWBMODE_T)i;
+
+ vcos_log_error("Unknown awb mode: %s", str);
+ return MMAL_PARAM_AWBMODE_AUTO;
+}
+
+/**
+ * Convert string to the MMAL parameter for image effects mode
+ * @param str Incoming string to match
+ * @return MMAL parameter matching the strong, or the AUTO option if no match found
+ */
+MMAL_PARAM_IMAGEFX_T imagefx_mode_from_string(const char *str)
+{
+ int i = raspicli_map_xref(str, imagefx_map, imagefx_map_size);
+
+ if( i != -1)
+ return (MMAL_PARAM_IMAGEFX_T)i;
+
+ vcos_log_error("Unknown image fx: %s", str);
+ return MMAL_PARAM_IMAGEFX_NONE;
+}
+
+/**
+ * Convert string to the MMAL parameter for exposure metering mode
+ * @param str Incoming string to match
+ * @return MMAL parameter matching the string, or the AUTO option if no match found
+ */
+static MMAL_PARAM_EXPOSUREMETERINGMODE_T metering_mode_from_string(const char *str)
+{
+ int i = raspicli_map_xref(str, metering_mode_map, metering_mode_map_size);
+
+ if( i != -1)
+ return (MMAL_PARAM_EXPOSUREMETERINGMODE_T)i;
+
+ vcos_log_error("Unknown metering mode: %s", str);
+ return MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE;
+}
+
+/**
+ * Convert string to the MMAL parameter for DRC level
+ * @param str Incoming string to match
+ * @return MMAL parameter matching the string, or the AUTO option if no match found
+ */
+static MMAL_PARAMETER_DRC_STRENGTH_T drc_mode_from_string(const char *str)
+{
+ int i = raspicli_map_xref(str, drc_mode_map, drc_mode_map_size);
+
+ if( i != -1)
+ return (MMAL_PARAMETER_DRC_STRENGTH_T)i;
+
+ vcos_log_error("Unknown DRC level: %s", str);
+ return MMAL_PARAMETER_DRC_STRENGTH_OFF;
+}
+
+/**
+ * Convert string to the MMAL parameter for exposure metering mode
+ * @param str Incoming string to match
+ * @return MMAL parameter matching the string, or the AUTO option if no match found
+ */
+MMAL_STEREOSCOPIC_MODE_T stereo_mode_from_string(const char *str)
+{
+ int i = raspicli_map_xref(str, stereo_mode_map, stereo_mode_map_size);
+
+ if( i != -1)
+ return (MMAL_STEREOSCOPIC_MODE_T)i;
+
+ vcos_log_error("Unknown metering mode: %s", str);
+ return MMAL_STEREOSCOPIC_MODE_NONE;
+}
+
+/**
+ * Parse a possible command pair - command and parameter
+ * @param arg1 Command
+ * @param arg2 Parameter (could be NULL)
+ * @return How many parameters were used, 0,1,2
+ */
+int raspicamcontrol_parse_cmdline(RASPICAM_CAMERA_PARAMETERS *params, const char *arg1, const char *arg2)
+{
+ int command_id, used = 0, num_parameters;
+
+ if (!arg1)
+ return 0;
+
+ command_id = raspicli_get_command_id(cmdline_commands, cmdline_commands_size, arg1, &num_parameters);
+
+ // If invalid command, or we are missing a parameter, drop out
+ if (command_id==-1 || (command_id != -1 && num_parameters > 0 && arg2 == NULL))
+ return 0;
+
+ switch (command_id)
+ {
+ case CommandSharpness : // sharpness - needs single number parameter
+ sscanf(arg2, "%d", ¶ms->sharpness);
+ used = 2;
+ break;
+
+ case CommandContrast : // contrast - needs single number parameter
+ sscanf(arg2, "%d", ¶ms->contrast);
+ used = 2;
+ break;
+
+ case CommandBrightness : // brightness - needs single number parameter
+ sscanf(arg2, "%d", ¶ms->brightness);
+ used = 2;
+ break;
+
+ case CommandSaturation : // saturation - needs single number parameter
+ sscanf(arg2, "%d", ¶ms->saturation);
+ used = 2;
+ break;
+
+ case CommandISO : // ISO - needs single number parameter
+ sscanf(arg2, "%d", ¶ms->ISO);
+ used = 2;
+ break;
+
+ case CommandVideoStab : // video stabilisation - if here, its on
+ params->videoStabilisation = 1;
+ used = 1;
+ break;
+
+ case CommandEVComp : // EV - needs single number parameter
+ sscanf(arg2, "%d", ¶ms->exposureCompensation);
+ used = 2;
+ break;
+
+ case CommandExposure : // exposure mode - needs string
+ params->exposureMode = exposure_mode_from_string(arg2);
+ used = 2;
+ break;
+
+ case CommandAWB : // AWB mode - needs single number parameter
+ params->awbMode = awb_mode_from_string(arg2);
+ used = 2;
+ break;
+
+ case CommandImageFX : // Image FX - needs string
+ params->imageEffect = imagefx_mode_from_string(arg2);
+ used = 2;
+ break;
+
+ case CommandColourFX : // Colour FX - needs string "u:v"
+ sscanf(arg2, "%d:%d", ¶ms->colourEffects.u, ¶ms->colourEffects.v);
+ params->colourEffects.enable = 1;
+ used = 2;
+ break;
+
+ case CommandMeterMode:
+ params->exposureMeterMode = metering_mode_from_string(arg2);
+ used = 2;
+ break;
+
+ case CommandRotation : // Rotation - degree
+ sscanf(arg2, "%d", ¶ms->rotation);
+ used = 2;
+ break;
+
+ case CommandHFlip :
+ params->hflip = 1;
+ used = 1;
+ break;
+
+ case CommandVFlip :
+ params->vflip = 1;
+ used = 1;
+ break;
+
+ case CommandROI :
+ {
+ double x,y,w,h;
+ int args;
+
+ args = sscanf(arg2, "%lf,%lf,%lf,%lf", &x,&y,&w,&h);
+
+ if (args != 4 || x > 1.0 || y > 1.0 || w > 1.0 || h > 1.0)
+ {
+ return 0;
+ }
+
+ // Make sure we stay within bounds
+ if (x + w > 1.0)
+ w = 1 - x;
+
+ if (y + h > 1.0)
+ h = 1 - y;
+
+ params->roi.x = x;
+ params->roi.y = y;
+ params->roi.w = w;
+ params->roi.h = h;
+
+ used = 2;
+ break;
+ }
+
+ case CommandShutterSpeed : // Shutter speed needs single number parameter
+ {
+ sscanf(arg2, "%d", ¶ms->shutter_speed);
+ used = 2;
+ break;
+ }
+
+ case CommandAwbGains :
+ {
+ double r,b;
+ int args;
+
+ args = sscanf(arg2, "%lf,%lf", &r,&b);
+
+ if (args != 2 || r > 8.0 || b > 8.0)
+ {
+ return 0;
+ }
+
+ params->awb_gains_r = r;
+ params->awb_gains_b = b;
+
+ used = 2;
+ break;
+ }
+
+ case CommandDRCLevel:
+ {
+ params->drc_level = drc_mode_from_string(arg2);
+ used = 2;
+ break;
+ }
+
+ case CommandStatsPass:
+ {
+ params->stats_pass = MMAL_TRUE;
+ used = 1;
+ break;
+ }
+
+ case CommandAnnotate:
+ {
+ char dummy;
+ unsigned int bitmask;
+ // If parameter is a number, assume its a bitmask, otherwise a string
+ if (sscanf(arg2, "%u%c", &bitmask, &dummy) == 1)
+ {
+ params->enable_annotate |= bitmask;
+ }
+ else
+ {
+ params->enable_annotate |= ANNOTATE_USER_TEXT;
+ //copy string char by char and replace "\n" with newline character
+ unsigned char c;
+ char const *s = arg2;
+ char *t = ¶ms->annotate_string[0];
+ int n=0;
+ while ((c = *s++) && n < MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3-1)
+ {
+ if (c == '\\' && *s)
+ {
+ switch (c = *s++)
+ {
+ case 'n':
+ c = '\n';
+ break;
+
+ default:
+ c = '\\';
+ s--;
+ break;
+ }
+ }
+ *(t++) = c;
+ n++;
+ }
+ *t='\0';
+
+ //params->annotate_string[MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3-1] = '\0';
+ }
+ used=2;
+ break;
+ }
+
+ case CommandAnnotateExtras:
+ {
+ // 3 parameters - text size (6-80), text colour (Hex VVUUYY) and background colour (Hex VVUUYY)
+ sscanf(arg2, "%u,%X,%X", ¶ms->annotate_text_size,
+ ¶ms->annotate_text_colour,
+ ¶ms->annotate_bg_colour);
+ used=2;
+ break;
+ }
+
+ case CommandStereoMode:
+ {
+ params->stereo_mode.mode = stereo_mode_from_string(arg2);
+ used = 2;
+ break;
+ }
+
+ case CommandStereoDecimate:
+ {
+ params->stereo_mode.decimate = MMAL_TRUE;
+ used = 1;
+ break;
+ }
+
+ case CommandStereoSwap:
+ {
+ params->stereo_mode.swap_eyes = MMAL_TRUE;
+ used = 1;
+ break;
+ }
+
+ }
+
+ return used;
+}
+
+/**
+ * Display help for command line options
+ */
+void raspicamcontrol_display_help()
+{
+ int i;
+
+ fprintf(stdout, "\nImage parameter commands\n\n");
+
+ raspicli_display_help(cmdline_commands, cmdline_commands_size);
+
+ fprintf(stdout, "\n\nNotes\n\nExposure mode options :\n%s", exposure_map[0].mode );
+
+ for (i=1;iexposureMode, exposure_map, exposure_map_size);
+ const char *awb_mode = raspicli_unmap_xref(params->awbMode, awb_map, awb_map_size);
+ const char *image_effect = raspicli_unmap_xref(params->imageEffect, imagefx_map, imagefx_map_size);
+ const char *metering_mode = raspicli_unmap_xref(params->exposureMeterMode, metering_mode_map, metering_mode_map_size);
+
+ fprintf(stderr, "Sharpness %d, Contrast %d, Brightness %d\n", params->sharpness, params->contrast, params->brightness);
+ fprintf(stderr, "Saturation %d, ISO %d, Video Stabilisation %s, Exposure compensation %d\n", params->saturation, params->ISO, params->videoStabilisation ? "Yes": "No", params->exposureCompensation);
+ fprintf(stderr, "Exposure Mode '%s', AWB Mode '%s', Image Effect '%s'\n", exp_mode, awb_mode, image_effect);
+ fprintf(stderr, "Metering Mode '%s', Colour Effect Enabled %s with U = %d, V = %d\n", metering_mode, params->colourEffects.enable ? "Yes":"No", params->colourEffects.u, params->colourEffects.v);
+ fprintf(stderr, "Rotation %d, hflip %s, vflip %s\n", params->rotation, params->hflip ? "Yes":"No",params->vflip ? "Yes":"No");
+ fprintf(stderr, "ROI x %lf, y %f, w %f h %f\n", params->roi.x, params->roi.y, params->roi.w, params->roi.h);
+}
+
+/**
+ * Convert a MMAL status return value to a simple boolean of success
+ * ALso displays a fault if code is not success
+ *
+ * @param status The error code to convert
+ * @return 0 if status is success, 1 otherwise
+ */
+int mmal_status_to_int(MMAL_STATUS_T status)
+{
+ if (status == MMAL_SUCCESS)
+ return 0;
+ else
+ {
+ switch (status)
+ {
+ case MMAL_ENOMEM : fprintf(stderr, "Out of memory"); vcos_log_error("Out of memory"); break;
+ case MMAL_ENOSPC : fprintf(stderr, "Out of resources (other than memory)"); vcos_log_error("Out of resources (other than memory)"); break;
+ case MMAL_EINVAL: fprintf(stderr, "Argument is invalid"); vcos_log_error("Argument is invalid"); break;
+ case MMAL_ENOSYS : fprintf(stderr, "Function not implemented"); vcos_log_error("Function not implemented"); break;
+ case MMAL_ENOENT : fprintf(stderr, "No such file or directory"); vcos_log_error("No such file or directory"); break;
+ case MMAL_ENXIO : fprintf(stderr, "No such device or address"); vcos_log_error("No such device or address"); break;
+ case MMAL_EIO : fprintf(stderr, "I/O error"); vcos_log_error("I/O error"); break;
+ case MMAL_ESPIPE : fprintf(stderr, "Illegal seek"); vcos_log_error("Illegal seek"); break;
+ case MMAL_ECORRUPT : fprintf(stderr, "Data is corrupt \attention FIXME: not POSIX"); vcos_log_error("Data is corrupt \attention FIXME: not POSIX"); break;
+ case MMAL_ENOTREADY : fprintf(stderr, "Component is not ready \attention FIXME: not POSIX"); vcos_log_error("Component is not ready \attention FIXME: not POSIX"); break;
+ case MMAL_ECONFIG : fprintf(stderr, "Component is not ready \attention FIXME: not POSIX"); vcos_log_error("Component is not configured \attention FIXME: not POSIX"); break;
+ case MMAL_EISCONN : fprintf(stderr, "Port is already connected "); vcos_log_error("Port is already connected "); break;
+ case MMAL_ENOTCONN : fprintf(stderr, "Port is disconnected"); vcos_log_error("Port is disconnected"); break;
+ case MMAL_EAGAIN : fprintf(stderr, "Resource temporarily unavailable. Try again later"); vcos_log_error("Resource temporarily unavailable. Try again later"); break;
+ case MMAL_EFAULT : fprintf(stderr, "Bad address"); vcos_log_error("Bad address"); break;
+ default : fprintf(stderr, "Unknown status error"); vcos_log_error("Unknown status error"); break;
+ }
+
+ return 1;
+ }
+}
+
+/**
+ * Give the supplied parameter block a set of default values
+ * @params Pointer to parameter block
+ */
+void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS *params)
+{
+ vcos_assert(params);
+
+ params->sharpness = 0;
+ params->contrast = 0;
+ params->brightness = 50;
+ params->saturation = 0;
+ params->ISO = 0; // 0 = auto
+ params->videoStabilisation = 0;
+ params->exposureCompensation = 0;
+ params->exposureMode = MMAL_PARAM_EXPOSUREMODE_AUTO;
+ params->exposureMeterMode = MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE;
+ params->awbMode = MMAL_PARAM_AWBMODE_AUTO;
+ params->imageEffect = MMAL_PARAM_IMAGEFX_NONE;
+ params->colourEffects.enable = 0;
+ params->colourEffects.u = 128;
+ params->colourEffects.v = 128;
+ params->rotation = 0;
+ params->hflip = params->vflip = 0;
+ params->roi.x = params->roi.y = 0.0;
+ params->roi.w = params->roi.h = 1.0;
+ params->shutter_speed = 0; // 0 = auto
+ params->awb_gains_r = 0; // Only have any function if AWB OFF is used.
+ params->awb_gains_b = 0;
+ params->drc_level = MMAL_PARAMETER_DRC_STRENGTH_OFF;
+ params->stats_pass = MMAL_FALSE;
+ params->enable_annotate = 0;
+ params->annotate_string[0] = '\0';
+ params->annotate_text_size = 0; //Use firmware default
+ params->annotate_text_colour = -1; //Use firmware default
+ params->annotate_bg_colour = -1; //Use firmware default
+ params->stereo_mode.mode = MMAL_STEREOSCOPIC_MODE_NONE;
+ params->stereo_mode.decimate = MMAL_FALSE;
+ params->stereo_mode.swap_eyes = MMAL_FALSE;
+}
+
+/**
+ * Get all the current camera parameters from specified camera component
+ * @param camera Pointer to camera component
+ * @param params Pointer to parameter block to accept settings
+ * @return 0 if successful, non-zero if unsuccessful
+ */
+int raspicamcontrol_get_all_parameters(MMAL_COMPONENT_T *camera, RASPICAM_CAMERA_PARAMETERS *params)
+{
+ vcos_assert(camera);
+ vcos_assert(params);
+
+ if (!camera || !params)
+ return 1;
+
+/* TODO : Write these get functions
+ params->sharpness = raspicamcontrol_get_sharpness(camera);
+ params->contrast = raspicamcontrol_get_contrast(camera);
+ params->brightness = raspicamcontrol_get_brightness(camera);
+ params->saturation = raspicamcontrol_get_saturation(camera);
+ params->ISO = raspicamcontrol_get_ISO(camera);
+ params->videoStabilisation = raspicamcontrol_get_video_stabilisation(camera);
+ params->exposureCompensation = raspicamcontrol_get_exposure_compensation(camera);
+ params->exposureMode = raspicamcontrol_get_exposure_mode(camera);
+ params->awbMode = raspicamcontrol_get_awb_mode(camera);
+ params->imageEffect = raspicamcontrol_get_image_effect(camera);
+ params->colourEffects = raspicamcontrol_get_colour_effect(camera);
+ params->thumbnailConfig = raspicamcontrol_get_thumbnail_config(camera);
+*/
+ return 0;
+}
+
+/**
+ * Set the specified camera to all the specified settings
+ * @param camera Pointer to camera component
+ * @param params Pointer to parameter block containing parameters
+ * @return 0 if successful, none-zero if unsuccessful.
+ */
+int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T *camera, const RASPICAM_CAMERA_PARAMETERS *params)
+{
+ int result;
+
+ result = raspicamcontrol_set_saturation(camera, params->saturation);
+ result += raspicamcontrol_set_sharpness(camera, params->sharpness);
+ result += raspicamcontrol_set_contrast(camera, params->contrast);
+ result += raspicamcontrol_set_brightness(camera, params->brightness);
+ result += raspicamcontrol_set_ISO(camera, params->ISO);
+ result += raspicamcontrol_set_video_stabilisation(camera, params->videoStabilisation);
+ result += raspicamcontrol_set_exposure_compensation(camera, params->exposureCompensation);
+ result += raspicamcontrol_set_exposure_mode(camera, params->exposureMode);
+ result += raspicamcontrol_set_metering_mode(camera, params->exposureMeterMode);
+ result += raspicamcontrol_set_awb_mode(camera, params->awbMode);
+ result += raspicamcontrol_set_awb_gains(camera, params->awb_gains_r, params->awb_gains_b);
+ result += raspicamcontrol_set_imageFX(camera, params->imageEffect);
+ result += raspicamcontrol_set_colourFX(camera, ¶ms->colourEffects);
+ //result += raspicamcontrol_set_thumbnail_parameters(camera, ¶ms->thumbnailConfig); TODO Not working for some reason
+ result += raspicamcontrol_set_rotation(camera, params->rotation);
+ result += raspicamcontrol_set_flips(camera, params->hflip, params->vflip);
+ result += raspicamcontrol_set_ROI(camera, params->roi);
+ result += raspicamcontrol_set_shutter_speed(camera, params->shutter_speed);
+ result += raspicamcontrol_set_DRC(camera, params->drc_level);
+ result += raspicamcontrol_set_stats_pass(camera, params->stats_pass);
+ result += raspicamcontrol_set_annotate(camera, params->enable_annotate, params->annotate_string,
+ params->annotate_text_size,
+ params->annotate_text_colour,
+ params->annotate_bg_colour);
+
+ return result;
+}
+
+/**
+ * Adjust the saturation level for images
+ * @param camera Pointer to camera component
+ * @param saturation Value to adjust, -100 to 100
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_saturation(MMAL_COMPONENT_T *camera, int saturation)
+{
+ int ret = 0;
+
+ if (!camera)
+ return 1;
+
+ if (saturation >= -100 && saturation <= 100)
+ {
+ MMAL_RATIONAL_T value = {saturation, 100};
+ ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_SATURATION, value));
+ }
+ else
+ {
+ vcos_log_error("Invalid saturation value");
+ ret = 1;
+ }
+
+ return ret;
+}
+
+/**
+ * Set the sharpness of the image
+ * @param camera Pointer to camera component
+ * @param sharpness Sharpness adjustment -100 to 100
+ */
+int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T *camera, int sharpness)
+{
+ int ret = 0;
+
+ if (!camera)
+ return 1;
+
+ if (sharpness >= -100 && sharpness <= 100)
+ {
+ MMAL_RATIONAL_T value = {sharpness, 100};
+ ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_SHARPNESS, value));
+ }
+ else
+ {
+ vcos_log_error("Invalid sharpness value");
+ ret = 1;
+ }
+
+ return ret;
+}
+
+/**
+ * Set the contrast adjustment for the image
+ * @param camera Pointer to camera component
+ * @param contrast Contrast adjustment -100 to 100
+ * @return
+ */
+int raspicamcontrol_set_contrast(MMAL_COMPONENT_T *camera, int contrast)
+{
+ int ret = 0;
+
+ if (!camera)
+ return 1;
+
+ if (contrast >= -100 && contrast <= 100)
+ {
+ MMAL_RATIONAL_T value = {contrast, 100};
+ ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_CONTRAST, value));
+ }
+ else
+ {
+ vcos_log_error("Invalid contrast value");
+ ret = 1;
+ }
+
+ return ret;
+}
+
+/**
+ * Adjust the brightness level for images
+ * @param camera Pointer to camera component
+ * @param brightness Value to adjust, 0 to 100
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_brightness(MMAL_COMPONENT_T *camera, int brightness)
+{
+ int ret = 0;
+
+ if (!camera)
+ return 1;
+
+ if (brightness >= 0 && brightness <= 100)
+ {
+ MMAL_RATIONAL_T value = {brightness, 100};
+ ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_BRIGHTNESS, value));
+ }
+ else
+ {
+ vcos_log_error("Invalid brightness value");
+ ret = 1;
+ }
+
+ return ret;
+}
+
+/**
+ * Adjust the ISO used for images
+ * @param camera Pointer to camera component
+ * @param ISO Value to set TODO :
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_ISO(MMAL_COMPONENT_T *camera, int ISO)
+{
+ if (!camera)
+ return 1;
+
+ return mmal_status_to_int(mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_ISO, ISO));
+}
+
+/**
+ * Adjust the metering mode for images
+ * @param camera Pointer to camera component
+ * @param saturation Value from following
+ * - MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE,
+ * - MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT,
+ * - MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT,
+ * - MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T m_mode )
+{
+ MMAL_PARAMETER_EXPOSUREMETERINGMODE_T meter_mode = {{MMAL_PARAMETER_EXP_METERING_MODE,sizeof(meter_mode)},
+ m_mode};
+ if (!camera)
+ return 1;
+
+ return mmal_status_to_int(mmal_port_parameter_set(camera->control, &meter_mode.hdr));
+}
+
+
+/**
+ * Set the video stabilisation flag. Only used in video mode
+ * @param camera Pointer to camera component
+ * @param saturation Flag 0 off 1 on
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T *camera, int vstabilisation)
+{
+ if (!camera)
+ return 1;
+
+ return mmal_status_to_int(mmal_port_parameter_set_boolean(camera->control, MMAL_PARAMETER_VIDEO_STABILISATION, vstabilisation));
+}
+
+/**
+ * Adjust the exposure compensation for images (EV)
+ * @param camera Pointer to camera component
+ * @param exp_comp Value to adjust, -10 to +10
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T *camera, int exp_comp)
+{
+ if (!camera)
+ return 1;
+
+ return mmal_status_to_int(mmal_port_parameter_set_int32(camera->control, MMAL_PARAMETER_EXPOSURE_COMP , exp_comp));
+}
+
+
+/**
+ * Set exposure mode for images
+ * @param camera Pointer to camera component
+ * @param mode Exposure mode to set from
+ * - MMAL_PARAM_EXPOSUREMODE_OFF,
+ * - MMAL_PARAM_EXPOSUREMODE_AUTO,
+ * - MMAL_PARAM_EXPOSUREMODE_NIGHT,
+ * - MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW,
+ * - MMAL_PARAM_EXPOSUREMODE_BACKLIGHT,
+ * - MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT,
+ * - MMAL_PARAM_EXPOSUREMODE_SPORTS,
+ * - MMAL_PARAM_EXPOSUREMODE_SNOW,
+ * - MMAL_PARAM_EXPOSUREMODE_BEACH,
+ * - MMAL_PARAM_EXPOSUREMODE_VERYLONG,
+ * - MMAL_PARAM_EXPOSUREMODE_FIXEDFPS,
+ * - MMAL_PARAM_EXPOSUREMODE_ANTISHAKE,
+ * - MMAL_PARAM_EXPOSUREMODE_FIREWORKS,
+ *
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMODE_T mode)
+{
+ MMAL_PARAMETER_EXPOSUREMODE_T exp_mode = {{MMAL_PARAMETER_EXPOSURE_MODE,sizeof(exp_mode)}, mode};
+
+ if (!camera)
+ return 1;
+
+ return mmal_status_to_int(mmal_port_parameter_set(camera->control, &exp_mode.hdr));
+}
+
+
+/**
+ * Set the aWB (auto white balance) mode for images
+ * @param camera Pointer to camera component
+ * @param awb_mode Value to set from
+ * - MMAL_PARAM_AWBMODE_OFF,
+ * - MMAL_PARAM_AWBMODE_AUTO,
+ * - MMAL_PARAM_AWBMODE_SUNLIGHT,
+ * - MMAL_PARAM_AWBMODE_CLOUDY,
+ * - MMAL_PARAM_AWBMODE_SHADE,
+ * - MMAL_PARAM_AWBMODE_TUNGSTEN,
+ * - MMAL_PARAM_AWBMODE_FLUORESCENT,
+ * - MMAL_PARAM_AWBMODE_INCANDESCENT,
+ * - MMAL_PARAM_AWBMODE_FLASH,
+ * - MMAL_PARAM_AWBMODE_HORIZON,
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_AWBMODE_T awb_mode)
+{
+ MMAL_PARAMETER_AWBMODE_T param = {{MMAL_PARAMETER_AWB_MODE,sizeof(param)}, awb_mode};
+
+ if (!camera)
+ return 1;
+
+ return mmal_status_to_int(mmal_port_parameter_set(camera->control, ¶m.hdr));
+}
+
+int raspicamcontrol_set_awb_gains(MMAL_COMPONENT_T *camera, float r_gain, float b_gain)
+{
+ MMAL_PARAMETER_AWB_GAINS_T param = {{MMAL_PARAMETER_CUSTOM_AWB_GAINS,sizeof(param)}, {0,0}, {0,0}};
+
+ if (!camera)
+ return 1;
+
+ if (!r_gain || !b_gain)
+ return 0;
+
+ param.r_gain.num = (unsigned int)(r_gain * 65536);
+ param.b_gain.num = (unsigned int)(b_gain * 65536);
+ param.r_gain.den = param.b_gain.den = 65536;
+ return mmal_status_to_int(mmal_port_parameter_set(camera->control, ¶m.hdr));
+}
+
+/**
+ * Set the image effect for the images
+ * @param camera Pointer to camera component
+ * @param imageFX Value from
+ * - MMAL_PARAM_IMAGEFX_NONE,
+ * - MMAL_PARAM_IMAGEFX_NEGATIVE,
+ * - MMAL_PARAM_IMAGEFX_SOLARIZE,
+ * - MMAL_PARAM_IMAGEFX_POSTERIZE,
+ * - MMAL_PARAM_IMAGEFX_WHITEBOARD,
+ * - MMAL_PARAM_IMAGEFX_BLACKBOARD,
+ * - MMAL_PARAM_IMAGEFX_SKETCH,
+ * - MMAL_PARAM_IMAGEFX_DENOISE,
+ * - MMAL_PARAM_IMAGEFX_EMBOSS,
+ * - MMAL_PARAM_IMAGEFX_OILPAINT,
+ * - MMAL_PARAM_IMAGEFX_HATCH,
+ * - MMAL_PARAM_IMAGEFX_GPEN,
+ * - MMAL_PARAM_IMAGEFX_PASTEL,
+ * - MMAL_PARAM_IMAGEFX_WATERCOLOUR,
+ * - MMAL_PARAM_IMAGEFX_FILM,
+ * - MMAL_PARAM_IMAGEFX_BLUR,
+ * - MMAL_PARAM_IMAGEFX_SATURATION,
+ * - MMAL_PARAM_IMAGEFX_COLOURSWAP,
+ * - MMAL_PARAM_IMAGEFX_WASHEDOUT,
+ * - MMAL_PARAM_IMAGEFX_POSTERISE,
+ * - MMAL_PARAM_IMAGEFX_COLOURPOINT,
+ * - MMAL_PARAM_IMAGEFX_COLOURBALANCE,
+ * - MMAL_PARAM_IMAGEFX_CARTOON,
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T *camera, MMAL_PARAM_IMAGEFX_T imageFX)
+{
+ MMAL_PARAMETER_IMAGEFX_T imgFX = {{MMAL_PARAMETER_IMAGE_EFFECT,sizeof(imgFX)}, imageFX};
+
+ if (!camera)
+ return 1;
+
+ return mmal_status_to_int(mmal_port_parameter_set(camera->control, &imgFX.hdr));
+}
+
+/* TODO :what to do with the image effects parameters?
+ MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imfx_param = {{MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,sizeof(imfx_param)},
+ imageFX, 0, {0}};
+mmal_port_parameter_set(camera->control, &imfx_param.hdr);
+ */
+
+/**
+ * Set the colour effect for images (Set UV component)
+ * @param camera Pointer to camera component
+ * @param colourFX Contains enable state and U and V numbers to set (e.g. 128,128 = Black and white)
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T *camera, const MMAL_PARAM_COLOURFX_T *colourFX)
+{
+ MMAL_PARAMETER_COLOURFX_T colfx = {{MMAL_PARAMETER_COLOUR_EFFECT,sizeof(colfx)}, 0, 0, 0};
+
+ if (!camera)
+ return 1;
+
+ colfx.enable = colourFX->enable;
+ colfx.u = colourFX->u;
+ colfx.v = colourFX->v;
+
+ return mmal_status_to_int(mmal_port_parameter_set(camera->control, &colfx.hdr));
+
+}
+
+
+/**
+ * Set the rotation of the image
+ * @param camera Pointer to camera component
+ * @param rotation Degree of rotation (any number, but will be converted to 0,90,180 or 270 only)
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_rotation(MMAL_COMPONENT_T *camera, int rotation)
+{
+ int ret;
+ int my_rotation = ((rotation % 360 ) / 90) * 90;
+
+ ret = mmal_port_parameter_set_int32(camera->output[0], MMAL_PARAMETER_ROTATION, my_rotation);
+ mmal_port_parameter_set_int32(camera->output[1], MMAL_PARAMETER_ROTATION, my_rotation);
+ mmal_port_parameter_set_int32(camera->output[2], MMAL_PARAMETER_ROTATION, my_rotation);
+
+ return ret;
+}
+
+/**
+ * Set the flips state of the image
+ * @param camera Pointer to camera component
+ * @param hflip If true, horizontally flip the image
+ * @param vflip If true, vertically flip the image
+ *
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_flips(MMAL_COMPONENT_T *camera, int hflip, int vflip)
+{
+ MMAL_PARAMETER_MIRROR_T mirror = {{MMAL_PARAMETER_MIRROR, sizeof(MMAL_PARAMETER_MIRROR_T)}, MMAL_PARAM_MIRROR_NONE};
+
+ if (hflip && vflip)
+ mirror.value = MMAL_PARAM_MIRROR_BOTH;
+ else
+ if (hflip)
+ mirror.value = MMAL_PARAM_MIRROR_HORIZONTAL;
+ else
+ if (vflip)
+ mirror.value = MMAL_PARAM_MIRROR_VERTICAL;
+
+ mmal_port_parameter_set(camera->output[0], &mirror.hdr);
+ mmal_port_parameter_set(camera->output[1], &mirror.hdr);
+ return mmal_port_parameter_set(camera->output[2], &mirror.hdr);
+}
+
+/**
+ * Set the ROI of the sensor to use for captures/preview
+ * @param camera Pointer to camera component
+ * @param rect Normalised coordinates of ROI rectangle
+ *
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_ROI(MMAL_COMPONENT_T *camera, PARAM_FLOAT_RECT_T rect)
+{
+ MMAL_PARAMETER_INPUT_CROP_T crop = {{MMAL_PARAMETER_INPUT_CROP, sizeof(MMAL_PARAMETER_INPUT_CROP_T)}};
+
+ crop.rect.x = (65536 * rect.x);
+ crop.rect.y = (65536 * rect.y);
+ crop.rect.width = (65536 * rect.w);
+ crop.rect.height = (65536 * rect.h);
+
+ return mmal_port_parameter_set(camera->control, &crop.hdr);
+}
+
+/**
+ * Zoom in and Zoom out by changing ROI
+ * @param camera Pointer to camera component
+ * @param zoom_command zoom command enum
+ * @return 0 if successful, non-zero otherwise
+ */
+int raspicamcontrol_zoom_in_zoom_out(MMAL_COMPONENT_T *camera, ZOOM_COMMAND_T zoom_command, PARAM_FLOAT_RECT_T *roi) {
+ MMAL_PARAMETER_INPUT_CROP_T crop;
+ crop.hdr.id = MMAL_PARAMETER_INPUT_CROP;
+ crop.hdr.size = sizeof(crop);
+
+ if (mmal_port_parameter_get(camera->control, &crop.hdr) != MMAL_SUCCESS)
+ {
+ vcos_log_error("mmal_port_parameter_get(camera->control, &crop.hdr) failed, skip it");
+ return 0;
+ }
+
+ if (zoom_command == ZOOM_IN)
+ {
+ if (crop.rect.width <= (zoom_full_16P16 + zoom_increment_16P16))
+ {
+ crop.rect.width = zoom_full_16P16;
+ crop.rect.height = zoom_full_16P16;
+ }
+ else
+ {
+ crop.rect.width -= zoom_increment_16P16;
+ crop.rect.height -= zoom_increment_16P16;
+ }
+ }
+ else if (zoom_command == ZOOM_OUT)
+ {
+ unsigned int increased_size = crop.rect.width + zoom_increment_16P16;
+ if (increased_size < crop.rect.width) //overflow
+ {
+ crop.rect.width = 65536;
+ crop.rect.height = 65536;
+ }
+ else
+ {
+ crop.rect.width = increased_size;
+ crop.rect.height = increased_size;
+ }
+ }
+
+ if (zoom_command == ZOOM_RESET)
+ {
+ crop.rect.x = 0;
+ crop.rect.y = 0;
+ crop.rect.width = 65536;
+ crop.rect.height = 65536;
+ }
+ else
+ {
+ unsigned int centered_top_coordinate = (65536 - crop.rect.width) / 2;
+ crop.rect.x = centered_top_coordinate;
+ crop.rect.y = centered_top_coordinate;
+ }
+
+ int ret = mmal_status_to_int(mmal_port_parameter_set(camera->control, &crop.hdr));
+
+ if (ret == 0) {
+ roi->x = roi->y = (double)crop.rect.x/65536;
+ roi->w = roi->h = (double)crop.rect.width/65536;
+ }
+ else
+ {
+ vcos_log_error("Failed to set crop values, x/y: %u, w/h: %u", crop.rect.x, crop.rect.width);
+ ret = 1;
+ }
+
+ return ret;
+}
+
+/**
+ * Adjust the exposure time used for images
+ * @param camera Pointer to camera component
+ * @param shutter speed in microseconds
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_shutter_speed(MMAL_COMPONENT_T *camera, int speed)
+{
+ if (!camera)
+ return 1;
+
+ return mmal_status_to_int(mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_SHUTTER_SPEED, speed));
+}
+
+/**
+ * Adjust the Dynamic range compression level
+ * @param camera Pointer to camera component
+ * @param strength Strength of DRC to apply
+ * MMAL_PARAMETER_DRC_STRENGTH_OFF
+ * MMAL_PARAMETER_DRC_STRENGTH_LOW
+ * MMAL_PARAMETER_DRC_STRENGTH_MEDIUM
+ * MMAL_PARAMETER_DRC_STRENGTH_HIGH
+ *
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_DRC(MMAL_COMPONENT_T *camera, MMAL_PARAMETER_DRC_STRENGTH_T strength)
+{
+ MMAL_PARAMETER_DRC_T drc = {{MMAL_PARAMETER_DYNAMIC_RANGE_COMPRESSION, sizeof(MMAL_PARAMETER_DRC_T)}, strength};
+
+ if (!camera)
+ return 1;
+
+ return mmal_status_to_int(mmal_port_parameter_set(camera->control, &drc.hdr));
+}
+
+int raspicamcontrol_set_stats_pass(MMAL_COMPONENT_T *camera, int stats_pass)
+{
+ if (!camera)
+ return 1;
+
+ return mmal_status_to_int(mmal_port_parameter_set_boolean(camera->control, MMAL_PARAMETER_CAPTURE_STATS_PASS, stats_pass));
+}
+
+
+/**
+ * Set the annotate data
+ * @param camera Pointer to camera component
+ * @param Bitmask of required annotation data. 0 for off.
+ * @param If set, a pointer to text string to use instead of bitmask, max length 32 characters
+ *
+ * @return 0 if successful, non-zero if any parameters out of range
+ */
+int raspicamcontrol_set_annotate(MMAL_COMPONENT_T *camera, const int settings, const char *string,
+ const int text_size, const int text_colour, const int bg_colour)
+{
+ MMAL_PARAMETER_CAMERA_ANNOTATE_V3_T annotate =
+ {{MMAL_PARAMETER_ANNOTATE, sizeof(MMAL_PARAMETER_CAMERA_ANNOTATE_V3_T)}};
+
+ if (settings)
+ {
+ time_t t = time(NULL);
+ struct tm tm = *localtime(&t);
+ char tmp[MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3];
+ int process_datetime = 1;
+
+ annotate.enable = 1;
+
+ if (settings & (ANNOTATE_APP_TEXT | ANNOTATE_USER_TEXT))
+ {
+ if ((settings & (ANNOTATE_TIME_TEXT | ANNOTATE_DATE_TEXT)) && strchr(string,'%') != NULL)
+ { //string contains strftime parameter?
+ strftime(annotate.text, MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3, string, &tm );
+ process_datetime = 0;
+ }else{
+ strncpy(annotate.text, string, MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3);
+ }
+ annotate.text[MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3-1] = '\0';
+ }
+
+ if (process_datetime && (settings & ANNOTATE_TIME_TEXT))
+ {
+ if(strlen(annotate.text)){
+ strftime(tmp, 32, " %X", &tm );
+ }else{
+ strftime(tmp, 32, "%X", &tm );
+ }
+ strncat(annotate.text, tmp, MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3 - strlen(annotate.text) - 1);
+ }
+
+ if (process_datetime && (settings & ANNOTATE_DATE_TEXT))
+ {
+ if(strlen(annotate.text)){
+ strftime(tmp, 32, " %x", &tm );
+ }else{
+ strftime(tmp, 32, "%x", &tm );
+ }
+ strncat(annotate.text, tmp, MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3 - strlen(annotate.text) - 1);
+ }
+
+ if (settings & ANNOTATE_SHUTTER_SETTINGS)
+ annotate.show_shutter = MMAL_TRUE;
+
+ if (settings & ANNOTATE_GAIN_SETTINGS)
+ annotate.show_analog_gain = MMAL_TRUE;
+
+ if (settings & ANNOTATE_LENS_SETTINGS)
+ annotate.show_lens = MMAL_TRUE;
+
+ if (settings & ANNOTATE_CAF_SETTINGS)
+ annotate.show_caf = MMAL_TRUE;
+
+ if (settings & ANNOTATE_MOTION_SETTINGS)
+ annotate.show_motion = MMAL_TRUE;
+
+ if (settings & ANNOTATE_FRAME_NUMBER)
+ annotate.show_frame_num = MMAL_TRUE;
+
+ if (settings & ANNOTATE_BLACK_BACKGROUND)
+ annotate.enable_text_background = MMAL_TRUE;
+
+ annotate.text_size = text_size;
+
+ if (text_colour != -1)
+ {
+ annotate.custom_text_colour = MMAL_TRUE;
+ annotate.custom_text_Y = text_colour&0xff;
+ annotate.custom_text_U = (text_colour>>8)&0xff;
+ annotate.custom_text_V = (text_colour>>16)&0xff;
+ }
+ else
+ annotate.custom_text_colour = MMAL_FALSE;
+
+ if (bg_colour != -1)
+ {
+ annotate.custom_background_colour = MMAL_TRUE;
+ annotate.custom_background_Y = bg_colour&0xff;
+ annotate.custom_background_U = (bg_colour>>8)&0xff;
+ annotate.custom_background_V = (bg_colour>>16)&0xff;
+ }
+ else
+ annotate.custom_background_colour = MMAL_FALSE;
+ }
+ else
+ annotate.enable = 0;
+
+ return mmal_status_to_int(mmal_port_parameter_set(camera->control, &annotate.hdr));
+}
+
+int raspicamcontrol_set_stereo_mode(MMAL_PORT_T *port, MMAL_PARAMETER_STEREOSCOPIC_MODE_T *stereo_mode)
+{
+ MMAL_PARAMETER_STEREOSCOPIC_MODE_T stereo = { {MMAL_PARAMETER_STEREOSCOPIC_MODE, sizeof(stereo)},
+ MMAL_STEREOSCOPIC_MODE_NONE, MMAL_FALSE, MMAL_FALSE };
+ if (stereo_mode->mode != MMAL_STEREOSCOPIC_MODE_NONE)
+ {
+ stereo.mode = stereo_mode->mode;
+ stereo.decimate = stereo_mode->decimate;
+ stereo.swap_eyes = stereo_mode->swap_eyes;
+ }
+ return mmal_status_to_int(mmal_port_parameter_set(port, &stereo.hdr));
+}
+
+/**
+ * Asked GPU how much memory it has allocated
+ *
+ * @return amount of memory in MB
+ */
+static int raspicamcontrol_get_mem_gpu(void)
+{
+ char response[80] = "";
+ int gpu_mem = 0;
+ if (vc_gencmd(response, sizeof response, "get_mem gpu") == 0)
+ vc_gencmd_number_property(response, "gpu", &gpu_mem);
+ return gpu_mem;
+}
+
+/**
+ * Ask GPU about its camera abilities
+ * @param supported None-zero if software supports the camera
+ * @param detected None-zero if a camera has been detected
+ */
+static void raspicamcontrol_get_camera(int *supported, int *detected)
+{
+ char response[80] = "";
+ if (vc_gencmd(response, sizeof response, "get_camera") == 0)
+ {
+ if (supported)
+ vc_gencmd_number_property(response, "supported", supported);
+ if (detected)
+ vc_gencmd_number_property(response, "detected", detected);
+ }
+}
+
+/**
+ * Check to see if camera is supported, and we have allocated enough meooryAsk GPU about its camera abilities
+ * @param supported None-zero if software supports the camera
+ * @param detected None-zero if a camera has been detected
+ */
+void raspicamcontrol_check_configuration(int min_gpu_mem)
+{
+ int gpu_mem = raspicamcontrol_get_mem_gpu();
+ int supported = 0, detected = 0;
+ raspicamcontrol_get_camera(&supported, &detected);
+ if (!supported)
+ vcos_log_error("Camera is not enabled in this build. Try running \"sudo raspi-config\" and ensure that \"camera\" has been enabled\n");
+ else if (gpu_mem < min_gpu_mem)
+ vcos_log_error("Only %dM of gpu_mem is configured. Try running \"sudo raspi-config\" and ensure that \"memory_split\" has a value of %d or greater\n", gpu_mem, min_gpu_mem);
+ else if (!detected)
+ vcos_log_error("Camera is not detected. Please check carefully the camera module is installed correctly\n");
+ else
+ vcos_log_error("Failed to run camera app. Please check for firmware updates\n");
+}
+
diff --git a/src/RaspiCamControl.cpp b/src/RaspiCamControl.cpp
deleted file mode 100644
index 3743ba0..0000000
--- a/src/RaspiCamControl.cpp
+++ /dev/null
@@ -1,922 +0,0 @@
-/*
-Copyright (c) 2013, Broadcom Europe Ltd
-Copyright (c) 2013, James Hughes
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
- * Redistributions of source code must retain the above copyright
- notice, this list of conditions and the following disclaimer.
- * Redistributions in binary form must reproduce the above copyright
- notice, this list of conditions and the following disclaimer in the
- documentation and/or other materials provided with the distribution.
- * Neither the name of the copyright holder nor the
- names of its contributors may be used to endorse or promote products
- derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifdef __arm__
-
-#include
-#include
-
-#include "interface/vcos/vcos.h"
-
-#include "RaspiCamControl.h"
-#include "interface/mmal/mmal.h"
-#include "interface/mmal/mmal_logging.h"
-#include "interface/mmal/util/mmal_default_components.h"
-#include "interface/mmal/util/mmal_util.h"
-#include "interface/mmal/util/mmal_util_params.h"
-#include "interface/vmcs_host/vc_vchi_gencmd.h"
-
-/// Cross reference structure, mode string against mode id
-typedef struct xref_t {
- char const* mode;
- int mmal_mode;
-} XREF_T;
-
-/// Structure to cross reference exposure strings against the MMAL parameter
-/// equivalent
-static XREF_T exposure_map[] = { { "off", MMAL_PARAM_EXPOSUREMODE_OFF },
- { "auto", MMAL_PARAM_EXPOSUREMODE_AUTO },
- { "night", MMAL_PARAM_EXPOSUREMODE_NIGHT },
- { "nightpreview", MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW },
- { "backlight", MMAL_PARAM_EXPOSUREMODE_BACKLIGHT },
- { "spotlight", MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT },
- { "sports", MMAL_PARAM_EXPOSUREMODE_SPORTS },
- { "snow", MMAL_PARAM_EXPOSUREMODE_SNOW },
- { "beach", MMAL_PARAM_EXPOSUREMODE_BEACH },
- { "verylong", MMAL_PARAM_EXPOSUREMODE_VERYLONG },
- { "fixedfps", MMAL_PARAM_EXPOSUREMODE_FIXEDFPS },
- { "antishake", MMAL_PARAM_EXPOSUREMODE_ANTISHAKE },
- { "fireworks", MMAL_PARAM_EXPOSUREMODE_FIREWORKS } };
-
-static const int exposure_map_size = sizeof(exposure_map) / sizeof(exposure_map[0]);
-
-/// Structure to cross reference awb strings against the MMAL parameter
-/// equivalent
-static XREF_T awb_map[] = { { "off", MMAL_PARAM_AWBMODE_OFF },
- { "auto", MMAL_PARAM_AWBMODE_AUTO },
- { "sun", MMAL_PARAM_AWBMODE_SUNLIGHT },
- { "cloud", MMAL_PARAM_AWBMODE_CLOUDY },
- { "shade", MMAL_PARAM_AWBMODE_SHADE },
- { "tungsten", MMAL_PARAM_AWBMODE_TUNGSTEN },
- { "fluorescent", MMAL_PARAM_AWBMODE_FLUORESCENT },
- { "incandescent", MMAL_PARAM_AWBMODE_INCANDESCENT },
- { "flash", MMAL_PARAM_AWBMODE_FLASH },
- { "horizon", MMAL_PARAM_AWBMODE_HORIZON } };
-
-static const int awb_map_size = sizeof(awb_map) / sizeof(awb_map[0]);
-
-/// Structure to cross reference image effect against the MMAL parameter
-/// equivalent
-static XREF_T imagefx_map[] = { { "none", MMAL_PARAM_IMAGEFX_NONE },
- { "negative", MMAL_PARAM_IMAGEFX_NEGATIVE },
- { "solarise", MMAL_PARAM_IMAGEFX_SOLARIZE },
- { "sketch", MMAL_PARAM_IMAGEFX_SKETCH },
- { "denoise", MMAL_PARAM_IMAGEFX_DENOISE },
- { "emboss", MMAL_PARAM_IMAGEFX_EMBOSS },
- { "oilpaint", MMAL_PARAM_IMAGEFX_OILPAINT },
- { "hatch", MMAL_PARAM_IMAGEFX_HATCH },
- { "gpen", MMAL_PARAM_IMAGEFX_GPEN },
- { "pastel", MMAL_PARAM_IMAGEFX_PASTEL },
- { "watercolour", MMAL_PARAM_IMAGEFX_WATERCOLOUR },
- { "film", MMAL_PARAM_IMAGEFX_FILM },
- { "blur", MMAL_PARAM_IMAGEFX_BLUR },
- { "saturation", MMAL_PARAM_IMAGEFX_SATURATION },
- { "colourswap", MMAL_PARAM_IMAGEFX_COLOURSWAP },
- { "washedout", MMAL_PARAM_IMAGEFX_WASHEDOUT },
- { "posterise", MMAL_PARAM_IMAGEFX_POSTERISE },
- { "colourpoint", MMAL_PARAM_IMAGEFX_COLOURPOINT },
- { "colourbalance", MMAL_PARAM_IMAGEFX_COLOURBALANCE },
- { "cartoon", MMAL_PARAM_IMAGEFX_CARTOON } };
-
-static const int imagefx_map_size = sizeof(imagefx_map) / sizeof(imagefx_map[0]);
-
-static XREF_T metering_mode_map[] = { { "average", MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE },
- { "spot", MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT },
- { "backlit", MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT },
- { "matrix", MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX } };
-
-static const int metering_mode_map_size = sizeof(metering_mode_map) / sizeof(metering_mode_map[0]);
-
-#define CommandSharpness 0
-#define CommandContrast 1
-#define CommandBrightness 2
-#define CommandSaturation 3
-#define CommandISO 4
-#define CommandVideoStab 5
-#define CommandEVComp 6
-#define CommandExposure 7
-#define CommandAWB 8
-#define CommandImageFX 9
-#define CommandColourFX 10
-#define CommandMeterMode 11
-#define CommandRotation 12
-#define CommandHFlip 13
-#define CommandVFlip 14
-#define CommandROI 15
-
-#define parameter_reset -99999
-
-/**
- * Update the passed in parameter according to the rest of the parameters
- * passed in.
- *
- *
- * @return 0 if reached end of cycle for this parameter, !0 otherwise
- */
-static int update_cycle_parameter(int* option, int min, int max, int increment) {
- vcos_assert(option);
- if (!option)
- return 0;
-
- if (*option == parameter_reset)
- *option = min - increment;
-
- *option += increment;
-
- if (*option > max) {
- *option = parameter_reset;
- return 0;
- } else
- return 1;
-}
-
-/**
- * Test/Demo code to cycle through a bunch of camera settings
- * This code is pretty hacky so please don't complain!!
- * It only does stuff that should have a visual impact (hence demo!)
- * This will override any user supplied parameters
- *
- * Each call of this function will move on to the next setting
- *
- * @param camera Pointer to the camera to change settings on.
- * @return 0 if reached end of complete sequence, !0 otherwise
- */
-
-int raspicamcontrol_cycle_test(MMAL_COMPONENT_T& camera) {
- static int parameter = 0;
- static int parameter_option = parameter_reset; // which value the parameter currently has
-
- // We are going to cycle through all the relevant entries in the parameter
- // block and send options to the camera.
- if (parameter == 0) {
- // sharpness
- if (update_cycle_parameter(¶meter_option, -100, 100, 10))
- raspicamcontrol_set_sharpness(camera, parameter_option);
- else {
- raspicamcontrol_set_sharpness(camera, 0);
- parameter++;
- }
- } else if (parameter == 1) {
- // contrast
- if (update_cycle_parameter(¶meter_option, -100, 100, 10))
- raspicamcontrol_set_contrast(camera, parameter_option);
- else {
- raspicamcontrol_set_contrast(camera, 0);
- parameter++;
- }
- } else if (parameter == 2) {
- // brightness
- if (update_cycle_parameter(¶meter_option, 0, 100, 10))
- raspicamcontrol_set_brightness(camera, parameter_option);
- else {
- raspicamcontrol_set_brightness(camera, 50);
- parameter++;
- }
- } else if (parameter == 3) {
- // contrast
- if (update_cycle_parameter(¶meter_option, -100, 100, 10))
- raspicamcontrol_set_saturation(camera, parameter_option);
- else {
- parameter++;
- raspicamcontrol_set_saturation(camera, 0);
- }
- } else if (parameter == 4) {
- // EV
- if (update_cycle_parameter(¶meter_option, -10, 10, 4))
- raspicamcontrol_set_exposure_compensation(camera, parameter_option);
- else {
- raspicamcontrol_set_exposure_compensation(camera, 0);
- parameter++;
- }
- } else if (parameter == 5) {
- // MMAL_PARAM_EXPOSUREMODE_T
- if (update_cycle_parameter(¶meter_option, 0, exposure_map_size, 1))
- raspicamcontrol_set_exposure_mode(camera, (MMAL_PARAM_EXPOSUREMODE_T)exposure_map[parameter_option].mmal_mode);
- else {
- raspicamcontrol_set_exposure_mode(camera, MMAL_PARAM_EXPOSUREMODE_AUTO);
- parameter++;
- }
- } else if (parameter == 6) {
- // MMAL_PARAM_AWB_T
- if (update_cycle_parameter(¶meter_option, 0, awb_map_size, 1))
- raspicamcontrol_set_awb_mode(camera, (MMAL_PARAM_AWBMODE_T)awb_map[parameter_option].mmal_mode);
- else {
- raspicamcontrol_set_awb_mode(camera, MMAL_PARAM_AWBMODE_AUTO);
- parameter++;
- }
- }
- if (parameter == 7) {
- // MMAL_PARAM_IMAGEFX_T
- if (update_cycle_parameter(¶meter_option, 0, imagefx_map_size, 1))
- raspicamcontrol_set_imageFX(camera, (MMAL_PARAM_IMAGEFX_T)imagefx_map[parameter_option].mmal_mode);
- else {
- raspicamcontrol_set_imageFX(camera, MMAL_PARAM_IMAGEFX_NONE);
- parameter++;
- }
- }
- if (parameter == 8) {
- MMAL_PARAM_COLOURFX_T colfx = { 0, 0, 0 };
- switch (parameter_option) {
- case parameter_reset:
- parameter_option = 1;
- colfx.u = 128;
- colfx.v = 128;
- break;
- case 1:
- parameter_option = 2;
- colfx.u = 100;
- colfx.v = 200;
- break;
- case 2:
- parameter_option = parameter_reset;
- colfx.enable = 0;
- parameter++;
- break;
- }
- raspicamcontrol_set_colourFX(camera, &colfx);
- }
-
- // Orientation
- if (parameter == 9) {
- switch (parameter_option) {
- case parameter_reset:
- raspicamcontrol_set_rotation(camera, 90);
- parameter_option = 1;
- break;
-
- case 1:
- raspicamcontrol_set_rotation(camera, 180);
- parameter_option = 2;
- break;
-
- case 2:
- raspicamcontrol_set_rotation(camera, 270);
- parameter_option = 3;
- break;
-
- case 3: {
- raspicamcontrol_set_rotation(camera, 0);
- raspicamcontrol_set_flips(camera, 1, 0);
- parameter_option = 4;
- break;
- }
- case 4: {
- raspicamcontrol_set_flips(camera, 0, 1);
- parameter_option = 5;
- break;
- }
- case 5: {
- raspicamcontrol_set_flips(camera, 1, 1);
- parameter_option = 6;
- break;
- }
- case 6: {
- raspicamcontrol_set_flips(camera, 0, 0);
- parameter_option = parameter_reset;
- parameter++;
- break;
- }
- }
- }
-
- if (parameter == 10) {
- parameter = 1;
- return 0;
- }
-
- return 1;
-}
-
-/**
- * Function to take a string, a mapping, and return the int equivalent
- * @param str Incoming string to match
- * @param map Mapping data
- * @param num_refs The number of items in the mapping data
- * @return The integer match for the string, or -1 if no match
- */
-static int map_xref(const char* str, const XREF_T* map, int num_refs) {
- int i;
-
- for (i = 0; i < num_refs; i++) {
- if (!strcasecmp(str, map[i].mode)) {
- return map[i].mmal_mode;
- }
- }
- return -1;
-}
-
-/**
- * Function to take a mmal enum (as int) and return the string equivalent
- * @param en Incoming int to match
- * @param map Mapping data
- * @param num_refs The number of items in the mapping data
- * @return const pointer to string, or NULL if no match
- */
-static const char* unmap_xref(const int en, XREF_T* map, int num_refs) {
- int i;
-
- for (i = 0; i < num_refs; i++) {
- if (en == map[i].mmal_mode) {
- return map[i].mode;
- }
- }
- return NULL;
-}
-
-/**
- * Convert string to the MMAL parameter for exposure mode
- * @param str Incoming string to match
- * @return MMAL parameter matching the string, or the AUTO option if no match
- * found
- */
-/*static*/ MMAL_PARAM_EXPOSUREMODE_T exposure_mode_from_string(const char* str) {
- int i = map_xref(str, exposure_map, exposure_map_size);
-
- if (i != -1)
- return (MMAL_PARAM_EXPOSUREMODE_T)i;
-
- vcos_log_error("Unknown exposure mode: %s", str);
- return MMAL_PARAM_EXPOSUREMODE_AUTO;
-}
-
-/**
- * Convert string to the MMAL parameter for AWB mode
- * @param str Incoming string to match
- * @return MMAL parameter matching the string, or the AUTO option if no match
- * found
- */
-MMAL_PARAM_AWBMODE_T awb_mode_from_string(const char* str) {
- int i = map_xref(str, awb_map, awb_map_size);
-
- if (i != -1)
- return (MMAL_PARAM_AWBMODE_T)i;
-
- vcos_log_error("Unknown awb mode: %s", str);
- return MMAL_PARAM_AWBMODE_AUTO;
-}
-
-/**
- * Convert string to the MMAL parameter for image effects mode
- * @param str Incoming string to match
- * @return MMAL parameter matching the strong, or the AUTO option if no match
- * found
- */
-MMAL_PARAM_IMAGEFX_T imagefx_mode_from_string(const char* str) {
- int i = map_xref(str, imagefx_map, imagefx_map_size);
-
- if (i != -1)
- return (MMAL_PARAM_IMAGEFX_T)i;
-
- vcos_log_error("Unknown image fx: %s", str);
- return MMAL_PARAM_IMAGEFX_NONE;
-}
-
-/**
- * Convert string to the MMAL parameter for exposure metering mode
- * @param str Incoming string to match
- * @return MMAL parameter matching the string, or the AUTO option if no match
- * found
- */
-MMAL_PARAM_EXPOSUREMETERINGMODE_T metering_mode_from_string(const char* str) {
- int i = map_xref(str, metering_mode_map, metering_mode_map_size);
-
- if (i != -1)
- return (MMAL_PARAM_EXPOSUREMETERINGMODE_T)i;
-
- vcos_log_error("Unknown metering mode: %s", str);
- return MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE;
-}
-
-/**
- * Dump contents of camera parameter structure to stdout for debugging/verbose
- * logging
- *
- * @param params Const ref to parameters structure to dump
- */
-void raspicamcontrol_dump_parameters(const RASPICAM_CAMERA_PARAMETERS& params) {
- const char* exp_mode = unmap_xref(params.exposureMode, exposure_map, exposure_map_size);
- const char* awb_mode = unmap_xref(params.awbMode, awb_map, awb_map_size);
- const char* image_effect = unmap_xref(params.imageEffect, imagefx_map, imagefx_map_size);
- const char* metering_mode = unmap_xref(params.exposureMeterMode, metering_mode_map, metering_mode_map_size);
-
- fprintf(stderr, "Sharpness %d, Contrast %d, Brightness %d\n", params.sharpness, params.contrast, params.brightness);
- fprintf(stderr, "Saturation %d, ISO %d, Video Stabilisation %s, Exposure "
- "compensation %d\n",
- params.saturation, params.ISO, params.videoStabilisation ? "Yes" : "No", params.exposureCompensation);
- fprintf(stderr, "Exposure Mode '%s', AWB Mode '%s', Image Effect '%s'\n", exp_mode, awb_mode, image_effect);
- fprintf(stderr, "Metering Mode '%s', Colour Effect Enabled %s with U = %d, V = %d\n", metering_mode,
- params.colourEffects.enable ? "Yes" : "No", params.colourEffects.u, params.colourEffects.v);
- fprintf(stderr, "Rotation %d, hflip %s, vflip %s\n", params.rotation, params.hflip ? "Yes" : "No",
- params.vflip ? "Yes" : "No");
- fprintf(stderr, "ROI x %lf, y %f, w %f h %f\n", params.roi.x, params.roi.y, params.roi.w, params.roi.h);
-}
-
-/**
- * Convert a MMAL status return value to a simple boolean of success
- * ALso displays a fault if code is not success
- *
- * @param status The error code to convert
- * @return 0 if status is sucess, 1 otherwise
- */
-int mmal_status_to_int(MMAL_STATUS_T status) {
- if (status == MMAL_SUCCESS)
- return 0;
- else {
- switch (status) {
- case MMAL_ENOMEM:
- vcos_log_error("Out of memory");
- break;
- case MMAL_ENOSPC:
- vcos_log_error("Out of resources (other than memory)");
- break;
- case MMAL_EINVAL:
- vcos_log_error("Argument is invalid");
- break;
- case MMAL_ENOSYS:
- vcos_log_error("Function not implemented");
- break;
- case MMAL_ENOENT:
- vcos_log_error("No such file or directory");
- break;
- case MMAL_ENXIO:
- vcos_log_error("No such device or address");
- break;
- case MMAL_EIO:
- vcos_log_error("I/O error");
- break;
- case MMAL_ESPIPE:
- vcos_log_error("Illegal seek");
- break;
- case MMAL_ECORRUPT:
- vcos_log_error("Data is corrupt \attention FIXME: not POSIX");
- break;
- case MMAL_ENOTREADY:
- vcos_log_error("Component is not ready \attention FIXME: not POSIX");
- break;
- case MMAL_ECONFIG:
- vcos_log_error("Component is not configured \attention FIXME: not POSIX");
- break;
- case MMAL_EISCONN:
- vcos_log_error("Port is already connected ");
- break;
- case MMAL_ENOTCONN:
- vcos_log_error("Port is disconnected");
- break;
- case MMAL_EAGAIN:
- vcos_log_error("Resource temporarily unavailable. Try again later");
- break;
- case MMAL_EFAULT:
- vcos_log_error("Bad address");
- break;
- default:
- vcos_log_error("Unknown status error");
- break;
- }
-
- return 1;
- }
-}
-
-/**
- * Give the supplied parameter block a set of default values
- * @params Pointer to parameter block
- */
-void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS& params) {
- params.sharpness = 0;
- params.contrast = 0;
- params.brightness = 50;
- params.saturation = 0;
- params.ISO = 400;
- params.videoStabilisation = 0;
- params.exposureCompensation = 0;
- params.exposureMode = MMAL_PARAM_EXPOSUREMODE_AUTO;
- params.exposureMeterMode = MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE;
- params.awbMode = MMAL_PARAM_AWBMODE_AUTO;
- // params.imageEffect = MMAL_PARAM_IMAGEFX_SATURATION;
- params.imageEffect = MMAL_PARAM_IMAGEFX_NONE;
- params.colourEffects.enable = 0;
- params.colourEffects.u = 128;
- params.colourEffects.v = 128;
- params.rotation = 0;
- params.hflip = params.vflip = 0;
- params.roi.x = params.roi.y = 0.0;
- params.roi.w = params.roi.h = 1.0;
- params.shutter_speed = 0; // automatic
-}
-
-/**
- * Set the specified camera to all the specified settings
- * @param camera Pointer to camera component
- * @param params refrence to parameter block containing parameters
- * @return 0 if successful, none-zero if unsuccessful.
- */
-int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T& camera, const RASPICAM_CAMERA_PARAMETERS& params) {
- int result;
-
- result = raspicamcontrol_set_saturation(camera, params.saturation);
- result += raspicamcontrol_set_sharpness(camera, params.sharpness);
- result += raspicamcontrol_set_contrast(camera, params.contrast);
- result += raspicamcontrol_set_brightness(camera, params.brightness);
- result += raspicamcontrol_set_ISO(camera, params.ISO);
- result += raspicamcontrol_set_video_stabilisation(camera, params.videoStabilisation);
- result += raspicamcontrol_set_exposure_compensation(camera, params.exposureCompensation);
- result += raspicamcontrol_set_exposure_mode(camera, params.exposureMode);
- result += raspicamcontrol_set_metering_mode(camera, params.exposureMeterMode);
- result += raspicamcontrol_set_awb_mode(camera, params.awbMode);
- result += raspicamcontrol_set_imageFX(camera, params.imageEffect);
- result += raspicamcontrol_set_colourFX(camera, ¶ms.colourEffects);
- // result += raspicamcontrol_set_thumbnail_parameters(camera,
- // ¶ms.thumbnailConfig); TODO Not working for some reason
- result += raspicamcontrol_set_rotation(camera, params.rotation);
- result += raspicamcontrol_set_flips(camera, params.hflip, params.vflip);
- result += raspicamcontrol_set_ROI(camera, params.roi);
- result += raspicamcontrol_set_shutter_speed(camera, params.shutter_speed);
- return result;
-}
-
-/**
- * Adjust the saturation level for images
- * @param camera Pointer to camera component
- * @param saturation Value to adjust, -100 to 100
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_saturation(MMAL_COMPONENT_T& camera, int saturation) {
- int ret = 0;
-
- if (saturation >= -100 && saturation <= 100) {
- MMAL_RATIONAL_T value = { saturation, 100 };
- ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera.control, MMAL_PARAMETER_SATURATION, value));
- } else {
- vcos_log_error("Invalid saturation value");
- ret = 1;
- }
-
- return ret;
-}
-
-/**
- * Set the sharpness of the image
- * @param camera Pointer to camera component
- * @param sharpness Sharpness adjustment -100 to 100
- */
-int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T& camera, int sharpness) {
- int ret = 0;
-
- if (sharpness >= -100 && sharpness <= 100) {
- MMAL_RATIONAL_T value = { sharpness, 100 };
- ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera.control, MMAL_PARAMETER_SHARPNESS, value));
- } else {
- vcos_log_error("Invalid sharpness value");
- ret = 1;
- }
-
- return ret;
-}
-
-/**
- * Set the contrast adjustment for the image
- * @param camera Pointer to camera component
- * @param contrast Contrast adjustment -100 to 100
- * @return
- */
-int raspicamcontrol_set_contrast(MMAL_COMPONENT_T& camera, int contrast) {
- int ret = 0;
-
- if (contrast >= -100 && contrast <= 100) {
- MMAL_RATIONAL_T value = { contrast, 100 };
- ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera.control, MMAL_PARAMETER_CONTRAST, value));
- } else {
- vcos_log_error("Invalid contrast value");
- ret = 1;
- }
-
- return ret;
-}
-
-/**
- * Adjust the brightness level for images
- * @param camera Pointer to camera component
- * @param brightness Value to adjust, 0 to 100
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_brightness(MMAL_COMPONENT_T& camera, int brightness) {
- int ret = 0;
-
- if (brightness >= 0 && brightness <= 100) {
- MMAL_RATIONAL_T value = { brightness, 100 };
- ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera.control, MMAL_PARAMETER_BRIGHTNESS, value));
- } else {
- vcos_log_error("Invalid brightness value");
- ret = 1;
- }
-
- return ret;
-}
-
-/**
- * Adjust the ISO used for images
- * @param camera Pointer to camera component
- * @param ISO Value to set TODO :
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_ISO(MMAL_COMPONENT_T& camera, int ISO) {
- return mmal_status_to_int(mmal_port_parameter_set_uint32(camera.control, MMAL_PARAMETER_ISO, ISO));
-}
-
-/**
- * Adjust the metering mode for images
- * @param camera Pointer to camera component
- * @param saturation Value from following
- * - MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE,
- * - MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT,
- * - MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT,
- * - MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T m_mode) {
- MMAL_PARAMETER_EXPOSUREMETERINGMODE_T meter_mode = { { MMAL_PARAMETER_EXP_METERING_MODE, sizeof(meter_mode) },
- m_mode };
-
- return mmal_status_to_int(mmal_port_parameter_set(camera.control, &meter_mode.hdr));
-}
-
-/**
- * Set the video stabilisation flag. Only used in video mode
- * @param camera Pointer to camera component
- * @param saturation Flag 0 off 1 on
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T& camera, int vstabilisation) {
- return mmal_status_to_int(
- mmal_port_parameter_set_boolean(camera.control, MMAL_PARAMETER_VIDEO_STABILISATION, vstabilisation));
-}
-
-/**
- * Adjust the exposure compensation for images (EV)
- * @param camera Pointer to camera component
- * @param exp_comp Value to adjust, -10 to +10
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T& camera, int exp_comp) {
- return mmal_status_to_int(mmal_port_parameter_set_int32(camera.control, MMAL_PARAMETER_EXPOSURE_COMP, exp_comp));
-}
-
-/**
- * Set exposure mode for images
- * @param camera Pointer to camera component
- * @param mode Exposure mode to set from
- * - MMAL_PARAM_EXPOSUREMODE_OFF,
- * - MMAL_PARAM_EXPOSUREMODE_AUTO,
- * - MMAL_PARAM_EXPOSUREMODE_NIGHT,
- * - MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW,
- * - MMAL_PARAM_EXPOSUREMODE_BACKLIGHT,
- * - MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT,
- * - MMAL_PARAM_EXPOSUREMODE_SPORTS,
- * - MMAL_PARAM_EXPOSUREMODE_SNOW,
- * - MMAL_PARAM_EXPOSUREMODE_BEACH,
- * - MMAL_PARAM_EXPOSUREMODE_VERYLONG,
- * - MMAL_PARAM_EXPOSUREMODE_FIXEDFPS,
- * - MMAL_PARAM_EXPOSUREMODE_ANTISHAKE,
- * - MMAL_PARAM_EXPOSUREMODE_FIREWORKS,
- *
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_EXPOSUREMODE_T mode) {
- MMAL_PARAMETER_EXPOSUREMODE_T exp_mode = { { MMAL_PARAMETER_EXPOSURE_MODE, sizeof(exp_mode) }, mode };
-
- return mmal_status_to_int(mmal_port_parameter_set(camera.control, &exp_mode.hdr));
-}
-
-/**
- * Set the aWB (auto white balance) mode for images
- * @param camera Pointer to camera component
- * @param awb_mode Value to set from
- * - MMAL_PARAM_AWBMODE_OFF,
- * - MMAL_PARAM_AWBMODE_AUTO,
- * - MMAL_PARAM_AWBMODE_SUNLIGHT,
- * - MMAL_PARAM_AWBMODE_CLOUDY,
- * - MMAL_PARAM_AWBMODE_SHADE,
- * - MMAL_PARAM_AWBMODE_TUNGSTEN,
- * - MMAL_PARAM_AWBMODE_FLUORESCENT,
- * - MMAL_PARAM_AWBMODE_INCANDESCENT,
- * - MMAL_PARAM_AWBMODE_FLASH,
- * - MMAL_PARAM_AWBMODE_HORIZON,
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_AWBMODE_T awb_mode) {
- MMAL_PARAMETER_AWBMODE_T param = { { MMAL_PARAMETER_AWB_MODE, sizeof(param) }, awb_mode };
-
- return mmal_status_to_int(mmal_port_parameter_set(camera.control, ¶m.hdr));
-}
-
-/**
- * Set the image effect for the images
- * @param camera Pointer to camera component
- * @param imageFX Value from
- * - MMAL_PARAM_IMAGEFX_NONE,
- * - MMAL_PARAM_IMAGEFX_NEGATIVE,
- * - MMAL_PARAM_IMAGEFX_SOLARIZE,
- * - MMAL_PARAM_IMAGEFX_POSTERIZE,
- * - MMAL_PARAM_IMAGEFX_WHITEBOARD,
- * - MMAL_PARAM_IMAGEFX_BLACKBOARD,
- * - MMAL_PARAM_IMAGEFX_SKETCH,
- * - MMAL_PARAM_IMAGEFX_DENOISE,
- * - MMAL_PARAM_IMAGEFX_EMBOSS,
- * - MMAL_PARAM_IMAGEFX_OILPAINT,
- * - MMAL_PARAM_IMAGEFX_HATCH,
- * - MMAL_PARAM_IMAGEFX_GPEN,
- * - MMAL_PARAM_IMAGEFX_PASTEL,
- * - MMAL_PARAM_IMAGEFX_WATERCOLOUR,
- * - MMAL_PARAM_IMAGEFX_FILM,
- * - MMAL_PARAM_IMAGEFX_BLUR,
- * - MMAL_PARAM_IMAGEFX_SATURATION,
- * - MMAL_PARAM_IMAGEFX_COLOURSWAP,
- * - MMAL_PARAM_IMAGEFX_WASHEDOUT,
- * - MMAL_PARAM_IMAGEFX_POSTERISE,
- * - MMAL_PARAM_IMAGEFX_COLOURPOINT,
- * - MMAL_PARAM_IMAGEFX_COLOURBALANCE,
- * - MMAL_PARAM_IMAGEFX_CARTOON,
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T& camera, MMAL_PARAM_IMAGEFX_T imageFX) {
- MMAL_PARAMETER_IMAGEFX_T imgFX = { { MMAL_PARAMETER_IMAGE_EFFECT, sizeof(imgFX) }, imageFX };
-
- return mmal_status_to_int(mmal_port_parameter_set(camera.control, &imgFX.hdr));
-}
-
-/* TODO :what to do with the image effects parameters?
- MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imfx_param =
-{{MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,sizeof(imfx_param)}, imageFX, 0, {0}};
-mmal_port_parameter_set(camera.control, &imfx_param.hdr);
- */
-
-/**
- * Set the colour effect for images (Set UV component)
- * @param camera Pointer to camera component
- * @param colourFX Contains enable state and U and V numbers to set (e.g.
- * 128,128 = Black and white)
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T& camera, const MMAL_PARAM_COLOURFX_T* colourFX) {
- MMAL_PARAMETER_COLOURFX_T colfx = { { MMAL_PARAMETER_COLOUR_EFFECT, sizeof(colfx) }, 0, 0, 0 };
-
- colfx.enable = colourFX->enable;
- colfx.u = colourFX->u;
- colfx.v = colourFX->v;
-
- return mmal_status_to_int(mmal_port_parameter_set(camera.control, &colfx.hdr));
-}
-
-/**
- * Set the rotation of the image
- * @param camera Pointer to camera component
- * @param rotation Degree of rotation (any number, but will be converted to
- * 0,90,180 or 270 only)
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_rotation(MMAL_COMPONENT_T& camera, int rotation) {
- int ret;
- int my_rotation = ((rotation % 360) / 90) * 90;
-
- ret = mmal_port_parameter_set_int32(camera.output[0], MMAL_PARAMETER_ROTATION, my_rotation);
- mmal_port_parameter_set_int32(camera.output[1], MMAL_PARAMETER_ROTATION, my_rotation);
- mmal_port_parameter_set_int32(camera.output[2], MMAL_PARAMETER_ROTATION, my_rotation);
-
- return ret;
-}
-
-/**
- * Set the flips state of the image
- * @param camera Pointer to camera component
- * @param hflip If true, horizontally flip the image
- * @param vflip If true, vertically flip the image
- *
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_flips(MMAL_COMPONENT_T& camera, int hflip, int vflip) {
- MMAL_PARAMETER_MIRROR_T mirror = { { MMAL_PARAMETER_MIRROR, sizeof(MMAL_PARAMETER_MIRROR_T) },
- MMAL_PARAM_MIRROR_NONE };
-
- if (hflip && vflip)
- mirror.value = MMAL_PARAM_MIRROR_BOTH;
- else if (hflip)
- mirror.value = MMAL_PARAM_MIRROR_HORIZONTAL;
- else if (vflip)
- mirror.value = MMAL_PARAM_MIRROR_VERTICAL;
-
- mmal_port_parameter_set(camera.output[0], &mirror.hdr);
- mmal_port_parameter_set(camera.output[1], &mirror.hdr);
- return mmal_port_parameter_set(camera.output[2], &mirror.hdr);
-}
-
-/**
- * Set the ROI of the sensor to use for captures/preview
- * @param camera Pointer to camera component
- * @param rect Normalised coordinates of ROI rectangle
- *
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_ROI(MMAL_COMPONENT_T& camera, PARAM_FLOAT_RECT_T rect) {
- MMAL_PARAMETER_INPUT_CROP_T crop = { { MMAL_PARAMETER_INPUT_CROP, sizeof(MMAL_PARAMETER_INPUT_CROP_T) } };
-
- crop.rect.x = (65536 * rect.x);
- crop.rect.y = (65536 * rect.y);
- crop.rect.width = (65536 * rect.w);
- crop.rect.height = (65536 * rect.h);
-
- return mmal_port_parameter_set(camera.control, &crop.hdr);
-}
-
-/**
- * Set the shuuter_speed of the sensor
- * @param camera Pointer to camera component
- * @param the shutter_speed in microseconds
- * @return 0 if successful, non-zero if any parameters out of range
- */
-int raspicamcontrol_set_shutter_speed(MMAL_COMPONENT_T& camera, int shutter_speed) {
- return mmal_status_to_int(
- mmal_port_parameter_set_uint32(camera.control, MMAL_PARAMETER_SHUTTER_SPEED, shutter_speed));
-}
-
-/**
- * Asked GPU how much memory it has allocated
- *
- * @return amount of memory in MB
- */
-/*static int raspicamcontrol_get_mem_gpu(void)
-{
- char response[80] = "";
- int gpu_mem = 0;
- if (vc_gencmd(response, sizeof response, "get_mem gpu") == 0)
- vc_gencmd_number_property(response, "gpu", &gpu_mem);
- return gpu_mem;
-}*/
-
-/**
- * Ask GPU about its camera abilities
- * @param supported None-zero if software supports the camera
- * @param detected None-zero if a camera has been detected
- */
-/*static void raspicamcontrol_get_camera(int *supported, int *detected)
-{
- char response[80] = "";
- if (vc_gencmd(response, sizeof response, "get_camera") == 0)
- {
- if (supported)
- vc_gencmd_number_property(response, "supported", supported);
- if (detected)
- vc_gencmd_number_property(response, "detected", detected);
- }
-}*/
-
-/**
- * Check to see if camera is supported, and we have allocated enough meooryAsk
- * GPU about its camera abilities
- * @param supported None-zero if software supports the camera
- * @param detected None-zero if a camera has been detected
- */
-void raspicamcontrol_check_configuration(int min_gpu_mem) {
- int gpu_mem = 256; // raspicamcontrol_get_mem_gpu();
- int supported = 1, detected = 1;
- // raspicamcontrol_get_camera(&supported, &detected);
- if (!supported)
- vcos_log_error("Camera is not enabled in this build. Try running \"sudo "
- "raspi-config\" and ensure that \"camera\" has been enabled\n");
- else if (gpu_mem < min_gpu_mem)
- vcos_log_error("Only %dM of gpu_mem is configured. Try running \"sudo "
- "raspi-config\" and ensure that \"memory_split\" has a value of %d "
- "or greater\n",
- gpu_mem, min_gpu_mem);
- else if (!detected)
- vcos_log_error("Camera is not detected. Please check carefully the camera module "
- "is installed correctly\n");
- else
- vcos_log_error("Failed to run camera app. Please check for firmware updates\n");
-}
-
-#endif // __arm__
diff --git a/src/raspicam_node.cpp b/src/raspicam_node.cpp
index 85d9de7..f4f5f56 100644
--- a/src/raspicam_node.cpp
+++ b/src/raspicam_node.cpp
@@ -71,9 +71,9 @@ int main(int argc, char** argv) {
#include "raspicam_node/MotionVectors.h"
#include
#include
-
+extern "C" {
#include "RaspiCamControl.h"
-
+}
#include
#include
@@ -141,8 +141,8 @@ typedef struct MMAL_PORT_USERDATA_T {
RASPIVID_STATE& pstate; // pointer to our state for use by callback
bool abort; // Set to 1 in callback if an error occurs to attempt to abort
// the capture
- int frame;
- int id;
+ size_t frame;
+ size_t id;
int frames_skipped = 0;
} PORT_USERDATA;
@@ -194,7 +194,7 @@ static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) {
nh.param("camera_id", state.camera_id, 0);
// Set up the camera_parameters to default
- raspicamcontrol_set_defaults(state.camera_parameters);
+ raspicamcontrol_set_defaults(&state.camera_parameters);
bool temp;
nh.param("hFlip", temp, false);
@@ -374,7 +374,7 @@ static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* bu
PORT_USERDATA* pData = port->userdata;
if (pData && pData->pstate.isInit) {
size_t bytes_written = buffer->length;
- if (buffer->length) {
+ if (buffer->length > 0) {
if (pData->id != INT_MAX) {
if (pData->id + buffer->length > IMG_BUFFER_SIZE) {
ROS_ERROR("pData->id (%d) + buffer->length (%d) > "
@@ -415,6 +415,7 @@ static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* bu
image.msg.height = pData->pstate.height;
image.msg.width = pData->pstate.width;
image.msg.step = (pData->pstate.width * 3);
+ // ROS_INFO("pData->id = %lu, height = %d, width = %d, size = %lu, frame = %lu\n", pData->id - buffer->offset, pData->pstate.height, pData->pstate.width, pData->pstate.height*pData->pstate.width * 3, pData->frame & 1);
auto start = pData->buffer[pData->frame & 1].get();
auto end = &(pData->buffer[pData->frame & 1].get()[pData->id]);
image.msg.data.resize(pData->id);
@@ -469,6 +470,27 @@ static MMAL_COMPONENT_T* create_camera_component(RASPIVID_STATE& state) {
goto error;
}
+ // state.camera_parameters.stereo_mode.mode = stereo_mode_from_string("tb");
+ status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[0], &state.camera_parameters.stereo_mode));
+ if (status != MMAL_SUCCESS)
+ {
+ vcos_log_error("Could not set stereo mode : error %d", status);
+ goto error;
+ }
+ status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[1], &state.camera_parameters.stereo_mode));
+
+ if (status != MMAL_SUCCESS)
+ {
+ vcos_log_error("Could not set stereo mode : error %d", status);
+ goto error;
+ }
+ status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[2], &state.camera_parameters.stereo_mode));
+
+ if (status != MMAL_SUCCESS)
+ {
+ vcos_log_error("Could not set stereo mode : error %d", status);
+ goto error;
+ }
if (!camera->output_num) {
vcos_log_error("Camera doesn't have output ports");
ROS_ERROR("Camera doesn't have output ports");
@@ -606,7 +628,7 @@ static MMAL_COMPONENT_T* create_camera_component(RASPIVID_STATE& state) {
goto error;
}
- raspicamcontrol_set_all_parameters(*camera, state.camera_parameters);
+ raspicamcontrol_set_all_parameters(camera, &state.camera_parameters);
state.camera_component.reset(camera);
@@ -1285,22 +1307,22 @@ void reconfigure_callback(raspicam_node::CameraConfig& config, uint32_t level, R
PARAM_FLOAT_RECT_T roi;
roi.x = roi.y = offset;
roi.w = roi.h = size;
- raspicamcontrol_set_ROI(*state.camera_component, roi);
+ raspicamcontrol_set_ROI(state.camera_component.get(), roi);
}
- raspicamcontrol_set_exposure_mode(*state.camera_component, exposure_mode_from_string(config.exposure_mode.c_str()));
+ raspicamcontrol_set_exposure_mode(state.camera_component.get(), exposure_mode_from_string(config.exposure_mode.c_str()));
- raspicamcontrol_set_awb_mode(*state.camera_component, awb_mode_from_string(config.awb_mode.c_str()));
+ raspicamcontrol_set_awb_mode(state.camera_component.get(), awb_mode_from_string(config.awb_mode.c_str()));
- raspicamcontrol_set_contrast(*state.camera_component, config.contrast);
- raspicamcontrol_set_sharpness(*state.camera_component, config.sharpness);
- raspicamcontrol_set_brightness(*state.camera_component, config.brightness);
- raspicamcontrol_set_saturation(*state.camera_component, config.saturation);
- raspicamcontrol_set_ISO(*state.camera_component, config.ISO);
- raspicamcontrol_set_exposure_compensation(*state.camera_component, config.exposure_compensation);
- raspicamcontrol_set_video_stabilisation(*state.camera_component, config.video_stabilisation);
- raspicamcontrol_set_flips(*state.camera_component, config.hFlip, config.vFlip);
- raspicamcontrol_set_shutter_speed(*state.camera_component, config.shutter_speed);
+ raspicamcontrol_set_contrast(state.camera_component.get(), config.contrast);
+ raspicamcontrol_set_sharpness(state.camera_component.get(), config.sharpness);
+ raspicamcontrol_set_brightness(state.camera_component.get(), config.brightness);
+ raspicamcontrol_set_saturation(state.camera_component.get(), config.saturation);
+ raspicamcontrol_set_ISO(state.camera_component.get(), config.ISO);
+ raspicamcontrol_set_exposure_compensation(state.camera_component.get(), config.exposure_compensation);
+ raspicamcontrol_set_video_stabilisation(state.camera_component.get(), config.video_stabilisation);
+ raspicamcontrol_set_flips(state.camera_component.get(), config.hFlip, config.vFlip);
+ raspicamcontrol_set_shutter_speed(state.camera_component.get(), config.shutter_speed);
ROS_DEBUG("Reconfigure done");
}
diff --git a/src/stereo_node.cpp b/src/stereo_node.cpp
new file mode 100644
index 0000000..ba50bfb
--- /dev/null
+++ b/src/stereo_node.cpp
@@ -0,0 +1,998 @@
+/*
+Copyright (c) 2013, Broadcom Europe Ltd
+Copyright (c) 2013, James Hughes
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of the copyright holder nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#define DBG_MSG(msg) std::cout << msg << std::endl;
+
+#ifdef __x86_64__
+
+#include
+
+int main(int argc, char** argv) {
+ (void)fprintf(stderr, "The raspicam_node for the x86/64 architecture is a fake!\n");
+ return 1;
+}
+
+#endif // __x86_64__
+
+// #ifdef __arm__
+#define SHUTDOWN(reason) \
+ ROS_ERROR(reason); \
+ close_cam(state); \
+ ros::shutdown();
+
+#define IF_FALSE_SHUTDOWN(statement, reason) \
+ if(statement == 1) \
+ { \
+ SHUTDOWN(reason) \
+ return; \
+ }
+
+// We use some GNU extensions (basename)
+#include
+#include
+#include
+#include
+
+#include
+
+#define VCOS_ALWAYS_WANT_LOGGING
+#define VERSION_STRING "v1.2"
+
+#include "bcm_host.h"
+#include "interface/vcos/vcos.h"
+
+#include "interface/mmal/mmal.h"
+#include "interface/mmal/mmal_buffer.h"
+#include "interface/mmal/mmal_logging.h"
+#include "interface/mmal/util/mmal_connection.h"
+#include "interface/mmal/util/mmal_default_components.h"
+#include "interface/mmal/util/mmal_util.h"
+#include "interface/mmal/util/mmal_util_params.h"
+
+#include "camera_info_manager/camera_info_manager.h"
+#include "ros/ros.h"
+#include "sensor_msgs/CameraInfo.h"
+#include "sensor_msgs/CompressedImage.h"
+#include "sensor_msgs/Image.h"
+#include "sensor_msgs/SetCameraInfo.h"
+#include
+#include "std_srvs/Empty.h"
+// #include "raspicam_node/MotionVectors.h"
+#include
+#include
+extern "C" {
+#include "RaspiCamControl.h"
+}
+#include
+#include
+
+#include "mmal_cxx_helper.h"
+#include
+#include
+#include
+// 16 193 408*3 = 48 580 224
+static constexpr int IMG_BUFFER_SIZE = 50 * 1024 * 1024; // 10 MB
+
+// Video format information
+static constexpr int VIDEO_FRAME_RATE_DEN = 1;
+
+// Video render needs at least 2 buffers.
+static constexpr int VIDEO_OUTPUT_BUFFERS_NUM = 3;
+
+/** Structure containing all state information for the current run
+ */
+struct RASPIVID_STATE {
+ RASPIVID_STATE()
+ : camera_component(nullptr)
+ , splitter_component(nullptr)
+ , splitter_connection(nullptr)
+ , splitter_pool(nullptr, mmal::default_delete_pool){}
+ bool isInit;
+ int width; /// Requested width of image
+ int height; /// requested height of image
+ int framerate; /// Requested frame rate (fps)
+ int quality;
+ int camera_id = 0;
+ std::string encoding;
+ std::string imagefx_mode;
+
+ RASPICAM_CAMERA_PARAMETERS camera_parameters; /// Camera setup parameters
+
+ mmal::component_ptr camera_component;
+ mmal::component_ptr splitter_component;
+
+ mmal::connection_ptr splitter_connection; /// Pointer to camera => splitter
+ mmal::pool_ptr splitter_pool; // Pointer buffer pool used by splitter (raw) output
+ // The Updater class advertises to /diagnostics, and has a
+ // ~diagnostic_period parameter that says how often the diagnostics
+ // should be published.
+ diagnostic_updater::Updater updater;
+};
+
+/** Struct used to pass information in encoder port userdata to callback
+ */
+typedef struct MMAL_PORT_USERDATA_T {
+ MMAL_PORT_USERDATA_T(RASPIVID_STATE& state) : pstate(state){};
+ std::unique_ptr buffer[2]; // Memory to write buffer data to.
+ RASPIVID_STATE& pstate; // pointer to our state for use by callback
+ bool abort; // Set to 1 in callback if an error occurs to attempt to abort
+ // the capture
+ int frame;
+ int id;
+
+ int frames_skipped = 0;
+} PORT_USERDATA;
+
+using diagnostic_updater::DiagnosedPublisher;
+using diagnostic_updater::FrequencyStatusParam;
+using diagnostic_updater::TimeStampStatusParam;
+
+// Helper template
+template
+struct DiagnosedMsgPublisher {
+ std::unique_ptr> pub;
+ T msg;
+};
+
+static DiagnosedMsgPublisher left_image;
+static DiagnosedMsgPublisher right_image;
+
+ros::Publisher left_camera_info_pub;
+ros::Publisher right_camera_info_pub;
+sensor_msgs::CameraInfo left_info;
+sensor_msgs::CameraInfo right_info;
+std::string camera_frame_id;
+std::string left_camera_name, right_camera_name;
+int skip_frames = 0;
+
+bool encode(cv::Mat& source_image,
+ sensor_msgs::Image& resized_msg, std::string encoding)
+{
+ // cv::Mat resized_image;
+ // cv::resize(source_image, resized_image, size, 0, 0, cv::INTER_LINEAR);
+ cv_bridge::CvImage bridge;
+ bridge.image = source_image;
+ bridge.header = resized_msg.header;
+ bridge.encoding = resized_msg.encoding;
+ resized_msg = *bridge.toImageMsg().get();
+ return true;
+}
+
+void print_image(const sensor_msgs::Image& msg)
+{
+ std::cout << "header { stamp: { sec: " << msg.header.stamp.sec << ", nsec: " << msg.header.stamp.nsec
+ << ", frameid: " << msg.header.frame_id << " }, \nheight: " << msg.height << "\nwidth: " << msg.width
+ << "\nstep: " << msg.step << "\nencoding: " << msg.encoding << "\ndata_size: " << msg.data.size() << std::endl;
+}
+
+int close_cam(RASPIVID_STATE& state) {
+ if (state.isInit) {
+ state.isInit = false;
+ MMAL_COMPONENT_T* camera = state.camera_component.get();
+ MMAL_COMPONENT_T* splitter = state.splitter_component.get();
+
+ // Destroy splitter port connection
+ state.splitter_connection.reset(nullptr);
+
+
+ // Destroy splitter component
+ if (splitter) {
+ // Get rid of any port buffers first
+ state.splitter_pool.reset(nullptr);
+ // Delete callback structure
+ if (splitter->output[1]->userdata) {
+ delete splitter->output[1]->userdata;
+ }
+ state.splitter_component.reset(nullptr);
+ }
+
+ // destroy camera component
+ if (camera) {
+ state.camera_component.reset(nullptr);
+ }
+ ROS_INFO("Video capture stopped\n");
+ return 0;
+ } else
+ return 1;
+}
+
+static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) {
+ nh.param("imagefx_mode", state.imagefx_mode, "denoise");
+ nh.param("encoding", state.encoding, "bgr8");
+
+
+ nh.param("width", state.width, 640);
+ nh.param("height", state.height, 480);
+ state.height*=2;
+ nh.param("quality", state.quality, 100);
+ if (state.quality < 0 && state.quality > 100) {
+ ROS_WARN("quality: %d is outside valid range 0-100, defaulting to 80", state.quality);
+ state.quality = 80;
+ }
+ nh.param("framerate", state.framerate, 15);
+ if (state.framerate < 0 && state.framerate > 90) {
+ ROS_WARN("framerate: %d is outside valid range 0-90, defaulting to 30", state.framerate);
+ state.framerate = 30;
+ }
+
+ // nh.param("camera_frame_id", camera_frame_id, "stereo");
+
+ nh.param("camera_id", state.camera_id, 0);
+
+ // Set up the camera_parameters to default
+ raspicamcontrol_set_defaults(&state.camera_parameters);
+ state.camera_parameters.imageEffect = imagefx_mode_from_string(state.imagefx_mode.c_str());
+ bool temp;
+ nh.param("hFlip", temp, false);
+ state.camera_parameters.hflip = temp; // Hack for bool param => int variable
+ nh.param("vFlip", temp, false);
+ state.camera_parameters.vflip = temp; // Hack for bool param => int variable
+ nh.param("shutter_speed", state.camera_parameters.shutter_speed, 0);
+
+ if(state.encoding == "mono8")
+ state.encoding = sensor_msgs::image_encodings::MONO8;
+ // state.camera_parameters.colourEffects.enable = 1;
+ else if(state.encoding == "bgr8")
+ {
+ state.encoding = sensor_msgs::image_encodings::BGR8;
+ }
+ else
+ {
+ ROS_WARN("Unknown encoding. Set to default");
+ state.encoding = sensor_msgs::image_encodings::BGR8;
+ }
+ state.isInit = false;
+}
+
+/**
+ * buffer header callback function for image encoder
+ *
+ * Callback will dump buffer data to the specific file
+ *
+ * @param port Pointer to port from which callback originated
+ * @param buffer mmal buffer header pointer
+ */
+void update_image(sensor_msgs::Image& msg, PORT_USERDATA* pData, ros::Time ts, const std::string& frame_name)
+{
+ msg.header.frame_id = frame_name;
+ msg.header.stamp = ts; //(double(buffer->dts)/1e6);
+ msg.encoding = pData->pstate.encoding;
+ msg.is_bigendian = false;
+ msg.height = pData->pstate.height;
+ msg.width = pData->pstate.width;
+ uint8_t channels = 1;
+ if(pData->pstate.encoding == sensor_msgs::image_encodings::BGR8)
+ channels = 3;
+ msg.step = (pData->pstate.width * channels);
+}
+static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* buffer) {
+ // We pass our file handle and other stuff in via the userdata field.
+ auto ts = ros::Time::now();
+ PORT_USERDATA* pData = port->userdata;
+ if (pData && pData->pstate.isInit) {
+ size_t bytes_written = buffer->length;
+ if (buffer->length) {
+ if (pData->id != INT_MAX) {
+ if (pData->id + buffer->length > IMG_BUFFER_SIZE) {
+ ROS_ERROR("pData->id (%d) + buffer->length (%d) > "
+ "IMG_BUFFER_SIZE (%d), skipping the frame",
+ pData->id, buffer->length, IMG_BUFFER_SIZE);
+ pData->id = INT_MAX; // mark this frame corrupted
+ } else {
+ mmal_buffer_header_mem_lock(buffer);
+ memcpy(&(pData->buffer[pData->frame & 1].get()[pData->id]), buffer->data, buffer->length);
+ pData->id += bytes_written;
+ mmal_buffer_header_mem_unlock(buffer);
+ }
+ }
+ }
+
+ if (bytes_written != buffer->length) {
+ vcos_log_error("Failed to write buffer data (%d from %d)- aborting", bytes_written, buffer->length);
+ ROS_ERROR("Failed to write buffer data (%d from %d)- aborting", bytes_written, buffer->length);
+ pData->abort = true;
+ }
+
+ int complete = false;
+ if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED))
+ complete = true;
+
+ if (complete) {
+ if (pData->id != INT_MAX) {
+ // ROS_INFO("Frame size %d", pData->id);
+ if (skip_frames > 0 && pData->frames_skipped < skip_frames) {
+ pData->frames_skipped++;
+ } else {
+ pData->frames_skipped = 0;
+
+ update_image(left_image.msg, pData, ts, left_camera_name);
+ update_image(right_image.msg, pData, ts, right_camera_name);
+ auto start = pData->buffer[pData->frame & 1].get();
+ auto end = &(pData->buffer[pData->frame & 1].get()[pData->id/2]);
+ auto start1 = &(pData->buffer[pData->frame & 1].get()[pData->id/2]);
+ auto end1 = &(pData->buffer[pData->frame & 1].get()[pData->id]);
+ if(start == NULL || start1 == NULL || end == NULL || end1 == NULL)
+ {
+ ROS_WARN("Failed to capture image. Empty buffer!");
+ return;
+ }
+ int mat_encoding = CV_8UC3;
+ // if(pData->pstate.encoding == sensor_msgs::image_encodings::BGR8)
+ // mat_encoding = CV_8UC3;
+ cv::Mat left_image_cv(cv::Size(pData->pstate.width, pData->pstate.height/2), mat_encoding );
+ cv::Mat right_image_cv(cv::Size(pData->pstate.width, pData->pstate.height/2), mat_encoding);
+ // left_image.msg.data.resize(pData->id/2);
+ // right_image.msg.data.resize(pData->id/2);
+ // std::copy(start1, end1, left_image.msg.data.begin());
+ std::copy(start, end, left_image_cv.data);
+ std::copy(start1, end1, right_image_cv.data);
+
+ if(pData->pstate.encoding == sensor_msgs::image_encodings::MONO8)
+ {
+ cv::cvtColor(left_image_cv, left_image_cv, cv::COLOR_BGR2GRAY);
+ cv::cvtColor(right_image_cv, right_image_cv, cv::COLOR_BGR2GRAY);
+ }
+ // cv::imwrite("/home/pi/Documents/left/image.png", left_image_cv);
+ // cv::imwrite("/home/pi/Documents/right/image.png", right_image_cv);
+ // std::copy(start, end, right_image.msg.data.begin());
+ encode(left_image_cv, left_image.msg, pData->pstate.encoding);
+ encode(right_image_cv, right_image.msg, pData->pstate.encoding);
+ // ROS_WARN("Publishing left image starting...");
+ left_image.pub->publish(left_image.msg);
+ right_image.pub->publish(right_image.msg);
+
+
+ left_info.header = left_image.msg.header;
+ right_info.header = left_image.msg.header;
+ left_camera_info_pub.publish(left_info);
+ right_camera_info_pub.publish(right_info);
+
+ }
+ }
+ pData->frame++;
+ pData->id = 0;
+ }
+ }
+
+ // release buffer back to the pool
+ mmal_buffer_header_release(buffer);
+
+ // and send one back to the port (if still open)
+ if (port->is_enabled) {
+ MMAL_STATUS_T status;
+
+ MMAL_BUFFER_HEADER_T* new_buffer = mmal_queue_get(pData->pstate.splitter_pool->queue);
+
+ if (new_buffer)
+ status = mmal_port_send_buffer(port, new_buffer);
+
+ if (!new_buffer || status != MMAL_SUCCESS) {
+ vcos_log_error("Unable to return a buffer to the splitter port");
+ ROS_ERROR("Unable to return a buffer to the splitter port");
+ }
+ }
+}
+
+/**
+ * Create the camera component, set up its ports
+ *
+ * @param state Pointer to state control struct
+ *
+ * @return 0 if failed, pointer to component if successful
+ *
+ */
+static MMAL_COMPONENT_T* create_camera_component(RASPIVID_STATE& state) {
+ MMAL_COMPONENT_T* camera = 0;
+ MMAL_ES_FORMAT_T* format;
+ MMAL_PORT_T *video_port = nullptr, *preview_port = nullptr, *still_port = nullptr;
+ MMAL_STATUS_T status;
+
+ /* Create the component */
+ status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
+
+ if (status != MMAL_SUCCESS) {
+ vcos_log_error("Failed to create camera component");
+ ROS_ERROR("Failed to create camera component");
+ goto error;
+ }
+
+ state.camera_parameters.stereo_mode.mode = stereo_mode_from_string("tb");
+ status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[0], &state.camera_parameters.stereo_mode));
+ if (status != MMAL_SUCCESS)
+ {
+ vcos_log_error("Could not set stereo mode : error %d", status);
+ goto error;
+ }
+ status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[1], &state.camera_parameters.stereo_mode));
+
+ if (status != MMAL_SUCCESS)
+ {
+ vcos_log_error("Could not set stereo mode : error %d", status);
+ goto error;
+ }
+ status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[2], &state.camera_parameters.stereo_mode));
+
+ if (status != MMAL_SUCCESS)
+ {
+ vcos_log_error("Could not set stereo mode : error %d", status);
+ goto error;
+ }
+ if (!camera->output_num) {
+ vcos_log_error("Camera doesn't have output ports");
+ ROS_ERROR("Camera doesn't have output ports");
+ goto error;
+ }
+
+ video_port = camera->output[mmal::camera_port::video];
+ preview_port = camera->output[mmal::camera_port::preview];
+ still_port = camera->output[mmal::camera_port::capture];
+
+ // set up the camera configuration
+ {
+ MMAL_PARAMETER_CAMERA_CONFIG_T cam_config;
+ cam_config.hdr.id = MMAL_PARAMETER_CAMERA_CONFIG;
+ cam_config.hdr.size = sizeof(cam_config);
+ cam_config.max_stills_w = state.width;
+ cam_config.max_stills_h = state.height;
+ cam_config.stills_yuv422 = 0;
+ cam_config.one_shot_stills = 0;
+ cam_config.max_preview_video_w = state.width;
+ cam_config.max_preview_video_h = state.height;
+ cam_config.num_preview_video_frames = 3;
+ cam_config.stills_capture_circular_buffer_height = 0;
+ cam_config.fast_preview_resume = 0;
+ cam_config.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC;
+
+ mmal_port_parameter_set(camera->control, &cam_config.hdr);
+ }
+
+ // Select the camera to use
+ {
+ MMAL_PARAMETER_INT32_T camera_num;
+ camera_num.hdr.id = MMAL_PARAMETER_CAMERA_NUM;
+ camera_num.hdr.size = sizeof(camera_num);
+ camera_num.value = state.camera_id;
+
+ status = mmal_port_parameter_set(camera->control, &camera_num.hdr);
+ if (status != MMAL_SUCCESS) {
+ ROS_ERROR("Could not select camera : error %d", status);
+ goto error;
+ }
+ }
+
+ // Now set up the port formats
+
+ // Set the encode format on the video port
+
+ format = video_port->format;
+ format->encoding_variant = MMAL_ENCODING_I420;
+
+ format->encoding = MMAL_ENCODING_I420;
+ format->es->video.width = state.width;
+ format->es->video.height = state.height;
+ format->es->video.crop.x = 0;
+ format->es->video.crop.y = 0;
+ format->es->video.crop.width = state.width;
+ format->es->video.crop.height = state.height;
+ format->es->video.frame_rate.num = state.framerate;
+ format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN;
+
+ status = mmal_port_format_commit(video_port);
+
+ if (status) {
+ vcos_log_error("camera video format couldn't be set");
+ ROS_ERROR("camera video format couldn't be set");
+ goto error;
+ }
+
+ // Ensure there are enough buffers to avoid dropping frames
+ if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
+ video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
+
+ // Set the encode format on the preview port
+
+ format = preview_port->format;
+ format->encoding_variant = MMAL_ENCODING_I420;
+
+ format->encoding = MMAL_ENCODING_I420;
+ format->es->video.width = state.width;
+ format->es->video.height = state.height;
+ format->es->video.crop.x = 0;
+ format->es->video.crop.y = 0;
+ format->es->video.crop.width = state.width;
+ format->es->video.crop.height = state.height;
+ format->es->video.frame_rate.num = state.framerate;
+ format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN;
+
+ status = mmal_port_format_commit(preview_port);
+
+ if (status) {
+ vcos_log_error("camera preview format couldn't be set");
+ ROS_ERROR("camera preview format couldn't be set");
+ goto error;
+ }
+
+ // Ensure there are enough buffers to avoid dropping frames
+ if (preview_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
+ preview_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
+
+ // Set the encode format on the still port
+
+ format = still_port->format;
+
+ format->encoding = MMAL_ENCODING_OPAQUE;
+ format->encoding_variant = MMAL_ENCODING_I420;
+
+ format->es->video.width = state.width;
+ format->es->video.height = state.height;
+ format->es->video.crop.x = 0;
+ format->es->video.crop.y = 0;
+ format->es->video.crop.width = state.width;
+ format->es->video.crop.height = state.height;
+ format->es->video.frame_rate.num = 1;
+ format->es->video.frame_rate.den = 1;
+
+ status = mmal_port_format_commit(still_port);
+
+ if (status) {
+ vcos_log_error("camera still format couldn't be set");
+ ROS_ERROR("camera still format couldn't be set");
+ goto error;
+ }
+
+ video_port->buffer_num = video_port->buffer_num_recommended;
+ /* Ensure there are enough buffers to avoid dropping frames */
+ if (still_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
+ still_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
+
+ /* Enable component */
+ status = mmal_component_enable(camera);
+
+ if (status) {
+ vcos_log_error("camera component couldn't be enabled");
+ ROS_ERROR("camera component couldn't be enabled");
+ goto error;
+ }
+
+ raspicamcontrol_set_all_parameters(camera, &state.camera_parameters);
+
+ state.camera_component.reset(camera);
+
+ ROS_INFO("Camera component done\n");
+
+ return camera;
+
+error:
+
+ if (camera)
+ mmal_component_destroy(camera);
+ return 0;
+}
+
+/**
+ * Create the splitter component, set up its ports
+ *
+ * @param state Pointer to state control struct
+ *
+ * @return MMAL_SUCCESS if all OK, something else otherwise
+ *
+ */
+static MMAL_STATUS_T create_splitter_component(RASPIVID_STATE& state) {
+ MMAL_COMPONENT_T* splitter = 0;
+ MMAL_PORT_T* splitter_input = nullptr;
+ MMAL_PORT_T *splitter_output_enc = nullptr, *splitter_output_raw = nullptr;
+ MMAL_STATUS_T status;
+ MMAL_POOL_T* pool;
+ MMAL_ES_FORMAT_T* format;
+
+ status = mmal_component_create(MMAL_COMPONENT_DEFAULT_VIDEO_SPLITTER, &splitter);
+
+ if (status != MMAL_SUCCESS) {
+ vcos_log_error("Unable to create image encoder component");
+ ROS_ERROR("Unable to create image encoder component");
+ goto error;
+ }
+
+ if (!splitter->input_num) {
+ status = MMAL_ENOSYS;
+ ROS_ERROR("Video splitter doesn't have input ports");
+ goto error;
+ }
+
+ if (splitter->output_num < 2) {
+ status = MMAL_ENOSYS;
+ ROS_ERROR("Video splitter doesn't have enough output ports");
+ goto error;
+ }
+
+ /*** Input Port setup ***/
+
+ splitter_input = splitter->input[0];
+
+ // We want same format on input as camera output
+ mmal_format_copy(splitter_input->format, state.camera_component->output[mmal::camera_port::video]->format);
+
+ if (splitter->input[0]->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
+ splitter->input[0]->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
+
+ // Commit the port changes to the output port
+ status = mmal_port_format_commit(splitter_input);
+
+ if (status != MMAL_SUCCESS) {
+ vcos_log_error("Unable to set format on splitter input port");
+ ROS_ERROR("Unable to set format on splitter input port");
+ goto error;
+ }
+
+ /*** Output to image encoder setup ***/
+
+ splitter_output_enc = splitter->output[0];
+
+ // Copy the format from the splitter input
+ mmal_format_copy(splitter_output_enc->format, splitter_input->format);
+
+ status = mmal_port_format_commit(splitter_output_enc);
+
+ if (status != MMAL_SUCCESS) {
+ vcos_log_error("Unable to set format on splitter output port for image encoder");
+ goto error;
+ }
+
+ /*** Output for raw ***/
+
+ splitter_output_raw = splitter->output[1];
+
+ // Copy the format from the splitter input
+ mmal_format_copy(splitter_output_raw->format, splitter_input->format);
+
+ // Use BGR24 (bgr8 in ROS)
+ format = splitter_output_raw->format;
+
+ format->encoding = MMAL_ENCODING_BGR24;
+ format->encoding_variant = 0; /* Irrelevant when not in opaque mode */
+
+ status = mmal_port_format_commit(splitter_output_raw);
+
+ if (status != MMAL_SUCCESS) {
+ vcos_log_error("Unable to set format on splitter output port for raw");
+ goto error;
+ }
+
+ /*** Setup all other output ports ***/
+
+ // start from 2
+ for (unsigned int i = 2; i < splitter->output_num; i++) {
+ mmal_format_copy(splitter->output[i]->format, splitter_input->format);
+
+ status = mmal_port_format_commit(splitter->output[i]);
+
+ if (status != MMAL_SUCCESS) {
+ vcos_log_error("Unable to set format on splitter output port %d", i);
+ goto error;
+ }
+ }
+
+ /*** Enable component ***/
+
+ status = mmal_component_enable(splitter);
+
+ if (status != MMAL_SUCCESS) {
+ vcos_log_error("Unable to enable splitter component");
+ ROS_ERROR("Unable to enable splitter component");
+ goto error;
+ }
+
+ /*** Create Pool ***/
+
+ // Create pool of buffer headers for the raw output port to consume
+ pool = mmal_port_pool_create(splitter_output_raw, splitter_output_raw->buffer_num, splitter_output_raw->buffer_size);
+
+ if (!pool) {
+ vcos_log_error("Failed to create buffer header pool for image encoder output port %s", splitter_output_raw->name);
+ ROS_ERROR("Failed to create buffer header pool for image encoder output port %s", splitter_output_raw->name);
+ }
+
+ /*** Push to state struct ***/
+
+ state.splitter_pool = mmal::pool_ptr(pool, [splitter](MMAL_POOL_T* ptr) {
+ if (splitter->output[1] && splitter->output[1]->is_enabled) {
+ mmal_port_disable(splitter->output[1]);
+ }
+ mmal_port_pool_destroy(splitter->output[1], ptr);
+ });
+
+ state.splitter_component.reset(splitter);
+
+ ROS_INFO("splitter component done\n");
+
+ return status;
+
+error:
+ if (splitter)
+ mmal_component_destroy(splitter);
+
+ return status;
+}
+
+/**
+ * Connect two specific ports together
+ *
+ * @param output_port Pointer the output port
+ * @param input_port Pointer the input port
+ * @param Pointer to a mmal connection pointer, reassigned if function
+ * successful
+ * @return Returns a MMAL_STATUS_T giving result of operation
+ *
+ */
+static MMAL_STATUS_T connect_ports(MMAL_PORT_T* output_port, MMAL_PORT_T* input_port,
+ mmal::connection_ptr& connection) {
+ MMAL_STATUS_T status;
+
+ MMAL_CONNECTION_T* new_connection = nullptr;
+
+ status = mmal_connection_create(&new_connection, output_port, input_port,
+ MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT);
+
+ if (status == MMAL_SUCCESS) {
+ status = mmal_connection_enable(new_connection);
+ if (status != MMAL_SUCCESS)
+ mmal_connection_destroy(new_connection);
+ }
+
+ connection.reset(new_connection);
+
+ return status;
+}
+
+/**
+ * init_cam
+
+ */
+int init_cam(RASPIVID_STATE& state) {
+ MMAL_STATUS_T status;
+ MMAL_PORT_T* camera_video_port = nullptr;
+ MMAL_PORT_T* splitter_input_port = nullptr;
+ MMAL_PORT_T* splitter_output_raw = nullptr;
+
+ bcm_host_init();
+ // Register our application with the logging system
+ vcos_log_register("RaspiVid", VCOS_LOG_CATEGORY);
+
+ // OK, we have a nice set of parameters. Now set up our components
+ // We have three components. Camera, splitter and encoder.
+
+ if (!create_camera_component(state)) {
+ ROS_ERROR("%s: Failed to create camera component", __func__);
+ } else if ((status = create_splitter_component(state)) != MMAL_SUCCESS) {
+ ROS_ERROR("%s: Failed to create splitter component", __func__);
+ // state.image_encoder_component.reset(nullptr);
+ // state.video_encoder_component.reset(nullptr);
+ state.camera_component.reset(nullptr);
+ } else {
+ camera_video_port = state.camera_component->output[mmal::camera_port::video];
+ splitter_input_port = state.splitter_component->input[0];
+
+ status = connect_ports(camera_video_port, splitter_input_port, state.splitter_connection);
+ if (status != MMAL_SUCCESS) {
+ ROS_ERROR("%s: Failed to connect camera video port to splitter input", __func__);
+ return 1;
+ }
+ splitter_output_raw = state.splitter_component->output[1];
+
+ PORT_USERDATA* callback_data_raw = new PORT_USERDATA(state);
+ callback_data_raw->buffer[0] = std::make_unique(IMG_BUFFER_SIZE);
+ callback_data_raw->buffer[1] = std::make_unique(IMG_BUFFER_SIZE);
+ // Set up our userdata - this is passed though to the callback where we
+ // need the information.
+ callback_data_raw->abort = false;
+ callback_data_raw->id = 0;
+ callback_data_raw->frame = 0;
+ splitter_output_raw->userdata = callback_data_raw;
+ // Enable the splitter output port and tell it its callback function
+ status = mmal_port_enable(splitter_output_raw, splitter_buffer_callback);
+ if (status != MMAL_SUCCESS) {
+ ROS_ERROR("Failed to setup splitter output");
+ return 1;
+ }
+ state.isInit = true;
+ }
+ return 0;
+}
+
+int start_capture(RASPIVID_STATE& state) {
+ if (!(state.isInit))
+ ROS_FATAL("Tried to start capture before camera is inited");
+
+ MMAL_PORT_T* camera_video_port = state.camera_component->output[mmal::camera_port::video];
+ MMAL_PORT_T* splitter_output_raw = state.splitter_component->output[1];
+ // ROS_INFO("Starting video capture (%d, %d, %d, %d)\n", state.width, state.height/2, state.quality, state.framerate); // FIXME:
+ ROS_INFO("Starting video capture (width: %d, height: %d, quality: %d, framerate: %d)\n", state.width, state.height/2, state.quality, state.framerate);
+ if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS) {
+ return 1;
+ }
+ int num = mmal_queue_length(state.splitter_pool->queue);
+ int q;
+ for (q = 0; q < num; q++) {
+ MMAL_BUFFER_HEADER_T* buffer = mmal_queue_get(state.splitter_pool->queue);
+
+ if (!buffer) {
+ vcos_log_error("Unable to get a required buffer %d from pool queue", q);
+ ROS_ERROR("Unable to get a required buffer %d from pool queue", q);
+ }
+
+ if (mmal_port_send_buffer(splitter_output_raw, buffer) != MMAL_SUCCESS) {
+ vcos_log_error("Unable to send a buffer to splitter output port (%d)", q);
+ ROS_ERROR("Unable to send a buffer to splitter output port (%d)", q);
+ }
+ }
+ ROS_INFO("Video capture started\n");
+ return 0;
+}
+
+void reconfigure_callback(raspicam_node::CameraConfig& config, uint32_t level, RASPIVID_STATE& state) {
+ ROS_INFO("figure Request: contrast %d, sharpness %d, brightness %d, "
+ "saturation %d, ISO %d, exposureCompensation %d,"
+ " videoStabilisation %d, vFlip %d, hFlip %d,"
+ " zoom %.2f, exposure_mode %s, awb_mode %s, shutter_speed %d",
+ config.contrast, config.sharpness, config.brightness, config.saturation, config.ISO,
+ config.exposure_compensation, config.video_stabilisation, config.vFlip, config.hFlip, config.zoom,
+ config.exposure_mode.c_str(), config.awb_mode.c_str(), config.shutter_speed);
+
+ if (!state.camera_component.get()) {
+ ROS_WARN("reconfiguring, but camera_component not initialized");
+ return;
+ }
+
+ if (config.zoom < 1.0) {
+ ROS_ERROR("Zoom value %f too small (must be at least 1.0)", config.zoom);
+ } else {
+ const double size = 1.0 / config.zoom;
+ const double offset = (1.0 - size) / 2.0;
+ PARAM_FLOAT_RECT_T roi;
+ roi.x = roi.y = offset;
+ roi.w = roi.h = size;
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_ROI(state.camera_component.get(), roi), "Failed to set ROI")
+ }
+
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_exposure_mode(state.camera_component.get(), exposure_mode_from_string(config.exposure_mode.c_str())), "Failed to set exposure mode")
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_awb_mode(state.camera_component.get(), awb_mode_from_string(config.awb_mode.c_str())), "Failed to set AWB mode")
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_contrast(state.camera_component.get() , config.contrast), "Failed to set contrast")
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_sharpness(state.camera_component.get(), config.sharpness), "Failed to set sharpness")
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_brightness(state.camera_component.get(), config.brightness), "Failed to set brightness")
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_saturation(state.camera_component.get(), config.saturation), "Failed to set saturation")
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_ISO(state.camera_component.get(), config.ISO) , "Failed to set ISO")
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_exposure_compensation(state.camera_component.get(), config.exposure_compensation) , "Failed to set exposure compensation")
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_video_stabilisation(state.camera_component.get(), config.video_stabilisation), "Failed to set video stabilisation")
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_flips(state.camera_component.get(), config.hFlip, config.vFlip), "Failed to set flips")
+ IF_FALSE_SHUTDOWN(raspicamcontrol_set_shutter_speed(state.camera_component.get(), config.shutter_speed), "Failed to set shutter speed")
+
+
+ ROS_INFO("Reconfigure done");
+}
+
+int main(int argc, char** argv) {
+#ifdef _DEBUG
+ DBG_MSG("//DEBUG// - Init rosnode!!!")
+#endif // _DEBUG
+ ros::init(argc, argv, "stereo_node");
+ ros::NodeHandle nh_params("~");
+#ifdef _DEBUG
+ DBG_MSG("//DEBUG// - Read parameters!!!")
+#endif // _DEBUG
+ bool private_topics;
+ nh_params.param("private_topics", private_topics, true);
+
+ // The node handle used for topics will be private or public depending on the value of the ~private_topics parameter
+ ros::NodeHandle nh_topics(private_topics ? std::string("~") : std::string());
+
+ nh_params.param("skip_frames", skip_frames, 0);
+
+ std::string left_camera_info_url, right_camera_info_url;
+
+
+ nh_params.param("left_camera_info_url", left_camera_info_url, std::string("package://raspicam_node/camera_info/left_camera.yaml"));
+ nh_params.param("right_camera_info_url", right_camera_info_url, std::string("package://raspicam_node/camera_info/right_camera.yaml"));
+ nh_params.param("left_camera_name", left_camera_name, std::string("left_camera"));
+ nh_params.param("right_camera_name", right_camera_name, std::string("right_camera"));
+
+ camera_info_manager::CameraInfoManager left_info_man(nh_params, left_camera_name, left_camera_info_url);
+ camera_info_manager::CameraInfoManager right_info_man(nh_params, right_camera_name, right_camera_info_url);
+
+ RASPIVID_STATE state_srv;
+#ifdef _DEBUG
+ DBG_MSG("//DEBUG// - configure_parameters!!!")
+#endif // _DEBUG
+ configure_parameters(state_srv, nh_params);
+#ifdef _DEBUG
+ DBG_MSG("//DEBUG// - parameters were set!!!")
+ DBG_MSG("//DEBUG// - Init camera!!!")
+#endif // _DEBUG
+ init_cam(state_srv);
+#ifdef _DEBUG
+ DBG_MSG("//DEBUG// - Done!!!")
+ DBG_MSG("//DEBUG// - Load left camera calibration!!!")
+#endif // _DEBUG
+ if(!left_info_man.loadCameraInfo(left_camera_info_url)) {
+ ROS_INFO("Calibration file missing. Left camera not calibrated");
+ } else {
+ left_info = left_info_man.getCameraInfo();
+ ROS_INFO("Camera successfully calibrated from config file");
+ }
+#ifdef _DEBUG
+ DBG_MSG("//DEBUG// - Load right camera calibration!!!")
+#endif // _DEBUG
+ if (!right_info_man.loadCameraInfo(right_camera_info_url)) {
+ ROS_INFO("Calibration file missing. Right camera not calibrated");
+ } else {
+ right_info = right_info_man.getCameraInfo();
+ ROS_INFO("Camera successfully calibrated from config file");
+ }
+
+#ifdef _DEBUG
+ DBG_MSG("setHardwareID!!!")
+#endif // _DEBUG
+
+ // diagnostics parameters
+ state_srv.updater.setHardwareID("raspicam");
+ double desired_freq = state_srv.framerate;
+ double min_freq = desired_freq * 0.95;
+ double max_freq = desired_freq * 1.05;
+#ifdef _DEBUG
+ DBG_MSG("//DEBUG// - Advertizing!!!")
+#endif // _DEBUG
+ auto left_image_pub = nh_topics.advertise("left/image_raw", 2);
+ left_image.pub.reset(new DiagnosedPublisher(
+ left_image_pub, state_srv.updater, FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10), TimeStampStatusParam(0, 0.2))); // if (state_srv.enable_imv_pub) {
+ auto right_image_pub = nh_topics.advertise("right/image_raw", 2);
+ right_image.pub.reset(new DiagnosedPublisher(
+ right_image_pub, state_srv.updater, FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10), TimeStampStatusParam(0, 0.2)));
+ if(left_image.pub == nullptr || right_image.pub == nullptr)
+ {
+ ROS_INFO("Publisher is corrupted");
+ return 0;
+ }
+ left_camera_info_pub = nh_topics.advertise("left/camera_info", 2);
+ right_camera_info_pub = nh_topics.advertise("right/camera_info", 2);
+
+ dynamic_reconfigure::Server server;
+ dynamic_reconfigure::Server::CallbackType f;
+ f = boost::bind(&reconfigure_callback, _1, _2, boost::ref(state_srv));
+#ifdef _DEBUG
+ DBG_MSG("//DEBUG// - Setting up Dynamic reconfigure!!!")
+#endif // _DEBUG
+ server.setCallback(f);
+#ifdef _DEBUG
+ DBG_MSG("//DEBUG// - start_capture!!!")
+#endif // _DEBUG
+ start_capture(state_srv);
+ ros::MultiThreadedSpinner spinner(4); // Use 4 threads
+ spinner.spin(); // spin() will not return until the node has been shutdown
+ close_cam(state_srv);
+ ros::shutdown();
+}
+
+// #endif // __arm__