├── include ├── RaspiCLI.h └── RaspiCamControl.h ├── package.xml ├── README.md ├── src ├── RaspiCLI.cpp ├── raspicam_node.cpp ├── raspicam_raw_node.cpp └── RaspiCamControl.cpp └── CMakeLists.txt /include/RaspiCLI.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2013, Broadcom Europe Ltd 3 | Copyright (c) 2013, James Hughes 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the copyright holder nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #ifndef RASPICLI_H_ 30 | #define RASPICLI_H_ 31 | 32 | typedef struct 33 | { 34 | int id; 35 | char *command; 36 | char *abbrev; 37 | char *help; 38 | int num_parameters; 39 | } COMMAND_LIST; 40 | 41 | 42 | void raspicli_display_help(const COMMAND_LIST *commands, const int num_commands); 43 | int raspicli_get_command_id(const COMMAND_LIST *commands, const int num_commands, const char *arg, int *num_parameters); 44 | 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | raspicam 4 | 0.0.0 5 | The raspicam package 6 | 7 | 8 | 9 | 10 | pi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | compressed_image_transport 44 | roscpp 45 | std_msgs 46 | compressed_image_transport 47 | roscpp 48 | std_msgs 49 | camera_info_manager 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | raspicam_node 2 | ============= 3 | 4 | Groovy ROS node for camera module of Raspberry Pi 5 | 6 | Now works at 90 fps thanks to the new firmware provided by the Raspberry Pi foundation 7 | 8 | 9 | 10 | Requirements 11 | 12 | - working ROS core and network 13 | 14 | - a Raspberry Pi 15 | 16 | - a camera module 17 | 18 | 19 | 20 | 21 | 22 | Get Raspbian http://elinux.org/RPi_Easy_SD_Card_Setup 23 | 24 | 25 | 26 | 27 | 28 | Enable Camera and expand FS http://www.raspberrypi.org/archives/3890 29 | 30 | 31 | sudo apt-get update 32 | 33 | sudo apt-get upgrade 34 | 35 | 36 | Get ROS Groovy from http://www.ros.org/wiki/groovy/Installation/Raspbian 37 | 38 | 39 | 40 | sudo apt-get install ros-groovy-image-transport ros-groovy-image-transport-plugins ros-groovy-image-transport-plugins ros-groovy-camera-info-manager 41 | 42 | 43 | 44 | 45 | git clone https://github.com/raspberrypi/userland.git /home/pi/userland 46 | 47 | cd /home/pi 48 | 49 | mkdir catkin_ws 50 | 51 | 52 | 53 | source /opt/ros/groovy/setup.bash 54 | 55 | export ROS_WORKSPACE=/home/pi/catkin_ws 56 | 57 | 58 | 59 | cd /home/pi/catkin_ws 60 | 61 | mkdir src 62 | 63 | cd src 64 | 65 | git clone https://github.com/fpasteau/raspicam_node.git raspicam 66 | 67 | cd .. 68 | 69 | catkin_make 70 | 71 | source devel/setup.bash 72 | 73 | then you can run the node using 74 | 75 | rosrun raspicam raspicam_node 76 | 77 | 78 | 79 | Topic: 80 | 81 | /camera/image/compressed (for raspicam_node): 82 | 83 | publish sensor_msgs/CompressedImage 84 | 85 | jpeg from the camera module 86 | 87 | /camera/image (for raspicam_raw_node): 88 | 89 | publish sensor_msgs/Image 90 | 91 | image in bgra8 from the camera module 92 | 93 | camera/camera_info : 94 | 95 | publish sensor_msgs/CameraInfo 96 | 97 | camera info for each frame 98 | 99 | 100 | 101 | Services : 102 | 103 | /camera/start_capture : 104 | 105 | start video capture and publication 106 | 107 | 108 | 109 | /camera/stop_capture : 110 | 111 | stop video capture and publication (buggy at the moment) 112 | 113 | /set_camera_info : 114 | 115 | set camera information (used for calibration) 116 | 117 | saved in package://raspicam/calibrations/camera.yaml 118 | 119 | 120 | Parameters : 121 | 122 | width : 123 | 124 | width of the captured images (0 < width <= 1920) 125 | 126 | height : 127 | 128 | height of the captured images (0 < width <= 1080) 129 | 130 | framerate : 131 | 132 | framerate of the captured images (0 < framerate <= 90) 133 | 134 | quality : 135 | 136 | quality of the captured images (0 < quality <= 100) 137 | 138 | tf_prefix : 139 | 140 | prefix for frame_id 141 | 142 | 143 | 144 | For parameter changes to be applied, the capture need to be restarted using /stop_capture and /start_capture services. 145 | 146 | 147 | Example : 148 | 149 | rosrun raspicam raspicam_node 150 | 151 | rosservice call /camera/start_capture 152 | 153 | rosrun image_view image_view image:=/camera/image _image_transport:=compressed 154 | 155 | If you want to try 90 fps mode, you'll have to decrease the quality factor. 156 | 157 | To try the 90 fps mode : 158 | 159 | rosrun raspicam raspicam_node _framerate:=90 _quality:=10 160 | 161 | rosservice call /camera/start_capture 162 | 163 | rosrun image_view image_view image:=/camera/image _image_transport:=compressed 164 | 165 | 166 | TO DO List : 167 | 168 | - remove warnings from raspicamcontrol 169 | 170 | - reenable vc_gencmd 171 | 172 | - check raspicam_raw_node for bugs 173 | 174 | 175 | 176 | -------------------------------------------------------------------------------- /src/RaspiCLI.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2013, Broadcom Europe Ltd 3 | Copyright (c) 2013, James Hughes 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the copyright holder nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | /** 30 | * \file RaspiCLI.c 31 | * Code for handling command line parameters 32 | * 33 | * \date 4th March 2013 34 | * \Author: James Hughes 35 | * 36 | * Description 37 | * 38 | * Some functions/structures for command line parameter parsing 39 | * 40 | */ 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include "interface/vcos/vcos.h" 47 | 48 | #include "RaspiCLI.h" 49 | 50 | 51 | /** 52 | * Convert a string from command line to a comand_id from the list 53 | * 54 | * @param commands Array of command to check 55 | * @param num_command Number of commands in the array 56 | * @param arg String to search for in the list 57 | * @param num_parameters Returns the number of parameters used by the command 58 | * 59 | * @return command ID if found, -1 if not found 60 | * 61 | */ 62 | int raspicli_get_command_id(const COMMAND_LIST *commands, const int num_commands, const char *arg, int *num_parameters) 63 | { 64 | int command_id = -1; 65 | int j; 66 | 67 | vcos_assert(commands); 68 | vcos_assert(num_parameters); 69 | vcos_assert(arg); 70 | 71 | if (!commands || !num_parameters || !arg) 72 | return -1; 73 | 74 | for (j = 0; j < num_commands; j++) 75 | { 76 | if (!strcmp(arg, commands[j].command) || 77 | !strcmp(arg, commands[j].abbrev)) 78 | { 79 | // match 80 | command_id = commands[j].id; 81 | *num_parameters = commands[j].num_parameters; 82 | break; 83 | } 84 | } 85 | 86 | return command_id; 87 | } 88 | 89 | 90 | /** 91 | * Display the list of commands in help format 92 | * 93 | * @param commands Array of command to check 94 | * @param num_command Number of commands in the arry 95 | * 96 | * 97 | */ 98 | void raspicli_display_help(const COMMAND_LIST *commands, const int num_commands) 99 | { 100 | int i; 101 | 102 | vcos_assert(commands); 103 | 104 | if (!commands) 105 | return; 106 | 107 | for (i = 0; i < num_commands; i++) 108 | { 109 | fprintf(stderr, "-%s, -%s\t: %s\n", commands[i].abbrev, 110 | commands[i].command, commands[i].help); 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(raspicam) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS compressed_image_transport roscpp std_msgs camera_info_manager) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ####################################### 19 | ## Declare ROS messages and services ## 20 | ####################################### 21 | 22 | ## Generate messages in the 'msg' folder 23 | # add_message_files( 24 | # FILES 25 | # Message1.msg 26 | # Message2.msg 27 | # ) 28 | 29 | ## Generate services in the 'srv' folder 30 | # add_service_files( 31 | # FILES 32 | # Service1.srv 33 | # Service2.srv 34 | # ) 35 | 36 | ## Generate added messages and services with any dependencies listed here 37 | # generate_messages( 38 | # DEPENDENCIES 39 | # std_msgs 40 | # ) 41 | 42 | ################################### 43 | ## catkin specific configuration ## 44 | ################################### 45 | ## The catkin_package macro generates cmake config files for your package 46 | ## Declare things to be passed to dependent projects 47 | ## LIBRARIES: libraries you create in this project that dependent projects also need 48 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 49 | ## DEPENDS: system dependencies of this project that dependent projects also need 50 | catkin_package( 51 | INCLUDE_DIRS include 52 | # LIBRARIES raspicam 53 | # CATKIN_DEPENDS compressed_image_transport roscpp std_msgs 54 | # DEPENDS system_lib 55 | ) 56 | 57 | ########### 58 | ## Build ## 59 | ########### 60 | 61 | ## Specify additional locations of header files 62 | ## Your package locations should be listed before other locations 63 | include_directories(include 64 | ${catkin_INCLUDE_DIRS} 65 | /home/pi/userland 66 | /opt/vc/include 67 | /opt/vc/include/interface/vcos/pthreads 68 | /opt/vc/include/interface/vmcs_host/linux 69 | ) 70 | 71 | ## Declare a cpp library 72 | add_library(raspicli STATIC 73 | src/RaspiCLI.cpp 74 | ) 75 | add_library(raspicamcontrol STATIC 76 | src/RaspiCamControl.cpp 77 | ) 78 | 79 | ## Declare a cpp executable 80 | add_executable(raspicam_node src/raspicam_node.cpp) 81 | add_executable(raspicam_raw_node src/raspicam_raw_node.cpp) 82 | 83 | ## Add cmake target dependencies of the executable/library 84 | ## as an example, message headers may need to be generated before nodes 85 | # add_dependencies(raspicam_node raspicam_generate_messages_cpp) 86 | 87 | ## Specify libraries to link a library or executable target against 88 | target_link_libraries(raspicam_node 89 | ${catkin_LIBRARIES} 90 | raspicamcontrol raspicli 91 | /opt/vc/lib/libbcm_host.so 92 | /opt/vc/lib/libvcos.so 93 | /opt/vc/lib/libmmal.so 94 | /opt/vc/lib/libmmal_core.so 95 | /opt/vc/lib/libmmal_util.so 96 | /opt/vc/lib/libmmal_vc_client.so 97 | /opt/vc/lib/libvchostif.a 98 | ) 99 | 100 | target_link_libraries(raspicam_raw_node 101 | ${catkin_LIBRARIES} 102 | raspicamcontrol raspicli 103 | /opt/vc/lib/libbcm_host.so 104 | /opt/vc/lib/libvcos.so 105 | /opt/vc/lib/libmmal.so 106 | /opt/vc/lib/libmmal_core.so 107 | /opt/vc/lib/libmmal_util.so 108 | /opt/vc/lib/libmmal_vc_client.so 109 | /opt/vc/lib/libvchostif.a 110 | ) 111 | 112 | ############# 113 | ## Install ## 114 | ############# 115 | 116 | # all install targets should use catkin DESTINATION variables 117 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 118 | 119 | ## Mark executable scripts (Python etc.) for installation 120 | ## in contrast to setup.py, you can choose the destination 121 | # install(PROGRAMS 122 | # scripts/my_python_script 123 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 124 | # ) 125 | 126 | ## Mark executables and/or libraries for installation 127 | # install(TARGETS raspicam raspicam_node 128 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 129 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 130 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 131 | # ) 132 | 133 | ## Mark cpp header files for installation 134 | # install(DIRECTORY include/${PROJECT_NAME}/ 135 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 136 | # FILES_MATCHING PATTERN "*.h" 137 | # PATTERN ".svn" EXCLUDE 138 | # ) 139 | 140 | ## Mark other files for installation (e.g. launch and bag files, etc.) 141 | # install(FILES 142 | # # myfile1 143 | # # myfile2 144 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 145 | # ) 146 | 147 | ############# 148 | ## Testing ## 149 | ############# 150 | 151 | ## Add gtest based cpp test target and link libraries 152 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_raspicam.cpp) 153 | # if(TARGET ${PROJECT_NAME}-test) 154 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 155 | # endif() 156 | 157 | ## Add folders to be run by python nosetests 158 | # catkin_add_nosetests(test) 159 | -------------------------------------------------------------------------------- /include/RaspiCamControl.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2013, Broadcom Europe Ltd 3 | Copyright (c) 2013, James Hughes 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the copyright holder nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #ifndef RASPICAMCONTROL_H_ 30 | #define RASPICAMCONTROL_H_ 31 | 32 | /* Various parameters 33 | * 34 | * Exposure Mode 35 | * MMAL_PARAM_EXPOSUREMODE_OFF, 36 | MMAL_PARAM_EXPOSUREMODE_AUTO, 37 | MMAL_PARAM_EXPOSUREMODE_NIGHT, 38 | MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW, 39 | MMAL_PARAM_EXPOSUREMODE_BACKLIGHT, 40 | MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT, 41 | MMAL_PARAM_EXPOSUREMODE_SPORTS, 42 | MMAL_PARAM_EXPOSUREMODE_SNOW, 43 | MMAL_PARAM_EXPOSUREMODE_BEACH, 44 | MMAL_PARAM_EXPOSUREMODE_VERYLONG, 45 | MMAL_PARAM_EXPOSUREMODE_FIXEDFPS, 46 | MMAL_PARAM_EXPOSUREMODE_ANTISHAKE, 47 | MMAL_PARAM_EXPOSUREMODE_FIREWORKS, 48 | * 49 | * AWB Mode 50 | * MMAL_PARAM_AWBMODE_OFF, 51 | MMAL_PARAM_AWBMODE_AUTO, 52 | MMAL_PARAM_AWBMODE_SUNLIGHT, 53 | MMAL_PARAM_AWBMODE_CLOUDY, 54 | MMAL_PARAM_AWBMODE_SHADE, 55 | MMAL_PARAM_AWBMODE_TUNGSTEN, 56 | MMAL_PARAM_AWBMODE_FLUORESCENT, 57 | MMAL_PARAM_AWBMODE_INCANDESCENT, 58 | MMAL_PARAM_AWBMODE_FLASH, 59 | MMAL_PARAM_AWBMODE_HORIZON, 60 | * 61 | * Image FX 62 | MMAL_PARAM_IMAGEFX_NONE, 63 | MMAL_PARAM_IMAGEFX_NEGATIVE, 64 | MMAL_PARAM_IMAGEFX_SOLARIZE, 65 | MMAL_PARAM_IMAGEFX_POSTERIZE, 66 | MMAL_PARAM_IMAGEFX_WHITEBOARD, 67 | MMAL_PARAM_IMAGEFX_BLACKBOARD, 68 | MMAL_PARAM_IMAGEFX_SKETCH, 69 | MMAL_PARAM_IMAGEFX_DENOISE, 70 | MMAL_PARAM_IMAGEFX_EMBOSS, 71 | MMAL_PARAM_IMAGEFX_OILPAINT, 72 | MMAL_PARAM_IMAGEFX_HATCH, 73 | MMAL_PARAM_IMAGEFX_GPEN, 74 | MMAL_PARAM_IMAGEFX_PASTEL, 75 | MMAL_PARAM_IMAGEFX_WATERCOLOUR, 76 | MMAL_PARAM_IMAGEFX_FILM, 77 | MMAL_PARAM_IMAGEFX_BLUR, 78 | MMAL_PARAM_IMAGEFX_SATURATION, 79 | MMAL_PARAM_IMAGEFX_COLOURSWAP, 80 | MMAL_PARAM_IMAGEFX_WASHEDOUT, 81 | MMAL_PARAM_IMAGEFX_POSTERISE, 82 | MMAL_PARAM_IMAGEFX_COLOURPOINT, 83 | MMAL_PARAM_IMAGEFX_COLOURBALANCE, 84 | MMAL_PARAM_IMAGEFX_CARTOON, 85 | 86 | */ 87 | 88 | 89 | 90 | // There isn't actually a MMAL structure for the following, so make one 91 | typedef struct 92 | { 93 | int enable; /// Turn colourFX on or off 94 | int u,v; /// U and V to use 95 | } MMAL_PARAM_COLOURFX_T; 96 | 97 | typedef struct 98 | { 99 | int enable; 100 | int width,height; 101 | int quality; 102 | } MMAL_PARAM_THUMBNAIL_CONFIG_T; 103 | 104 | typedef struct 105 | { 106 | double x; 107 | double y; 108 | double w; 109 | double h; 110 | } PARAM_FLOAT_RECT_T; 111 | 112 | /// struct contain camera settings 113 | typedef struct 114 | { 115 | int sharpness; /// -100 to 100 116 | int contrast; /// -100 to 100 117 | int brightness; /// 0 to 100 118 | int saturation; /// -100 to 100 119 | int ISO; /// TODO : what range? 120 | int videoStabilisation; /// 0 or 1 (false or true) 121 | int exposureCompensation; /// -10 to +10 ? 122 | MMAL_PARAM_EXPOSUREMODE_T exposureMode; 123 | MMAL_PARAM_EXPOSUREMETERINGMODE_T exposureMeterMode; 124 | MMAL_PARAM_AWBMODE_T awbMode; 125 | MMAL_PARAM_IMAGEFX_T imageEffect; 126 | MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imageEffectsParameters; 127 | MMAL_PARAM_COLOURFX_T colourEffects; 128 | int rotation; /// 0-359 129 | int hflip; /// 0 or 1 130 | int vflip; /// 0 or 1 131 | PARAM_FLOAT_RECT_T roi; /// region of interest to use on the sensor. Normalised [0,1] values in the rect 132 | } RASPICAM_CAMERA_PARAMETERS; 133 | 134 | 135 | void raspicamcontrol_check_configuration(int min_gpu_mem); 136 | 137 | int raspicamcontrol_parse_cmdline(RASPICAM_CAMERA_PARAMETERS *params, const char *arg1, const char *arg2); 138 | void raspicamcontrol_display_help(); 139 | int raspicamcontrol_cycle_test(MMAL_COMPONENT_T *camera); 140 | 141 | int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T *camera, const RASPICAM_CAMERA_PARAMETERS *params); 142 | int raspicamcontrol_get_all_parameters(MMAL_COMPONENT_T *camera, RASPICAM_CAMERA_PARAMETERS *params); 143 | void raspicamcontrol_dump_parameters(const RASPICAM_CAMERA_PARAMETERS *params); 144 | 145 | void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS *params); 146 | 147 | void raspicamcontrol_check_configuration(int min_gpu_mem); 148 | 149 | // Individual setting functions 150 | int raspicamcontrol_set_saturation(MMAL_COMPONENT_T *camera, int saturation); 151 | int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T *camera, int sharpness); 152 | int raspicamcontrol_set_contrast(MMAL_COMPONENT_T *camera, int contrast); 153 | int raspicamcontrol_set_brightness(MMAL_COMPONENT_T *camera, int brightness); 154 | int raspicamcontrol_set_ISO(MMAL_COMPONENT_T *camera, int ISO); 155 | int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T mode); 156 | int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T *camera, int vstabilisation); 157 | int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T *camera, int exp_comp); 158 | int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMODE_T mode); 159 | int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_AWBMODE_T awb_mode); 160 | int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T *camera, MMAL_PARAM_IMAGEFX_T imageFX); 161 | int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T *camera, const MMAL_PARAM_COLOURFX_T *colourFX); 162 | int raspicamcontrol_set_rotation(MMAL_COMPONENT_T *camera, int rotation); 163 | int raspicamcontrol_set_flips(MMAL_COMPONENT_T *camera, int hflip, int vflip); 164 | int raspicamcontrol_set_ROI(MMAL_COMPONENT_T *camera, PARAM_FLOAT_RECT_T rect); 165 | 166 | //Individual getting functions 167 | int raspicamcontrol_get_saturation(MMAL_COMPONENT_T *camera); 168 | int raspicamcontrol_get_sharpness(MMAL_COMPONENT_T *camera); 169 | int raspicamcontrol_get_contrast(MMAL_COMPONENT_T *camera); 170 | int raspicamcontrol_get_brightness(MMAL_COMPONENT_T *camera); 171 | int raspicamcontrol_get_ISO(MMAL_COMPONENT_T *camera); 172 | MMAL_PARAM_EXPOSUREMETERINGMODE_T raspicamcontrol_get_metering_mode(MMAL_COMPONENT_T *camera); 173 | int raspicamcontrol_get_video_stabilisation(MMAL_COMPONENT_T *camera); 174 | int raspicamcontrol_get_exposure_compensation(MMAL_COMPONENT_T *camera); 175 | MMAL_PARAM_THUMBNAIL_CONFIG_T raspicamcontrol_get_thumbnail_parameters(MMAL_COMPONENT_T *camera); 176 | MMAL_PARAM_EXPOSUREMODE_T raspicamcontrol_get_exposure_mode(MMAL_COMPONENT_T *camera); 177 | MMAL_PARAM_AWBMODE_T raspicamcontrol_get_awb_mode(MMAL_COMPONENT_T *camera); 178 | MMAL_PARAM_IMAGEFX_T raspicamcontrol_get_imageFX(MMAL_COMPONENT_T *camera); 179 | MMAL_PARAM_COLOURFX_T raspicamcontrol_get_colourFX(MMAL_COMPONENT_T *camera); 180 | 181 | 182 | #endif /* RASPICAMCONTROL_H_ */ 183 | -------------------------------------------------------------------------------- /src/raspicam_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2013, Broadcom Europe Ltd 3 | Copyright (c) 2013, James Hughes 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the copyright holder nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | /** 30 | * \file RaspiVid.c 31 | * Command line program to capture a camera video stream and encode it to file. 32 | * Also optionally display a preview/viewfinder of current camera input. 33 | * 34 | * \date 28th Feb 2013 35 | * \Author: James Hughes 36 | * 37 | * Description 38 | * 39 | * 3 components are created; camera, preview and video encoder. 40 | * Camera component has three ports, preview, video and stills. 41 | * This program connects preview and stills to the preview and video 42 | * encoder. Using mmal we don't need to worry about buffers between these 43 | * components, but we do need to handle buffers from the encoder, which 44 | * are simply written straight to the file in the requisite buffer callback. 45 | * 46 | * We use the RaspiCamControl code to handle the specific camera settings. 47 | * We use the RaspiPreview code to handle the (generic) preview window 48 | */ 49 | 50 | // We use some GNU extensions (basename) 51 | #include 52 | #include 53 | #include 54 | #include 55 | #define VCOS_ALWAYS_WANT_LOGGING 56 | 57 | #define VERSION_STRING "v1.2" 58 | 59 | #include "bcm_host.h" 60 | #include "interface/vcos/vcos.h" 61 | 62 | #include "interface/mmal/mmal.h" 63 | #include "interface/mmal/mmal_logging.h" 64 | #include "interface/mmal/mmal_buffer.h" 65 | #include "interface/mmal/util/mmal_util.h" 66 | #include "interface/mmal/util/mmal_util_params.h" 67 | #include "interface/mmal/util/mmal_default_components.h" 68 | #include "interface/mmal/util/mmal_connection.h" 69 | 70 | #include "ros/ros.h" 71 | #include "sensor_msgs/CompressedImage.h" 72 | #include "std_srvs/Empty.h" 73 | #include "sensor_msgs/CameraInfo.h" 74 | #include "sensor_msgs/SetCameraInfo.h" 75 | #include "camera_info_manager/camera_info_manager.h" 76 | 77 | #include "RaspiCamControl.h" 78 | #include "RaspiCLI.h" 79 | 80 | 81 | #include 82 | 83 | /// Camera number to use - we only have one camera, indexed from 0. 84 | #define CAMERA_NUMBER 0 85 | 86 | // Standard port setting for the camera component 87 | #define MMAL_CAMERA_PREVIEW_PORT 0 88 | #define MMAL_CAMERA_VIDEO_PORT 1 89 | #define MMAL_CAMERA_CAPTURE_PORT 2 90 | 91 | // Video format information 92 | #define VIDEO_FRAME_RATE_NUM 30 93 | #define VIDEO_FRAME_RATE_DEN 1 94 | 95 | /// Video render needs at least 2 buffers. 96 | #define VIDEO_OUTPUT_BUFFERS_NUM 3 97 | 98 | 99 | /// Interval at which we check for an failure abort during capture 100 | 101 | 102 | 103 | int mmal_status_to_int(MMAL_STATUS_T status); 104 | 105 | /** Structure containing all state information for the current run 106 | */ 107 | typedef struct 108 | { 109 | int isInit; 110 | int width; /// Requested width of image 111 | int height; /// requested height of image 112 | int framerate; /// Requested frame rate (fps) 113 | int quality; 114 | RASPICAM_CAMERA_PARAMETERS camera_parameters; /// Camera setup parameters 115 | 116 | MMAL_COMPONENT_T *camera_component; /// Pointer to the camera component 117 | MMAL_COMPONENT_T *encoder_component; /// Pointer to the encoder component 118 | MMAL_CONNECTION_T *preview_connection; /// Pointer to the connection from camera to preview 119 | MMAL_CONNECTION_T *encoder_connection; /// Pointer to the connection from camera to encoder 120 | 121 | MMAL_POOL_T *video_pool; /// Pointer to the pool of buffers used by encoder output port 122 | MMAL_POOL_T *encoder_pool; /// Pointer to the pool of buffers used by encoder output port 123 | ros::Publisher *image_pub; 124 | } RASPIVID_STATE; 125 | 126 | RASPIVID_STATE state_srv; 127 | ros::Publisher image_pub; 128 | ros::Publisher camera_info_pub; 129 | sensor_msgs::CameraInfo c_info; 130 | std::string tf_prefix; 131 | 132 | /** Struct used to pass information in encoder port userdata to callback 133 | */ 134 | typedef struct 135 | { 136 | unsigned char *buffer[2]; /// File handle to write buffer data to. 137 | RASPIVID_STATE *pstate; /// pointer to our state in case required in callback 138 | int abort; /// Set to 1 in callback if an error occurs to attempt to abort the capture 139 | int frame; 140 | int id; 141 | } PORT_USERDATA; 142 | 143 | static void display_valid_parameters(char *app_name); 144 | 145 | 146 | 147 | 148 | /** 149 | * Assign a default set of parameters to the state passed in 150 | * 151 | * @param state Pointer to state structure to assign defaults to 152 | */ 153 | static void get_status(RASPIVID_STATE *state) 154 | { 155 | int temp; 156 | std::string str; 157 | if (!state) 158 | { 159 | vcos_assert(0); 160 | return; 161 | } 162 | 163 | // Default everything to zero 164 | memset(state, 0, sizeof(RASPIVID_STATE)); 165 | 166 | if (ros::param::get("~width", temp )){ 167 | if(temp > 0 && temp <= 1920) 168 | state->width = temp; 169 | else state->width = 640; 170 | }else{ 171 | state->width = 640; 172 | ros::param::set("~width", 640); 173 | } 174 | 175 | if (ros::param::get("~height", temp )){ 176 | if(temp > 0 && temp <= 1080) 177 | state->height = temp; 178 | else state->height = 480; 179 | }else{ 180 | state->height = 480; 181 | ros::param::set("~height", 480); 182 | } 183 | 184 | if (ros::param::get("~quality", temp )){ 185 | if(temp > 0 && temp <= 100) 186 | state->quality = temp; 187 | else state->quality = 80; 188 | }else{ 189 | state->quality = 80; 190 | ros::param::set("~quality", 80); 191 | } 192 | 193 | if (ros::param::get("~framerate", temp )){ 194 | if(temp > 0 && temp <= 90) 195 | state->framerate = temp; 196 | else state->framerate = 30; 197 | }else{ 198 | state->framerate = 30; 199 | ros::param::set("~framerate", 30); 200 | } 201 | 202 | if (ros::param::get("~tf_prefix", str)){ 203 | tf_prefix = str; 204 | }else{ 205 | tf_prefix = ""; 206 | ros::param::set("~tf_prefix", ""); 207 | } 208 | 209 | state->isInit = 0; 210 | 211 | // Setup preview window defaults 212 | //raspipreview_set_defaults(&state->preview_parameters); 213 | 214 | // Set up the camera_parameters to default 215 | raspicamcontrol_set_defaults(&state->camera_parameters); 216 | } 217 | 218 | 219 | 220 | 221 | 222 | 223 | /** 224 | * buffer header callback function for encoder 225 | * 226 | * Callback will dump buffer data to the specific file 227 | * 228 | * @param port Pointer to port from which callback originated 229 | * @param buffer mmal buffer header pointer 230 | */ 231 | static void encoder_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer) 232 | { 233 | MMAL_BUFFER_HEADER_T *new_buffer; 234 | int complete = 0; 235 | 236 | // We pass our file handle and other stuff in via the userdata field. 237 | 238 | PORT_USERDATA *pData = (PORT_USERDATA *)port->userdata; 239 | if (pData && pData->pstate->isInit) 240 | { 241 | int bytes_written = buffer->length; 242 | if (buffer->length) 243 | { 244 | mmal_buffer_header_mem_lock(buffer); 245 | memcpy(&(pData->buffer[pData->frame & 1][pData->id]), buffer->data, buffer->length); 246 | pData->id += bytes_written; 247 | mmal_buffer_header_mem_unlock(buffer); 248 | } 249 | 250 | if (bytes_written != buffer->length) 251 | { 252 | vcos_log_error("Failed to write buffer data (%d from %d)- aborting", bytes_written, buffer->length); 253 | pData->abort = 1; 254 | } 255 | if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED)) 256 | complete = 1; 257 | 258 | if (complete){ 259 | sensor_msgs::CompressedImage msg; 260 | msg.header.seq = pData->frame; 261 | msg.header.frame_id = tf_prefix; 262 | msg.header.frame_id.append("/camera"); 263 | msg.header.stamp = ros::Time::now(); 264 | msg.format = "jpg"; 265 | msg.data.insert( msg.data.end(), pData->buffer[pData->frame & 1], &(pData->buffer[pData->frame & 1][pData->id]) ); 266 | image_pub.publish(msg); 267 | c_info.header.seq = pData->frame; 268 | c_info.header.stamp = msg.header.stamp; 269 | c_info.header.frame_id = msg.header.frame_id; 270 | camera_info_pub.publish(c_info); 271 | pData->frame++; 272 | pData->id = 0; 273 | } 274 | } 275 | 276 | // release buffer back to the pool 277 | mmal_buffer_header_release(buffer); 278 | 279 | // and send one back to the port (if still open) 280 | if (port->is_enabled) 281 | { 282 | MMAL_STATUS_T status; 283 | 284 | new_buffer = mmal_queue_get(pData->pstate->encoder_pool->queue); 285 | 286 | if (new_buffer) 287 | status = mmal_port_send_buffer(port, new_buffer); 288 | 289 | if (!new_buffer || status != MMAL_SUCCESS) 290 | vcos_log_error("Unable to return a buffer to the encoder port"); 291 | } 292 | } 293 | 294 | 295 | 296 | /** 297 | * Create the camera component, set up its ports 298 | * 299 | * @param state Pointer to state control struct 300 | * 301 | * @return 0 if failed, pointer to component if successful 302 | * 303 | */ 304 | static MMAL_COMPONENT_T *create_camera_component(RASPIVID_STATE *state) 305 | { 306 | MMAL_COMPONENT_T *camera = 0; 307 | MMAL_ES_FORMAT_T *format; 308 | MMAL_PORT_T *preview_port = NULL, *video_port = NULL, *still_port = NULL; 309 | MMAL_STATUS_T status; 310 | 311 | /* Create the component */ 312 | status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera); 313 | 314 | if (status != MMAL_SUCCESS) 315 | { 316 | vcos_log_error("Failed to create camera component"); 317 | goto error; 318 | } 319 | 320 | if (!camera->output_num) 321 | { 322 | vcos_log_error("Camera doesn't have output ports"); 323 | goto error; 324 | } 325 | 326 | video_port = camera->output[MMAL_CAMERA_VIDEO_PORT]; 327 | still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT]; 328 | 329 | 330 | // set up the camera configuration 331 | { 332 | MMAL_PARAMETER_CAMERA_CONFIG_T cam_config; 333 | cam_config.hdr.id = MMAL_PARAMETER_CAMERA_CONFIG; 334 | cam_config.hdr.size = sizeof(cam_config); 335 | cam_config.max_stills_w = state->width; 336 | cam_config.max_stills_h = state->height; 337 | cam_config.stills_yuv422 = 0; 338 | cam_config.one_shot_stills = 0; 339 | cam_config.max_preview_video_w = state->width; 340 | cam_config.max_preview_video_h = state->height; 341 | cam_config.num_preview_video_frames = 3; 342 | cam_config.stills_capture_circular_buffer_height = 0; 343 | cam_config.fast_preview_resume = 0; 344 | cam_config.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC; 345 | 346 | mmal_port_parameter_set(camera->control, &cam_config.hdr); 347 | } 348 | 349 | // Now set up the port formats 350 | 351 | // Set the encode format on the video port 352 | 353 | format = video_port->format; 354 | format->encoding_variant = MMAL_ENCODING_I420; 355 | 356 | format->encoding = MMAL_ENCODING_I420; 357 | format->es->video.width = state->width; 358 | format->es->video.height = state->height; 359 | format->es->video.crop.x = 0; 360 | format->es->video.crop.y = 0; 361 | format->es->video.crop.width = state->width; 362 | format->es->video.crop.height = state->height; 363 | format->es->video.frame_rate.num = state->framerate; 364 | format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN; 365 | 366 | status = mmal_port_format_commit(video_port); 367 | 368 | if (status) 369 | { 370 | vcos_log_error("camera video format couldn't be set"); 371 | goto error; 372 | } 373 | 374 | // Ensure there are enough buffers to avoid dropping frames 375 | if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) 376 | video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM; 377 | 378 | // Set the encode format on the still port 379 | 380 | format = still_port->format; 381 | 382 | format->encoding = MMAL_ENCODING_OPAQUE; 383 | format->encoding_variant = MMAL_ENCODING_I420; 384 | 385 | format->es->video.width = state->width; 386 | format->es->video.height = state->height; 387 | format->es->video.crop.x = 0; 388 | format->es->video.crop.y = 0; 389 | format->es->video.crop.width = state->width; 390 | format->es->video.crop.height = state->height; 391 | format->es->video.frame_rate.num = 1; 392 | format->es->video.frame_rate.den = 1; 393 | 394 | status = mmal_port_format_commit(still_port); 395 | 396 | if (status) 397 | { 398 | vcos_log_error("camera still format couldn't be set"); 399 | goto error; 400 | } 401 | 402 | video_port->buffer_num = video_port->buffer_num_recommended; 403 | /* Ensure there are enough buffers to avoid dropping frames */ 404 | if (still_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) 405 | still_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM; 406 | 407 | /* Enable component */ 408 | status = mmal_component_enable(camera); 409 | 410 | if (status) 411 | { 412 | vcos_log_error("camera component couldn't be enabled"); 413 | goto error; 414 | } 415 | 416 | raspicamcontrol_set_all_parameters(camera, &state->camera_parameters); 417 | 418 | state->camera_component = camera; 419 | 420 | ROS_INFO("Camera component done\n"); 421 | 422 | return camera; 423 | 424 | error: 425 | 426 | if (camera) 427 | mmal_component_destroy(camera); 428 | 429 | return 0; 430 | } 431 | 432 | /** 433 | * Destroy the camera component 434 | * 435 | * @param state Pointer to state control struct 436 | * 437 | */ 438 | static void destroy_camera_component(RASPIVID_STATE *state) 439 | { 440 | if (state->camera_component) 441 | { 442 | mmal_component_destroy(state->camera_component); 443 | state->camera_component = NULL; 444 | } 445 | } 446 | 447 | /** 448 | * Create the encoder component, set up its ports 449 | * 450 | * @param state Pointer to state control struct 451 | * 452 | * @return MMAL_SUCCESS if all OK, something else otherwise 453 | * 454 | */ 455 | static MMAL_STATUS_T create_encoder_component(RASPIVID_STATE *state) 456 | { 457 | MMAL_COMPONENT_T *encoder = 0; 458 | MMAL_PORT_T *encoder_input = NULL, *encoder_output = NULL; 459 | MMAL_STATUS_T status; 460 | MMAL_POOL_T *pool; 461 | 462 | status = mmal_component_create(MMAL_COMPONENT_DEFAULT_IMAGE_ENCODER, &encoder); 463 | 464 | if (status != MMAL_SUCCESS) 465 | { 466 | vcos_log_error("Unable to create video encoder component"); 467 | goto error; 468 | } 469 | 470 | if (!encoder->input_num || !encoder->output_num) 471 | { 472 | status = MMAL_ENOSYS; 473 | vcos_log_error("Video encoder doesn't have input/output ports"); 474 | goto error; 475 | } 476 | 477 | encoder_input = encoder->input[0]; 478 | encoder_output = encoder->output[0]; 479 | 480 | // We want same format on input and output 481 | mmal_format_copy(encoder_output->format, encoder_input->format); 482 | 483 | // Only supporting H264 at the moment 484 | encoder_output->format->encoding = MMAL_ENCODING_JPEG; 485 | 486 | 487 | encoder_output->buffer_size = encoder_output->buffer_size_recommended; 488 | 489 | if (encoder_output->buffer_size < encoder_output->buffer_size_min) 490 | encoder_output->buffer_size = encoder_output->buffer_size_min; 491 | 492 | encoder_output->buffer_num = encoder_output->buffer_num_recommended; 493 | 494 | if (encoder_output->buffer_num < encoder_output->buffer_num_min) 495 | encoder_output->buffer_num = encoder_output->buffer_num_min; 496 | 497 | // Commit the port changes to the output port 498 | status = mmal_port_format_commit(encoder_output); 499 | 500 | if (status != MMAL_SUCCESS) 501 | { 502 | vcos_log_error("Unable to set format on video encoder output port"); 503 | goto error; 504 | } 505 | 506 | // Set the JPEG quality level 507 | status = mmal_port_parameter_set_uint32(encoder_output, MMAL_PARAMETER_JPEG_Q_FACTOR, state->quality); 508 | 509 | if (status != MMAL_SUCCESS) 510 | { 511 | vcos_log_error("Unable to set JPEG quality"); 512 | goto error; 513 | } 514 | 515 | 516 | // Enable component 517 | status = mmal_component_enable(encoder); 518 | 519 | if (status != MMAL_SUCCESS) 520 | { 521 | vcos_log_error("Unable to enable video encoder component"); 522 | goto error; 523 | } 524 | 525 | /* Create pool of buffer headers for the output port to consume */ 526 | pool = mmal_port_pool_create(encoder_output, encoder_output->buffer_num, encoder_output->buffer_size); 527 | 528 | if (!pool) 529 | { 530 | vcos_log_error("Failed to create buffer header pool for encoder output port %s", encoder_output->name); 531 | } 532 | 533 | state->encoder_pool = pool; 534 | state->encoder_component = encoder; 535 | 536 | ROS_INFO("Encoder component done\n"); 537 | 538 | return status; 539 | 540 | error: 541 | if (encoder) 542 | mmal_component_destroy(encoder); 543 | 544 | return status; 545 | } 546 | 547 | /** 548 | * Destroy the encoder component 549 | * 550 | * @param state Pointer to state control struct 551 | * 552 | */ 553 | static void destroy_encoder_component(RASPIVID_STATE *state) 554 | { 555 | // Get rid of any port buffers first 556 | if (state->video_pool) 557 | { 558 | mmal_port_pool_destroy(state->encoder_component->output[0], state->video_pool); 559 | } 560 | 561 | if (state->encoder_component) 562 | { 563 | mmal_component_destroy(state->encoder_component); 564 | state->encoder_component = NULL; 565 | } 566 | } 567 | 568 | /** 569 | * Connect two specific ports together 570 | * 571 | * @param output_port Pointer the output port 572 | * @param input_port Pointer the input port 573 | * @param Pointer to a mmal connection pointer, reassigned if function successful 574 | * @return Returns a MMAL_STATUS_T giving result of operation 575 | * 576 | */ 577 | static MMAL_STATUS_T connect_ports(MMAL_PORT_T *output_port, MMAL_PORT_T *input_port, MMAL_CONNECTION_T **connection) 578 | { 579 | MMAL_STATUS_T status; 580 | 581 | status = mmal_connection_create(connection, output_port, input_port, MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT); 582 | 583 | if (status == MMAL_SUCCESS) 584 | { 585 | status = mmal_connection_enable(*connection); 586 | if (status != MMAL_SUCCESS) 587 | mmal_connection_destroy(*connection); 588 | } 589 | 590 | return status; 591 | } 592 | 593 | /** 594 | * Checks if specified port is valid and enabled, then disables it 595 | * 596 | * @param port Pointer the port 597 | * 598 | */ 599 | static void check_disable_port(MMAL_PORT_T *port) 600 | { 601 | if (port && port->is_enabled) 602 | mmal_port_disable(port); 603 | } 604 | 605 | /** 606 | * Handler for sigint signals 607 | * 608 | * @param signal_number ID of incoming signal. 609 | * 610 | */ 611 | static void signal_handler(int signal_number) 612 | { 613 | // Going to abort on all signals 614 | vcos_log_error("Aborting program\n"); 615 | 616 | // TODO : Need to close any open stuff...how? 617 | 618 | exit(255); 619 | } 620 | 621 | /** 622 | * init_cam 623 | 624 | */ 625 | int init_cam(RASPIVID_STATE *state) 626 | { 627 | // Our main data storage vessel.. 628 | MMAL_STATUS_T status; 629 | MMAL_PORT_T *camera_video_port = NULL; 630 | MMAL_PORT_T *camera_still_port = NULL; 631 | MMAL_PORT_T *preview_input_port = NULL; 632 | MMAL_PORT_T *encoder_input_port = NULL; 633 | MMAL_PORT_T *encoder_output_port = NULL; 634 | 635 | bcm_host_init(); 636 | get_status(state); 637 | // Register our application with the logging system 638 | vcos_log_register("RaspiVid", VCOS_LOG_CATEGORY); 639 | 640 | signal(SIGINT, signal_handler); 641 | 642 | // OK, we have a nice set of parameters. Now set up our components 643 | // We have three components. Camera, Preview and encoder. 644 | 645 | if (!create_camera_component(state)) 646 | { 647 | ROS_INFO("%s: Failed to create camera component", __func__); 648 | } 649 | else if ((status = create_encoder_component(state)) != MMAL_SUCCESS) 650 | { 651 | ROS_INFO("%s: Failed to create encode component", __func__); 652 | destroy_camera_component(state); 653 | } 654 | else 655 | { 656 | PORT_USERDATA * callback_data_enc = (PORT_USERDATA *) malloc (sizeof(PORT_USERDATA)); 657 | camera_video_port = state->camera_component->output[MMAL_CAMERA_VIDEO_PORT]; 658 | camera_still_port = state->camera_component->output[MMAL_CAMERA_CAPTURE_PORT]; 659 | encoder_input_port = state->encoder_component->input[0]; 660 | encoder_output_port = state->encoder_component->output[0]; 661 | status = connect_ports(camera_video_port, encoder_input_port, &state->encoder_connection); 662 | if (status != MMAL_SUCCESS) 663 | { 664 | ROS_INFO("%s: Failed to connect camera video port to encoder input", __func__); 665 | return 1; 666 | } 667 | callback_data_enc->buffer[0] = (unsigned char *) malloc ( 1024 * 1024 ); 668 | callback_data_enc->buffer[1] = (unsigned char *) malloc ( 1024 * 1024 ); 669 | // Set up our userdata - this is passed though to the callback where we need the information. 670 | callback_data_enc->pstate = state; 671 | callback_data_enc->abort = 0; 672 | callback_data_enc->id = 0; 673 | callback_data_enc->frame = 0; 674 | encoder_output_port->userdata = (struct MMAL_PORT_USERDATA_T *) callback_data_enc; 675 | PORT_USERDATA *pData = (PORT_USERDATA *)encoder_output_port->userdata; 676 | // Enable the encoder output port and tell it its callback function 677 | status = mmal_port_enable(encoder_output_port, encoder_buffer_callback); 678 | if (status != MMAL_SUCCESS) 679 | { 680 | ROS_INFO("Failed to setup encoder output"); 681 | return 1; 682 | } 683 | state->isInit = 1; 684 | } 685 | return 0; 686 | } 687 | 688 | 689 | int start_capture(RASPIVID_STATE *state){ 690 | if(!(state->isInit)) init_cam(state); 691 | MMAL_PORT_T *camera_video_port = state->camera_component->output[MMAL_CAMERA_VIDEO_PORT]; 692 | MMAL_PORT_T *encoder_output_port = state->encoder_component->output[0]; 693 | ROS_INFO("Starting video capture (%d, %d, %d, %d)\n", state->width, state->height, state->quality, state->framerate); 694 | 695 | if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS) 696 | { 697 | return 1; 698 | } 699 | // Send all the buffers to the video port 700 | { 701 | int num = mmal_queue_length(state->encoder_pool->queue); 702 | int q; 703 | for (q=0;qencoder_pool->queue); 706 | 707 | if (!buffer) 708 | vcos_log_error("Unable to get a required buffer %d from pool queue", q); 709 | 710 | if (mmal_port_send_buffer(encoder_output_port, buffer)!= MMAL_SUCCESS) 711 | vcos_log_error("Unable to send a buffer to encoder output port (%d)", q); 712 | 713 | } 714 | } 715 | ROS_INFO("Video capture started\n"); 716 | return 0; 717 | 718 | } 719 | 720 | 721 | 722 | int close_cam(RASPIVID_STATE *state){ 723 | if(state->isInit){ 724 | state -> isInit = 0; 725 | MMAL_COMPONENT_T *camera = state->camera_component; 726 | MMAL_COMPONENT_T *encoder = state->encoder_component; 727 | MMAL_PORT_T *encoder_output_port = state->encoder_component->output[0]; 728 | MMAL_PORT_T *camera_still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT]; 729 | PORT_USERDATA * pData = (PORT_USERDATA *)encoder_output_port->userdata; 730 | 731 | if (camera_still_port && camera_still_port->is_enabled) 732 | mmal_port_disable(camera_still_port); 733 | 734 | if (encoder->output[0] && encoder->output[0]->is_enabled) 735 | mmal_port_disable(encoder->output[0]); 736 | 737 | mmal_connection_destroy(state->encoder_connection); 738 | 739 | // Disable components 740 | if (encoder) 741 | mmal_component_disable(encoder); 742 | 743 | if (camera) 744 | mmal_component_disable(camera); 745 | 746 | //Destroy encoder component 747 | // Get rid of any port buffers first 748 | if (state->encoder_pool) 749 | { 750 | mmal_port_pool_destroy(encoder->output[0], state->encoder_pool); 751 | } 752 | 753 | 754 | free(pData->buffer[0]); 755 | free(pData->buffer[1]); 756 | 757 | if (encoder) 758 | { 759 | mmal_component_destroy(encoder); 760 | encoder = NULL; 761 | } 762 | //destroy camera component 763 | if (camera) 764 | { 765 | mmal_component_destroy(camera); 766 | camera = NULL; 767 | } 768 | ROS_INFO("Video capture stopped\n"); 769 | return 0; 770 | }else return 1; 771 | } 772 | 773 | bool serv_start_cap( std_srvs::Empty::Request &req, 774 | std_srvs::Empty::Response &res ) 775 | { 776 | start_capture(&state_srv); 777 | return true; 778 | } 779 | 780 | 781 | bool serv_stop_cap( std_srvs::Empty::Request &req, 782 | std_srvs::Empty::Response &res ) 783 | { 784 | close_cam(&state_srv); 785 | return true; 786 | } 787 | 788 | 789 | 790 | int main(int argc, char **argv){ 791 | ros::init(argc, argv, "raspicam_node"); 792 | ros::NodeHandle n; 793 | camera_info_manager::CameraInfoManager c_info_man (n, "camera", "package://raspicam/calibrations/camera.yaml"); 794 | get_status(&state_srv); 795 | 796 | if(!c_info_man.loadCameraInfo ("package://raspicam/calibrations/camera.yaml")){ 797 | ROS_INFO("Calibration file missing. Camera not calibrated"); 798 | } 799 | else 800 | { 801 | c_info = c_info_man.getCameraInfo (); 802 | ROS_INFO("Camera successfully calibrated"); 803 | } 804 | image_pub = n.advertise("camera/image/compressed", 1); 805 | camera_info_pub = n.advertise("camera/camera_info", 1); 806 | ros::ServiceServer start_cam = n.advertiseService("camera/start_capture", serv_start_cap); 807 | ros::ServiceServer stop_cam = n.advertiseService("camera/stop_capture", serv_stop_cap); 808 | ros::spin(); 809 | close_cam(&state_srv); 810 | return 0; 811 | } 812 | -------------------------------------------------------------------------------- /src/raspicam_raw_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2013, Broadcom Europe Ltd 3 | Copyright (c) 2013, James Hughes 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the copyright holder nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | /** 30 | * \file RaspiVid.c 31 | * Command line program to capture a camera video stream and encode it to file. 32 | * Also optionally display a preview/viewfinder of current camera input. 33 | * 34 | * \date 28th Feb 2013 35 | * \Author: James Hughes 36 | * 37 | * Description 38 | * 39 | * 3 components are created; camera, preview and video encoder. 40 | * Camera component has three ports, preview, video and stills. 41 | * This program connects preview and stills to the preview and video 42 | * encoder. Using mmal we don't need to worry about buffers between these 43 | * components, but we do need to handle buffers from the encoder, which 44 | * are simply written straight to the file in the requisite buffer callback. 45 | * 46 | * We use the RaspiCamControl code to handle the specific camera settings. 47 | * We use the RaspiPreview code to handle the (generic) preview window 48 | */ 49 | 50 | // We use some GNU extensions (basename) 51 | #include 52 | #include 53 | #include 54 | #include 55 | #define VCOS_ALWAYS_WANT_LOGGING 56 | 57 | #define VERSION_STRING "v1.2" 58 | 59 | #include "bcm_host.h" 60 | #include "interface/vcos/vcos.h" 61 | 62 | #include "interface/mmal/mmal.h" 63 | #include "interface/mmal/mmal_logging.h" 64 | #include "interface/mmal/mmal_buffer.h" 65 | #include "interface/mmal/util/mmal_util.h" 66 | #include "interface/mmal/util/mmal_util_params.h" 67 | #include "interface/mmal/util/mmal_default_components.h" 68 | #include "interface/mmal/util/mmal_connection.h" 69 | 70 | #include "ros/ros.h" 71 | #include "sensor_msgs/Image.h" 72 | #include "std_srvs/Empty.h" 73 | #include "sensor_msgs/CameraInfo.h" 74 | #include "sensor_msgs/SetCameraInfo.h" 75 | #include "camera_info_manager/camera_info_manager.h" 76 | 77 | #include "RaspiCamControl.h" 78 | #include "RaspiCLI.h" 79 | 80 | 81 | #include 82 | 83 | /// Camera number to use - we only have one camera, indexed from 0. 84 | #define CAMERA_NUMBER 0 85 | 86 | // Standard port setting for the camera component 87 | #define MMAL_CAMERA_PREVIEW_PORT 0 88 | #define MMAL_CAMERA_VIDEO_PORT 1 89 | #define MMAL_CAMERA_CAPTURE_PORT 2 90 | 91 | // Video format information 92 | #define VIDEO_FRAME_RATE_NUM 30 93 | #define VIDEO_FRAME_RATE_DEN 1 94 | 95 | /// Video render needs at least 2 buffers. 96 | #define VIDEO_OUTPUT_BUFFERS_NUM 3 97 | 98 | 99 | /// Interval at which we check for an failure abort during capture 100 | 101 | 102 | 103 | int mmal_status_to_int(MMAL_STATUS_T status); 104 | 105 | /** Structure containing all state information for the current run 106 | */ 107 | typedef struct 108 | { 109 | int isInit; 110 | int width; /// Requested width of image 111 | int height; /// requested height of image 112 | int framerate; /// Requested frame rate (fps) 113 | int quality; 114 | RASPICAM_CAMERA_PARAMETERS camera_parameters; /// Camera setup parameters 115 | 116 | MMAL_COMPONENT_T *camera_component; /// Pointer to the camera component 117 | MMAL_COMPONENT_T *encoder_component; /// Pointer to the encoder component 118 | MMAL_CONNECTION_T *preview_connection; /// Pointer to the connection from camera to preview 119 | MMAL_CONNECTION_T *encoder_connection; /// Pointer to the connection from camera to encoder 120 | 121 | //MMAL_POOL_T *video_pool; /// Pointer to the pool of buffers used by encoder output port 122 | MMAL_POOL_T *camera_pool; /// Pointer to the pool of buffers used by encoder output port 123 | ros::Publisher *image_pub; 124 | } RASPIVID_STATE; 125 | 126 | RASPIVID_STATE state_srv; 127 | ros::Publisher image_pub; 128 | ros::Publisher camera_info_pub; 129 | sensor_msgs::CameraInfo c_info; 130 | std::string tf_prefix; 131 | 132 | /** Struct used to pass information in encoder port userdata to callback 133 | */ 134 | typedef struct 135 | { 136 | unsigned char *buffer[2]; /// File handle to write buffer data to. 137 | RASPIVID_STATE *pstate; /// pointer to our state in case required in callback 138 | int abort; /// Set to 1 in callback if an error occurs to attempt to abort the capture 139 | int frame; 140 | int id; 141 | } PORT_USERDATA; 142 | 143 | static void display_valid_parameters(char *app_name); 144 | 145 | 146 | 147 | 148 | /** 149 | * Assign a default set of parameters to the state passed in 150 | * 151 | * @param state Pointer to state structure to assign defaults to 152 | */ 153 | static void get_status(RASPIVID_STATE *state) 154 | { 155 | int temp; 156 | std::string str; 157 | if (!state) 158 | { 159 | vcos_assert(0); 160 | return; 161 | } 162 | 163 | // Default everything to zero 164 | memset(state, 0, sizeof(RASPIVID_STATE)); 165 | 166 | if (ros::param::get("~width", temp )){ 167 | if(temp > 0 && temp <= 1920) 168 | state->width = temp; 169 | else state->width = 640; 170 | }else{ 171 | state->width = 640; 172 | ros::param::set("~width", 640); 173 | } 174 | 175 | if (ros::param::get("~height", temp )){ 176 | if(temp > 0 && temp <= 1080) 177 | state->height = temp; 178 | else state->height = 480; 179 | }else{ 180 | state->height = 480; 181 | ros::param::set("~height", 480); 182 | } 183 | 184 | if (ros::param::get("~quality", temp )){ 185 | if(temp > 0 && temp <= 100) 186 | state->quality = temp; 187 | else state->quality = 80; 188 | }else{ 189 | state->quality = 80; 190 | ros::param::set("~quality", 80); 191 | } 192 | 193 | if (ros::param::get("~framerate", temp )){ 194 | if(temp > 0 && temp <= 90) 195 | state->framerate = temp; 196 | else state->framerate = 30; 197 | }else{ 198 | state->framerate = 30; 199 | ros::param::set("~framerate", 30); 200 | } 201 | 202 | if (ros::param::get("~tf_prefix", str)){ 203 | tf_prefix = str; 204 | }else{ 205 | tf_prefix = ""; 206 | ros::param::set("~tf_prefix", ""); 207 | } 208 | 209 | state->isInit = 0; 210 | 211 | // Setup preview window defaults 212 | //raspipreview_set_defaults(&state->preview_parameters); 213 | 214 | // Set up the camera_parameters to default 215 | raspicamcontrol_set_defaults(&state->camera_parameters); 216 | } 217 | 218 | 219 | 220 | 221 | 222 | 223 | /** 224 | * buffer header callback function for encoder 225 | * 226 | * Callback will dump buffer data to the specific file 227 | * 228 | * @param port Pointer to port from which callback originated 229 | * @param buffer mmal buffer header pointer 230 | */ 231 | static void camera_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer) 232 | { 233 | MMAL_BUFFER_HEADER_T *new_buffer; 234 | int complete = 0; 235 | 236 | // We pass our file handle and other stuff in via the userdata field. 237 | 238 | PORT_USERDATA *pData = (PORT_USERDATA *)port->userdata; 239 | if (pData && pData->pstate->isInit) 240 | { 241 | int bytes_written = buffer->length; 242 | if (buffer->length) 243 | { 244 | mmal_buffer_header_mem_lock(buffer); 245 | memcpy(&(pData->buffer[pData->frame & 1][pData->id]), buffer->data, buffer->length); 246 | pData->id += bytes_written; 247 | mmal_buffer_header_mem_unlock(buffer); 248 | } 249 | 250 | if (bytes_written != buffer->length) 251 | { 252 | vcos_log_error("Failed to write buffer data (%d from %d)- aborting", bytes_written, buffer->length); 253 | pData->abort = 1; 254 | } 255 | if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED)) 256 | complete = 1; 257 | 258 | if (complete){ 259 | sensor_msgs::Image msg; 260 | msg.header.seq = pData->frame; 261 | msg.header.frame_id = tf_prefix; 262 | msg.header.frame_id.append("/camera"); 263 | msg.header.stamp = ros::Time::now(); 264 | msg.height = pData->pstate->height; 265 | msg.width = pData->pstate->width; 266 | msg.encoding = "bgra8"; 267 | msg.is_bigendian = 0; 268 | msg.step = pData->pstate->width*4; 269 | msg.data.insert( msg.data.end(), pData->buffer[pData->frame & 1], &(pData->buffer[pData->frame & 1][pData->id]) ); 270 | image_pub.publish(msg); 271 | c_info.header.seq = pData->frame; 272 | c_info.header.stamp = msg.header.stamp; 273 | c_info.header.frame_id = msg.header.frame_id; 274 | camera_info_pub.publish(c_info); 275 | pData->frame++; 276 | pData->id = 0; 277 | } 278 | } 279 | else 280 | { 281 | vcos_log_error("Received a encoder buffer callback with no state"); 282 | } 283 | // release buffer back to the pool 284 | mmal_buffer_header_release(buffer); 285 | 286 | // and send one back to the port (if still open) 287 | if (port->is_enabled) 288 | { 289 | MMAL_STATUS_T status; 290 | 291 | new_buffer = mmal_queue_get(pData->pstate->camera_pool->queue); 292 | 293 | if (new_buffer) 294 | status = mmal_port_send_buffer(port, new_buffer); 295 | 296 | if (!new_buffer || status != MMAL_SUCCESS) 297 | vcos_log_error("Unable to return a buffer to the encoder port"); 298 | }else{ 299 | 300 | ROS_INFO("oups"); 301 | } 302 | } 303 | 304 | 305 | 306 | /** 307 | * Create the camera component, set up its ports 308 | * 309 | * @param state Pointer to state control struct 310 | * 311 | * @return 0 if failed, pointer to component if successful 312 | * 313 | */ 314 | static MMAL_COMPONENT_T *create_camera_component(RASPIVID_STATE *state) 315 | { 316 | MMAL_COMPONENT_T *camera = 0; 317 | MMAL_ES_FORMAT_T *format; 318 | MMAL_PORT_T *preview_port = NULL, *video_port = NULL, *still_port = NULL; 319 | MMAL_POOL_T *pool; 320 | MMAL_STATUS_T status; 321 | 322 | /* Create the component */ 323 | status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera); 324 | 325 | if (status != MMAL_SUCCESS) 326 | { 327 | vcos_log_error("Failed to create camera component"); 328 | goto error; 329 | } 330 | 331 | if (!camera->output_num) 332 | { 333 | vcos_log_error("Camera doesn't have output ports"); 334 | goto error; 335 | } 336 | 337 | video_port = camera->output[MMAL_CAMERA_VIDEO_PORT]; 338 | still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT]; 339 | 340 | 341 | // set up the camera configuration 342 | { 343 | MMAL_PARAMETER_CAMERA_CONFIG_T cam_config; 344 | cam_config.hdr.id = MMAL_PARAMETER_CAMERA_CONFIG; 345 | cam_config.hdr.size = sizeof(cam_config); 346 | cam_config.max_stills_w = state->width; 347 | cam_config.max_stills_h = state->height; 348 | cam_config.stills_yuv422 = 0; 349 | cam_config.one_shot_stills = 0; 350 | cam_config.max_preview_video_w = state->width; 351 | cam_config.max_preview_video_h = state->height; 352 | cam_config.num_preview_video_frames = 3; 353 | cam_config.stills_capture_circular_buffer_height = 0; 354 | cam_config.fast_preview_resume = 0; 355 | cam_config.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC; 356 | 357 | mmal_port_parameter_set(camera->control, &cam_config.hdr); 358 | } 359 | 360 | // Now set up the port formats 361 | 362 | // Set the encode format on the video port 363 | 364 | format = video_port->format; 365 | //format->encoding_variant = MMAL_ENCODING_I420; 366 | 367 | //format->encoding = MMAL_ENCODING_I420; 368 | format->encoding = MMAL_ENCODING_BGRA; 369 | format->encoding_variant = MMAL_ENCODING_BGRA; 370 | 371 | format->es->video.width = state->width; 372 | format->es->video.height = state->height; 373 | format->es->video.crop.x = 0; 374 | format->es->video.crop.y = 0; 375 | format->es->video.crop.width = state->width; 376 | format->es->video.crop.height = state->height; 377 | format->es->video.frame_rate.num = state->framerate; 378 | format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN; 379 | 380 | status = mmal_port_format_commit(video_port); 381 | 382 | if (status) 383 | { 384 | vcos_log_error("camera video format couldn't be set"); 385 | goto error; 386 | } 387 | 388 | // Ensure there are enough buffers to avoid dropping frames 389 | if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) 390 | video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM; 391 | 392 | // Set the encode format on the still port 393 | 394 | format = still_port->format; 395 | 396 | format->encoding = MMAL_ENCODING_OPAQUE; 397 | format->encoding_variant = MMAL_ENCODING_I420; 398 | //format->encoding = MMAL_ENCODING_BGR16; 399 | //format->encoding_variant = MMAL_ENCODING_BGR16; 400 | 401 | format->es->video.width = state->width; 402 | format->es->video.height = state->height; 403 | format->es->video.crop.x = 0; 404 | format->es->video.crop.y = 0; 405 | format->es->video.crop.width = state->width; 406 | format->es->video.crop.height = state->height; 407 | format->es->video.frame_rate.num = 1; 408 | format->es->video.frame_rate.den = 1; 409 | 410 | status = mmal_port_format_commit(still_port); 411 | 412 | if (status) 413 | { 414 | vcos_log_error("camera still format couldn't be set"); 415 | goto error; 416 | } 417 | 418 | video_port->buffer_num = video_port->buffer_num_recommended; 419 | /* Ensure there are enough buffers to avoid dropping frames */ 420 | if (still_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) 421 | still_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM; 422 | 423 | /* Enable component */ 424 | status = mmal_component_enable(camera); 425 | 426 | if (status) 427 | { 428 | vcos_log_error("camera component couldn't be enabled"); 429 | goto error; 430 | } 431 | 432 | raspicamcontrol_set_all_parameters(camera, &state->camera_parameters); 433 | 434 | /* Create pool of buffer headers for the output port to consume */ 435 | pool = mmal_port_pool_create(video_port, video_port->buffer_num, video_port->buffer_size); 436 | 437 | if (!pool) 438 | { 439 | vcos_log_error("Failed to create buffer header pool for camera port %s", video_port->name); 440 | } 441 | 442 | state->camera_pool = pool; 443 | state->camera_component = camera; 444 | 445 | ROS_INFO("Camera component done\n"); 446 | 447 | return camera; 448 | 449 | error: 450 | 451 | if (camera) 452 | mmal_component_destroy(camera); 453 | 454 | return 0; 455 | } 456 | 457 | /** 458 | * Destroy the camera component 459 | * 460 | * @param state Pointer to state control struct 461 | * 462 | */ 463 | static void destroy_camera_component(RASPIVID_STATE *state) 464 | { 465 | if (state->camera_component) 466 | { 467 | mmal_component_destroy(state->camera_component); 468 | state->camera_component = NULL; 469 | } 470 | } 471 | 472 | /** 473 | * Create the encoder component, set up its ports 474 | * 475 | * @param state Pointer to state control struct 476 | * 477 | * @return MMAL_SUCCESS if all OK, something else otherwise 478 | * 479 | */ 480 | /*static MMAL_STATUS_T create_encoder_component(RASPIVID_STATE *state) 481 | { 482 | MMAL_COMPONENT_T *encoder = 0; 483 | MMAL_PORT_T *encoder_input = NULL, *encoder_output = NULL; 484 | MMAL_STATUS_T status; 485 | MMAL_POOL_T *pool; 486 | 487 | status = mmal_component_create(MMAL_COMPONENT_DEFAULT_IMAGE_ENCODER, &encoder); 488 | 489 | if (status != MMAL_SUCCESS) 490 | { 491 | vcos_log_error("Unable to create video encoder component"); 492 | goto error; 493 | } 494 | 495 | if (!encoder->input_num || !encoder->output_num) 496 | { 497 | status = MMAL_ENOSYS; 498 | vcos_log_error("Video encoder doesn't have input/output ports"); 499 | goto error; 500 | } 501 | 502 | encoder_input = encoder->input[0]; 503 | encoder_output = encoder->output[0]; 504 | 505 | // We want same format on input and output 506 | mmal_format_copy(encoder_output->format, encoder_input->format); 507 | 508 | // Only supporting H264 at the moment 509 | encoder_output->format->encoding = MMAL_ENCODING_JPEG; 510 | 511 | 512 | encoder_output->buffer_size = encoder_output->buffer_size_recommended; 513 | 514 | if (encoder_output->buffer_size < encoder_output->buffer_size_min) 515 | encoder_output->buffer_size = encoder_output->buffer_size_min; 516 | 517 | encoder_output->buffer_num = encoder_output->buffer_num_recommended; 518 | 519 | if (encoder_output->buffer_num < encoder_output->buffer_num_min) 520 | encoder_output->buffer_num = encoder_output->buffer_num_min; 521 | 522 | // Commit the port changes to the output port 523 | status = mmal_port_format_commit(encoder_output); 524 | 525 | if (status != MMAL_SUCCESS) 526 | { 527 | vcos_log_error("Unable to set format on video encoder output port"); 528 | goto error; 529 | } 530 | 531 | // Set the JPEG quality level 532 | status = mmal_port_parameter_set_uint32(encoder_output, MMAL_PARAMETER_JPEG_Q_FACTOR, state->quality); 533 | 534 | if (status != MMAL_SUCCESS) 535 | { 536 | vcos_log_error("Unable to set JPEG quality"); 537 | goto error; 538 | } 539 | 540 | 541 | // Enable component 542 | status = mmal_component_enable(encoder); 543 | 544 | if (status != MMAL_SUCCESS) 545 | { 546 | vcos_log_error("Unable to enable video encoder component"); 547 | goto error; 548 | } 549 | 550 | /* Create pool of buffer headers for the output port to consume 551 | pool = mmal_port_pool_create(encoder_output, encoder_output->buffer_num, encoder_output->buffer_size); 552 | 553 | if (!pool) 554 | { 555 | vcos_log_error("Failed to create buffer header pool for encoder output port %s", encoder_output->name); 556 | } 557 | 558 | state->encoder_pool = pool; 559 | state->encoder_component = encoder; 560 | 561 | ROS_INFO("Encoder component done\n"); 562 | 563 | return status; 564 | 565 | error: 566 | if (encoder) 567 | mmal_component_destroy(encoder); 568 | 569 | return status; 570 | } 571 | */ 572 | /** 573 | * Destroy the encoder component 574 | * 575 | * @param state Pointer to state control struct 576 | * 577 | */ 578 | /*static void destroy_encoder_component(RASPIVID_STATE *state) 579 | { 580 | // Get rid of any port buffers first 581 | if (state->encoder_pool) 582 | { 583 | mmal_port_pool_destroy(state->encoder_component->output[0], state->encoder_pool); 584 | } 585 | 586 | if (state->encoder_component) 587 | { 588 | mmal_component_destroy(state->encoder_component); 589 | state->encoder_component = NULL; 590 | } 591 | }*/ 592 | 593 | /** 594 | * Connect two specific ports together 595 | * 596 | * @param output_port Pointer the output port 597 | * @param input_port Pointer the input port 598 | * @param Pointer to a mmal connection pointer, reassigned if function successful 599 | * @return Returns a MMAL_STATUS_T giving result of operation 600 | * 601 | */ 602 | static MMAL_STATUS_T connect_ports(MMAL_PORT_T *output_port, MMAL_PORT_T *input_port, MMAL_CONNECTION_T **connection) 603 | { 604 | MMAL_STATUS_T status; 605 | 606 | status = mmal_connection_create(connection, output_port, input_port, MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT); 607 | 608 | if (status == MMAL_SUCCESS) 609 | { 610 | status = mmal_connection_enable(*connection); 611 | if (status != MMAL_SUCCESS) 612 | mmal_connection_destroy(*connection); 613 | } 614 | 615 | return status; 616 | } 617 | 618 | /** 619 | * Checks if specified port is valid and enabled, then disables it 620 | * 621 | * @param port Pointer the port 622 | * 623 | */ 624 | static void check_disable_port(MMAL_PORT_T *port) 625 | { 626 | if (port && port->is_enabled) 627 | mmal_port_disable(port); 628 | } 629 | 630 | /** 631 | * Handler for sigint signals 632 | * 633 | * @param signal_number ID of incoming signal. 634 | * 635 | */ 636 | static void signal_handler(int signal_number) 637 | { 638 | // Going to abort on all signals 639 | vcos_log_error("Aborting program\n"); 640 | 641 | // TODO : Need to close any open stuff...how? 642 | 643 | exit(255); 644 | } 645 | 646 | /** 647 | * init_cam 648 | 649 | */ 650 | int init_cam(RASPIVID_STATE *state) 651 | { 652 | // Our main data storage vessel.. 653 | MMAL_STATUS_T status; 654 | MMAL_PORT_T *camera_video_port = NULL; 655 | MMAL_PORT_T *camera_still_port = NULL; 656 | MMAL_PORT_T *preview_input_port = NULL; 657 | //MMAL_PORT_T *encoder_input_port = NULL; 658 | // MMAL_PORT_T *encoder_output_port = NULL; 659 | 660 | bcm_host_init(); 661 | get_status(state); 662 | // Register our application with the logging system 663 | vcos_log_register("RaspiVid", VCOS_LOG_CATEGORY); 664 | 665 | signal(SIGINT, signal_handler); 666 | 667 | // OK, we have a nice set of parameters. Now set up our components 668 | // We have three components. Camera, Preview and encoder. 669 | 670 | if (!create_camera_component(state)) 671 | { 672 | ROS_INFO("%s: Failed to create camera component", __func__); 673 | } 674 | /*else if ((status = create_encoder_component(state)) != MMAL_SUCCESS) 675 | { 676 | ROS_INFO("%s: Failed to create encode component", __func__); 677 | destroy_camera_component(state); 678 | }*/ 679 | else 680 | { 681 | PORT_USERDATA * callback_data = (PORT_USERDATA *) malloc (sizeof(PORT_USERDATA)); 682 | camera_video_port = state->camera_component->output[MMAL_CAMERA_VIDEO_PORT]; 683 | camera_still_port = state->camera_component->output[MMAL_CAMERA_CAPTURE_PORT]; 684 | //encoder_input_port = state->encoder_component->input[0]; 685 | //encoder_output_port = state->encoder_component->output[0]; 686 | /*status = connect_ports(camera_video_port, encoder_input_port, &state->encoder_connection); 687 | if (status != MMAL_SUCCESS) 688 | { 689 | ROS_INFO("%s: Failed to connect camera video port to encoder input", __func__); 690 | return 1; 691 | }*/ 692 | callback_data->buffer[0] = (unsigned char *) malloc ( state->width * state->height * 8 ); 693 | callback_data->buffer[1] = (unsigned char *) malloc ( state->width * state->height * 8 ); 694 | // Set up our userdata - this is passed though to the callback where we need the information. 695 | callback_data->pstate = state; 696 | callback_data->abort = 0; 697 | callback_data->id = 0; 698 | callback_data->frame = 0; 699 | //encoder_output_port->userdata = (struct MMAL_PORT_USERDATA_T *) callback_data_enc; 700 | camera_video_port->userdata = (struct MMAL_PORT_USERDATA_T *) callback_data; 701 | //PORT_USERDATA *pData = (PORT_USERDATA *)encoder_output_port->userdata; 702 | PORT_USERDATA *pData = (PORT_USERDATA *)camera_video_port->userdata; 703 | // Enable the encoder output port and tell it its callback function 704 | //status = mmal_port_enable(encoder_output_port, encoder_buffer_callback); 705 | status = mmal_port_enable(camera_video_port, camera_buffer_callback); 706 | if (status != MMAL_SUCCESS) 707 | { 708 | ROS_INFO("Failed to setup encoder output"); 709 | return 1; 710 | } 711 | state->isInit = 1; 712 | } 713 | return 0; 714 | } 715 | 716 | 717 | int start_capture(RASPIVID_STATE *state){ 718 | if(!(state->isInit)) init_cam(state); 719 | MMAL_PORT_T *camera_video_port = state->camera_component->output[MMAL_CAMERA_VIDEO_PORT]; 720 | //MMAL_PORT_T *encoder_output_port = state->encoder_component->output[0]; 721 | ROS_INFO("Starting video capture (%d, %d, %d, %d)\n", state->width, state->height, state->quality, state->framerate); 722 | 723 | if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS) 724 | { 725 | return 1; 726 | } 727 | // Send all the buffers to the video port 728 | { 729 | int num = mmal_queue_length(state->camera_pool->queue); 730 | int q; 731 | for (q=0;qcamera_pool->queue); 734 | 735 | if (!buffer) 736 | vcos_log_error("Unable to get a required buffer %d from pool queue", q); 737 | 738 | //if (mmal_port_send_buffer(encoder_output_port, buffer)!= MMAL_SUCCESS) 739 | if (mmal_port_send_buffer(camera_video_port, buffer)!= MMAL_SUCCESS) 740 | vcos_log_error("Unable to send a buffer to encoder output port (%d)", q); 741 | 742 | } 743 | } 744 | ROS_INFO("Video capture started\n"); 745 | return 0; 746 | 747 | } 748 | 749 | 750 | 751 | int close_cam(RASPIVID_STATE *state){ 752 | if(state->isInit){ 753 | state -> isInit = 0; 754 | MMAL_COMPONENT_T *camera = state->camera_component; 755 | MMAL_PORT_T *camera_video_port = camera->output[MMAL_CAMERA_VIDEO_PORT]; 756 | //MMAL_COMPONENT_T *encoder = state->encoder_component; 757 | //MMAL_PORT_T *encoder_output_port = state->encoder_component->output[0]; 758 | MMAL_PORT_T *camera_still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT]; 759 | PORT_USERDATA * pData = (PORT_USERDATA *)camera_video_port->userdata; 760 | 761 | if (camera_still_port && camera_still_port->is_enabled) 762 | mmal_port_disable(camera_still_port); 763 | 764 | if (camera_video_port && camera_video_port->is_enabled) 765 | mmal_port_disable(camera_video_port); 766 | 767 | //mmal_connection_destroy(state->encoder_connection); 768 | 769 | // Disable components 770 | /*if (encoder) 771 | mmal_component_disable(encoder); 772 | */ 773 | if (camera) 774 | mmal_component_disable(camera); 775 | 776 | //Destroy encoder component 777 | // Get rid of any port buffers first 778 | if (state->camera_pool) 779 | { 780 | mmal_port_pool_destroy(camera_video_port, state->camera_pool); 781 | } 782 | 783 | 784 | free(pData->buffer[0]); 785 | free(pData->buffer[1]); 786 | 787 | /*if (encoder) 788 | { 789 | mmal_component_destroy(encoder); 790 | encoder = NULL; 791 | }*/ 792 | //destroy camera component 793 | if (camera) 794 | { 795 | mmal_component_destroy(camera); 796 | camera = NULL; 797 | } 798 | return 0; 799 | }else return 1; 800 | } 801 | 802 | bool serv_start_cap( std_srvs::Empty::Request &req, 803 | std_srvs::Empty::Response &res ) 804 | { 805 | start_capture(&state_srv); 806 | return true; 807 | } 808 | 809 | 810 | bool serv_stop_cap( std_srvs::Empty::Request &req, 811 | std_srvs::Empty::Response &res ) 812 | { 813 | close_cam(&state_srv); 814 | return true; 815 | } 816 | 817 | 818 | 819 | int main(int argc, char **argv){ 820 | ros::init(argc, argv, "raspicam_raw_node"); 821 | ros::NodeHandle n; 822 | camera_info_manager::CameraInfoManager c_info_man (n, "camera", "package://raspicam/calibrations/camera.yaml"); 823 | get_status(&state_srv); 824 | 825 | if(!c_info_man.loadCameraInfo ("package://raspicam/calibrations/camera.yaml")){ 826 | ROS_INFO("Calibration file missing. Camera not calibrated"); 827 | } 828 | else 829 | { 830 | c_info = c_info_man.getCameraInfo (); 831 | ROS_INFO("Camera successfully calibrated"); 832 | } 833 | image_pub = n.advertise("camera/image", 1); 834 | camera_info_pub = n.advertise("camera/camera_info", 1); 835 | ros::ServiceServer start_cam = n.advertiseService("camera/start_capture", serv_start_cap); 836 | ros::ServiceServer stop_cam = n.advertiseService("camera/stop_capture", serv_stop_cap); 837 | ros::spin(); 838 | close_cam(&state_srv); 839 | return 0; 840 | } 841 | -------------------------------------------------------------------------------- /src/RaspiCamControl.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2013, Broadcom Europe Ltd 3 | Copyright (c) 2013, James Hughes 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the copyright holder nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | #include "interface/vcos/vcos.h" 33 | 34 | #include "interface/vmcs_host/vc_vchi_gencmd.h" 35 | #include "interface/mmal/mmal.h" 36 | #include "interface/mmal/mmal_logging.h" 37 | #include "interface/mmal/util/mmal_util.h" 38 | #include "interface/mmal/util/mmal_util_params.h" 39 | #include "interface/mmal/util/mmal_default_components.h" 40 | #include "RaspiCamControl.h" 41 | #include "RaspiCLI.h" 42 | 43 | /// Cross reference structure, mode string against mode id 44 | typedef struct xref_t 45 | { 46 | char *mode; 47 | int mmal_mode; 48 | } XREF_T; 49 | 50 | /// Structure to cross reference exposure strings against the MMAL parameter equivalent 51 | static XREF_T exposure_map[] = 52 | { 53 | {"off", MMAL_PARAM_EXPOSUREMODE_OFF}, 54 | {"auto", MMAL_PARAM_EXPOSUREMODE_AUTO}, 55 | {"night", MMAL_PARAM_EXPOSUREMODE_NIGHT}, 56 | {"nightpreview", MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW}, 57 | {"backlight", MMAL_PARAM_EXPOSUREMODE_BACKLIGHT}, 58 | {"spotlight", MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT}, 59 | {"sports", MMAL_PARAM_EXPOSUREMODE_SPORTS}, 60 | {"snow", MMAL_PARAM_EXPOSUREMODE_SNOW}, 61 | {"beach", MMAL_PARAM_EXPOSUREMODE_BEACH}, 62 | {"verylong", MMAL_PARAM_EXPOSUREMODE_VERYLONG}, 63 | {"fixedfps", MMAL_PARAM_EXPOSUREMODE_FIXEDFPS}, 64 | {"antishake", MMAL_PARAM_EXPOSUREMODE_ANTISHAKE}, 65 | {"fireworks", MMAL_PARAM_EXPOSUREMODE_FIREWORKS} 66 | }; 67 | 68 | static const int exposure_map_size = sizeof(exposure_map) / sizeof(exposure_map[0]); 69 | 70 | /// Structure to cross reference awb strings against the MMAL parameter equivalent 71 | static XREF_T awb_map[] = 72 | { 73 | {"off", MMAL_PARAM_AWBMODE_OFF}, 74 | {"auto", MMAL_PARAM_AWBMODE_AUTO}, 75 | {"sun", MMAL_PARAM_AWBMODE_SUNLIGHT}, 76 | {"cloud", MMAL_PARAM_AWBMODE_CLOUDY}, 77 | {"shade", MMAL_PARAM_AWBMODE_SHADE}, 78 | {"tungsten", MMAL_PARAM_AWBMODE_TUNGSTEN}, 79 | {"fluorescent", MMAL_PARAM_AWBMODE_FLUORESCENT}, 80 | {"incandescent", MMAL_PARAM_AWBMODE_INCANDESCENT}, 81 | {"flash", MMAL_PARAM_AWBMODE_FLASH}, 82 | {"horizon", MMAL_PARAM_AWBMODE_HORIZON} 83 | }; 84 | 85 | static const int awb_map_size = sizeof(awb_map) / sizeof(awb_map[0]); 86 | 87 | /// Structure to cross reference image effect against the MMAL parameter equivalent 88 | static XREF_T imagefx_map[] = 89 | { 90 | {"none", MMAL_PARAM_IMAGEFX_NONE}, 91 | {"negative", MMAL_PARAM_IMAGEFX_NEGATIVE}, 92 | {"solarise", MMAL_PARAM_IMAGEFX_SOLARIZE}, 93 | {"sketch", MMAL_PARAM_IMAGEFX_SKETCH}, 94 | {"denoise", MMAL_PARAM_IMAGEFX_DENOISE}, 95 | {"emboss", MMAL_PARAM_IMAGEFX_EMBOSS}, 96 | {"oilpaint", MMAL_PARAM_IMAGEFX_OILPAINT}, 97 | {"hatch", MMAL_PARAM_IMAGEFX_HATCH}, 98 | {"gpen", MMAL_PARAM_IMAGEFX_GPEN}, 99 | {"pastel", MMAL_PARAM_IMAGEFX_PASTEL}, 100 | {"watercolour", MMAL_PARAM_IMAGEFX_WATERCOLOUR}, 101 | {"film", MMAL_PARAM_IMAGEFX_FILM}, 102 | {"blur", MMAL_PARAM_IMAGEFX_BLUR}, 103 | {"saturation", MMAL_PARAM_IMAGEFX_SATURATION}, 104 | {"colourswap", MMAL_PARAM_IMAGEFX_COLOURSWAP}, 105 | {"washedout", MMAL_PARAM_IMAGEFX_WASHEDOUT}, 106 | {"posterise", MMAL_PARAM_IMAGEFX_POSTERISE}, 107 | {"colourpoint", MMAL_PARAM_IMAGEFX_COLOURPOINT}, 108 | {"colourbalance", MMAL_PARAM_IMAGEFX_COLOURBALANCE}, 109 | {"cartoon", MMAL_PARAM_IMAGEFX_CARTOON} 110 | }; 111 | 112 | static const int imagefx_map_size = sizeof(imagefx_map) / sizeof(imagefx_map[0]); 113 | 114 | static XREF_T metering_mode_map[] = 115 | { 116 | {"average", MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE}, 117 | {"spot", MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT}, 118 | {"backlit", MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT}, 119 | {"matrix", MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX} 120 | }; 121 | 122 | static const int metering_mode_map_size = sizeof(metering_mode_map)/sizeof(metering_mode_map[0]); 123 | 124 | 125 | #define CommandSharpness 0 126 | #define CommandContrast 1 127 | #define CommandBrightness 2 128 | #define CommandSaturation 3 129 | #define CommandISO 4 130 | #define CommandVideoStab 5 131 | #define CommandEVComp 6 132 | #define CommandExposure 7 133 | #define CommandAWB 8 134 | #define CommandImageFX 9 135 | #define CommandColourFX 10 136 | #define CommandMeterMode 11 137 | #define CommandRotation 12 138 | #define CommandHFlip 13 139 | #define CommandVFlip 14 140 | #define CommandROI 15 141 | 142 | static COMMAND_LIST cmdline_commands[] = 143 | { 144 | {CommandSharpness, "-sharpness", "sh", "Set image sharpness (-100 to 100)", 1}, 145 | {CommandContrast, "-contrast", "co", "Set image contrast (-100 to 100)", 1}, 146 | {CommandBrightness, "-brightness","br", "Set image brightness (0 to 100)", 1}, 147 | {CommandSaturation, "-saturation","sa", "Set image saturation (-100 to 100)", 1}, 148 | {CommandISO, "-ISO", "ISO","Set capture ISO", 1}, 149 | {CommandVideoStab, "-vstab", "vs", "Turn on video stablisation", 0}, 150 | {CommandEVComp, "-ev", "ev", "Set EV compensation", 1}, 151 | {CommandExposure, "-exposure", "ex", "Set exposure mode (see Notes)", 1}, 152 | {CommandAWB, "-awb", "awb","Set AWB mode (see Notes)", 1}, 153 | {CommandImageFX, "-imxfx", "ifx","Set image effect (see Notes)", 1}, 154 | {CommandColourFX, "-colfx", "cfx","Set colour effect (U:V)", 1}, 155 | {CommandMeterMode, "-metering", "mm", "Set metering mode (see Notes)", 1}, 156 | {CommandRotation, "-rotation", "rot","Set image rotation (0-359)", 1}, 157 | {CommandHFlip, "-hflip", "hf", "Set horizontal flip", 0}, 158 | {CommandVFlip, "-vflip", "vf", "Set vertical flip", 0}, 159 | {CommandROI, "-roi", "roi","Set region of interest (x,y,w,d as normalised coordinates [0.0-1.0])", 1} 160 | }; 161 | 162 | static int cmdline_commands_size = sizeof(cmdline_commands) / sizeof(cmdline_commands[0]); 163 | 164 | 165 | #define parameter_reset -99999 166 | 167 | /** 168 | * Update the passed in parameter according to the rest of the parameters 169 | * passed in. 170 | * 171 | * 172 | * @return 0 if reached end of cycle for this parameter, !0 otherwise 173 | */ 174 | static int update_cycle_parameter(int *option, int min, int max, int increment) 175 | { 176 | vcos_assert(option); 177 | if (!option) 178 | return 0; 179 | 180 | if (*option == parameter_reset) 181 | *option = min - increment; 182 | 183 | *option += increment; 184 | 185 | if (*option > max) 186 | { 187 | *option = parameter_reset; 188 | return 0; 189 | } 190 | else 191 | return 1; 192 | } 193 | 194 | 195 | /** 196 | * Test/Demo code to cycle through a bunch of camera settings 197 | * This code is pretty hacky so please don't complain!! 198 | * It only does stuff that should have a visual impact (hence demo!) 199 | * This will override any user supplied parameters 200 | * 201 | * Each call of this function will move on to the next setting 202 | * 203 | * @param camera Pointer to the camera to change settings on. 204 | * @return 0 if reached end of complete sequence, !0 otherwise 205 | */ 206 | 207 | int raspicamcontrol_cycle_test(MMAL_COMPONENT_T *camera) 208 | { 209 | static int parameter = 0; 210 | static int parameter_option = parameter_reset; // which value the parameter currently has 211 | 212 | vcos_assert(camera); 213 | 214 | // We are going to cycle through all the relevant entries in the parameter block 215 | // and send options to the camera. 216 | if (parameter == 0) 217 | { 218 | // sharpness 219 | if (update_cycle_parameter(¶meter_option, -100, 100, 10)) 220 | raspicamcontrol_set_sharpness(camera, parameter_option); 221 | else 222 | { 223 | raspicamcontrol_set_sharpness(camera, 0); 224 | parameter++; 225 | } 226 | } 227 | else 228 | if (parameter == 1) 229 | { 230 | // contrast 231 | if (update_cycle_parameter(¶meter_option, -100, 100, 10)) 232 | raspicamcontrol_set_contrast(camera, parameter_option); 233 | else 234 | { 235 | raspicamcontrol_set_contrast(camera, 0); 236 | parameter++; 237 | } 238 | } 239 | else 240 | if (parameter == 2) 241 | { 242 | // brightness 243 | if (update_cycle_parameter(¶meter_option, 0, 100, 10)) 244 | raspicamcontrol_set_brightness(camera, parameter_option); 245 | else 246 | { 247 | raspicamcontrol_set_brightness(camera, 50); 248 | parameter++; 249 | } 250 | } 251 | else 252 | if (parameter == 3) 253 | { 254 | // contrast 255 | if (update_cycle_parameter(¶meter_option, -100, 100, 10)) 256 | raspicamcontrol_set_saturation(camera, parameter_option); 257 | else 258 | { 259 | parameter++; 260 | raspicamcontrol_set_saturation(camera, 0); 261 | } 262 | } 263 | else 264 | if (parameter == 4) 265 | { 266 | // EV 267 | if (update_cycle_parameter(¶meter_option, -10, 10, 4)) 268 | raspicamcontrol_set_exposure_compensation(camera, parameter_option); 269 | else 270 | { 271 | raspicamcontrol_set_exposure_compensation(camera, 0); 272 | parameter++; 273 | } 274 | } 275 | else 276 | if (parameter == 5) 277 | { 278 | // MMAL_PARAM_EXPOSUREMODE_T 279 | if (update_cycle_parameter(¶meter_option, 0, exposure_map_size, 1)) 280 | raspicamcontrol_set_exposure_mode(camera, (MMAL_PARAM_EXPOSUREMODE_T) exposure_map[parameter_option].mmal_mode); 281 | else 282 | { 283 | raspicamcontrol_set_exposure_mode(camera, MMAL_PARAM_EXPOSUREMODE_AUTO); 284 | parameter++; 285 | } 286 | } 287 | else 288 | if (parameter == 6) 289 | { 290 | // MMAL_PARAM_AWB_T 291 | if (update_cycle_parameter(¶meter_option, 0, awb_map_size, 1)) 292 | raspicamcontrol_set_awb_mode(camera, (MMAL_PARAM_AWBMODE_T) awb_map[parameter_option].mmal_mode); 293 | else 294 | { 295 | raspicamcontrol_set_awb_mode(camera, MMAL_PARAM_AWBMODE_AUTO); 296 | parameter++; 297 | } 298 | } 299 | if (parameter == 7) 300 | { 301 | // MMAL_PARAM_IMAGEFX_T 302 | if (update_cycle_parameter(¶meter_option, 0, imagefx_map_size, 1)) 303 | raspicamcontrol_set_imageFX(camera, (MMAL_PARAM_IMAGEFX_T) imagefx_map[parameter_option].mmal_mode); 304 | else 305 | { 306 | raspicamcontrol_set_imageFX(camera, MMAL_PARAM_IMAGEFX_NONE); 307 | parameter++; 308 | } 309 | } 310 | if (parameter == 8) 311 | { 312 | MMAL_PARAM_COLOURFX_T colfx = {0,0,0}; 313 | switch (parameter_option) 314 | { 315 | case parameter_reset : 316 | parameter_option = 1; 317 | colfx.u = 128; 318 | colfx.v = 128; 319 | break; 320 | case 1 : 321 | parameter_option = 2; 322 | colfx.u = 100; 323 | colfx.v = 200; 324 | break; 325 | case 2 : 326 | parameter_option = parameter_reset; 327 | colfx.enable = 0; 328 | parameter++; 329 | break; 330 | } 331 | raspicamcontrol_set_colourFX(camera, &colfx); 332 | } 333 | 334 | // Orientation 335 | if (parameter == 9) 336 | { 337 | switch (parameter_option) 338 | { 339 | case parameter_reset: 340 | raspicamcontrol_set_rotation(camera, 90); 341 | parameter_option = 1; 342 | break; 343 | 344 | case 1 : 345 | raspicamcontrol_set_rotation(camera, 180); 346 | parameter_option = 2; 347 | break; 348 | 349 | case 2 : 350 | raspicamcontrol_set_rotation(camera, 270); 351 | parameter_option = 3; 352 | break; 353 | 354 | case 3 : 355 | { 356 | raspicamcontrol_set_rotation(camera, 0); 357 | raspicamcontrol_set_flips(camera, 1,0); 358 | parameter_option = 4; 359 | break; 360 | } 361 | case 4 : 362 | { 363 | raspicamcontrol_set_flips(camera, 0,1); 364 | parameter_option = 5; 365 | break; 366 | } 367 | case 5 : 368 | { 369 | raspicamcontrol_set_flips(camera, 1, 1); 370 | parameter_option = 6; 371 | break; 372 | } 373 | case 6 : 374 | { 375 | raspicamcontrol_set_flips(camera, 0, 0); 376 | parameter_option = parameter_reset; 377 | parameter++; 378 | break; 379 | } 380 | } 381 | } 382 | 383 | if (parameter == 10) 384 | { 385 | parameter = 1; 386 | return 0; 387 | } 388 | 389 | return 1; 390 | } 391 | 392 | 393 | /** 394 | * Function to take a string, a mapping, and return the int equivalent 395 | * @param str Incoming string to match 396 | * @param map Mapping data 397 | * @param num_refs The number of items in the mapping data 398 | * @return The integer match for the string, or -1 if no match 399 | */ 400 | static int map_xref(const char *str, const XREF_T *map, int num_refs) 401 | { 402 | int i; 403 | 404 | for (i=0;i 0 && arg2 == NULL)) 516 | return 0; 517 | 518 | switch (command_id) 519 | { 520 | case CommandSharpness : // sharpness - needs single number parameter 521 | sscanf(arg2, "%d", ¶ms->sharpness); 522 | used = 2; 523 | break; 524 | 525 | case CommandContrast : // contrast - needs single number parameter 526 | sscanf(arg2, "%d", ¶ms->contrast); 527 | used = 2; 528 | break; 529 | 530 | case CommandBrightness : // brightness - needs single number parameter 531 | sscanf(arg2, "%d", ¶ms->brightness); 532 | used = 2; 533 | break; 534 | 535 | case CommandSaturation : // saturation - needs single number parameter 536 | sscanf(arg2, "%d", ¶ms->saturation); 537 | used = 2; 538 | break; 539 | 540 | case CommandISO : // ISO - needs single number parameter 541 | sscanf(arg2, "%d", ¶ms->ISO); 542 | used = 2; 543 | break; 544 | 545 | case CommandVideoStab : // video stabilisation - if here, its on 546 | params->videoStabilisation = 1; 547 | used = 1; 548 | break; 549 | 550 | case CommandEVComp : // EV - needs single number parameter 551 | sscanf(arg2, "%d", ¶ms->exposureCompensation); 552 | used = 2; 553 | break; 554 | 555 | case CommandExposure : // exposure mode - needs string 556 | params->exposureMode = exposure_mode_from_string(arg2); 557 | used = 2; 558 | break; 559 | 560 | case CommandAWB : // AWB mode - needs single number parameter 561 | params->awbMode = awb_mode_from_string(arg2); 562 | used = 2; 563 | break; 564 | 565 | case CommandImageFX : // Image FX - needs string 566 | params->imageEffect = imagefx_mode_from_string(arg2); 567 | used = 2; 568 | break; 569 | 570 | case CommandColourFX : // Colour FX - needs string "u:v" 571 | sscanf(arg2, "%d:%d", ¶ms->colourEffects.u, ¶ms->colourEffects.u); 572 | params->colourEffects.enable = 1; 573 | used = 2; 574 | break; 575 | 576 | case CommandMeterMode: 577 | params->exposureMeterMode = metering_mode_from_string(arg2); 578 | used = 2; 579 | break; 580 | 581 | case CommandRotation : // Rotation - degree 582 | sscanf(arg2, "%d", ¶ms->rotation); 583 | used = 2; 584 | break; 585 | 586 | case CommandHFlip : 587 | params->hflip = 1; 588 | used = 1; 589 | break; 590 | 591 | case CommandVFlip : 592 | params->vflip = 1; 593 | used = 1; 594 | break; 595 | 596 | case CommandROI : 597 | { 598 | double x,y,w,h; 599 | int args; 600 | 601 | args = sscanf(arg2, "%lf,%lf,%lf,%lf", &x,&y,&w,&h); 602 | 603 | if (args != 4 || x > 1.0 || y > 1.0 || w > 1.0 || h > 1.0) 604 | { 605 | return 0; 606 | } 607 | 608 | // Make sure we stay within bounds 609 | if (x + w > 1.0) 610 | w = 1 - x; 611 | 612 | if (y + h > 1.0) 613 | h = 1 - y; 614 | 615 | params->roi.x = x; 616 | params->roi.y = y; 617 | params->roi.w = w; 618 | params->roi.h = h; 619 | 620 | used = 2; 621 | break; 622 | } 623 | 624 | } 625 | 626 | return used; 627 | } 628 | 629 | /** 630 | * Display help for command line options 631 | */ 632 | void raspicamcontrol_display_help() 633 | { 634 | int i; 635 | 636 | fprintf(stderr, "\nImage parameter commands\n\n"); 637 | 638 | raspicli_display_help(cmdline_commands, cmdline_commands_size); 639 | 640 | fprintf(stderr, "\n\nNotes\n\nExposure mode options :\n%s", exposure_map[0].mode ); 641 | 642 | for (i=1;iexposureMode, exposure_map, exposure_map_size); 680 | const char *awb_mode = unmap_xref(params->awbMode, awb_map, awb_map_size); 681 | const char *image_effect = unmap_xref(params->imageEffect, imagefx_map, imagefx_map_size); 682 | const char *metering_mode = unmap_xref(params->exposureMeterMode, metering_mode_map, metering_mode_map_size); 683 | 684 | fprintf(stderr, "Sharpness %d, Contrast %d, Brightness %d\n", params->sharpness, params->contrast, params->brightness); 685 | fprintf(stderr, "Saturation %d, ISO %d, Video Stabilisation %s, Exposure compensation %d\n", params->saturation, params->ISO, params->videoStabilisation ? "Yes": "No", params->exposureCompensation); 686 | fprintf(stderr, "Exposure Mode '%s', AWB Mode '%s', Image Effect '%s'\n", exp_mode, awb_mode, image_effect); 687 | 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); 688 | fprintf(stderr, "Rotation %d, hflip %s, vflip %s\n", params->rotation, params->hflip ? "Yes":"No",params->vflip ? "Yes":"No"); 689 | fprintf(stderr, "ROI x %lf, y %f, w %f h %f\n", params->roi.x, params->roi.y, params->roi.w, params->roi.h); 690 | } 691 | 692 | /** 693 | * Convert a MMAL status return value to a simple boolean of success 694 | * ALso displays a fault if code is not success 695 | * 696 | * @param status The error code to convert 697 | * @return 0 if status is sucess, 1 otherwise 698 | */ 699 | int mmal_status_to_int(MMAL_STATUS_T status) 700 | { 701 | if (status == MMAL_SUCCESS) 702 | return 0; 703 | else 704 | { 705 | switch (status) 706 | { 707 | case MMAL_ENOMEM : vcos_log_error("Out of memory"); break; 708 | case MMAL_ENOSPC : vcos_log_error("Out of resources (other than memory)"); break; 709 | case MMAL_EINVAL: vcos_log_error("Argument is invalid"); break; 710 | case MMAL_ENOSYS : vcos_log_error("Function not implemented"); break; 711 | case MMAL_ENOENT : vcos_log_error("No such file or directory"); break; 712 | case MMAL_ENXIO : vcos_log_error("No such device or address"); break; 713 | case MMAL_EIO : vcos_log_error("I/O error"); break; 714 | case MMAL_ESPIPE : vcos_log_error("Illegal seek"); break; 715 | case MMAL_ECORRUPT : vcos_log_error("Data is corrupt \attention FIXME: not POSIX"); break; 716 | case MMAL_ENOTREADY :vcos_log_error("Component is not ready \attention FIXME: not POSIX"); break; 717 | case MMAL_ECONFIG : vcos_log_error("Component is not configured \attention FIXME: not POSIX"); break; 718 | case MMAL_EISCONN : vcos_log_error("Port is already connected "); break; 719 | case MMAL_ENOTCONN : vcos_log_error("Port is disconnected"); break; 720 | case MMAL_EAGAIN : vcos_log_error("Resource temporarily unavailable. Try again later"); break; 721 | case MMAL_EFAULT : vcos_log_error("Bad address"); break; 722 | default : vcos_log_error("Unknown status error"); break; 723 | } 724 | 725 | return 1; 726 | } 727 | } 728 | 729 | /** 730 | * Give the supplied parameter block a set of default values 731 | * @params Pointer to parameter block 732 | */ 733 | void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS *params) 734 | { 735 | vcos_assert(params); 736 | 737 | params->sharpness = 0; 738 | params->contrast = 0; 739 | params->brightness = 50; 740 | params->saturation = 0; 741 | params->ISO = 400; 742 | params->videoStabilisation = 0; 743 | params->exposureCompensation = 0; 744 | params->exposureMode = MMAL_PARAM_EXPOSUREMODE_AUTO; 745 | params->exposureMeterMode = MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE; 746 | params->awbMode = MMAL_PARAM_AWBMODE_AUTO; 747 | params->imageEffect = MMAL_PARAM_IMAGEFX_NONE; 748 | params->colourEffects.enable = 0; 749 | params->colourEffects.u = 128; 750 | params->colourEffects.v = 128; 751 | params->rotation = 0; 752 | params->hflip = params->vflip = 0; 753 | params->roi.x = params->roi.y = 0.0; 754 | params->roi.w = params->roi.h = 1.0; 755 | } 756 | 757 | /** 758 | * Get all the current camera parameters from specified camera component 759 | * @param camera Pointer to camera component 760 | * @param params Pointer to parameter block to accept settings 761 | * @return 0 if successful, non-zero if unsuccessful 762 | */ 763 | int raspicamcontrol_get_all_parameters(MMAL_COMPONENT_T *camera, RASPICAM_CAMERA_PARAMETERS *params) 764 | { 765 | vcos_assert(camera); 766 | vcos_assert(params); 767 | 768 | if (!camera || !params) 769 | return 1; 770 | 771 | /* TODO : Write these get functions 772 | params->sharpness = raspicamcontrol_get_sharpness(camera); 773 | params->contrast = raspicamcontrol_get_contrast(camera); 774 | params->brightness = raspicamcontrol_get_brightness(camera); 775 | params->saturation = raspicamcontrol_get_saturation(camera); 776 | params->ISO = raspicamcontrol_get_ISO(camera); 777 | params->videoStabilisation = raspicamcontrol_get_video_stabilisation(camera); 778 | params->exposureCompensation = raspicamcontrol_get_exposure_compensation(camera); 779 | params->exposureMode = raspicamcontrol_get_exposure_mode(camera); 780 | params->awbMode = raspicamcontrol_get_awb_mode(camera); 781 | params->imageEffect = raspicamcontrol_get_image_effect(camera); 782 | params->colourEffects = raspicamcontrol_get_colour_effect(camera); 783 | params->thumbnailConfig = raspicamcontrol_get_thumbnail_config(camera); 784 | */ 785 | return 0; 786 | } 787 | 788 | /** 789 | * Set the specified camera to all the specified settings 790 | * @param camera Pointer to camera component 791 | * @param params Pointer to parameter block containing parameters 792 | * @return 0 if successful, none-zero if unsuccessful. 793 | */ 794 | int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T *camera, const RASPICAM_CAMERA_PARAMETERS *params) 795 | { 796 | int result; 797 | 798 | result = raspicamcontrol_set_saturation(camera, params->saturation); 799 | result += raspicamcontrol_set_sharpness(camera, params->sharpness); 800 | result += raspicamcontrol_set_contrast(camera, params->contrast); 801 | result += raspicamcontrol_set_brightness(camera, params->brightness); 802 | result += raspicamcontrol_set_ISO(camera, params->ISO); 803 | result += raspicamcontrol_set_video_stabilisation(camera, params->videoStabilisation); 804 | result += raspicamcontrol_set_exposure_compensation(camera, params->exposureCompensation); 805 | result += raspicamcontrol_set_exposure_mode(camera, params->exposureMode); 806 | result += raspicamcontrol_set_metering_mode(camera, params->exposureMeterMode); 807 | result += raspicamcontrol_set_awb_mode(camera, params->awbMode); 808 | result += raspicamcontrol_set_imageFX(camera, params->imageEffect); 809 | result += raspicamcontrol_set_colourFX(camera, ¶ms->colourEffects); 810 | //result += raspicamcontrol_set_thumbnail_parameters(camera, ¶ms->thumbnailConfig); TODO Not working for some reason 811 | result += raspicamcontrol_set_rotation(camera, params->rotation); 812 | result += raspicamcontrol_set_flips(camera, params->hflip, params->vflip); 813 | result += raspicamcontrol_set_ROI(camera, params->roi); 814 | 815 | return result; 816 | } 817 | 818 | /** 819 | * Adjust the saturation level for images 820 | * @param camera Pointer to camera component 821 | * @param saturation Value to adjust, -100 to 100 822 | * @return 0 if successful, non-zero if any parameters out of range 823 | */ 824 | int raspicamcontrol_set_saturation(MMAL_COMPONENT_T *camera, int saturation) 825 | { 826 | int ret = 0; 827 | 828 | if (!camera) 829 | return 1; 830 | 831 | if (saturation >= -100 && saturation <= 100) 832 | { 833 | MMAL_RATIONAL_T value = {saturation, 100}; 834 | ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_SATURATION, value)); 835 | } 836 | else 837 | { 838 | vcos_log_error("Invalid saturation value"); 839 | ret = 1; 840 | } 841 | 842 | return ret; 843 | } 844 | 845 | /** 846 | * Set the sharpness of the image 847 | * @param camera Pointer to camera component 848 | * @param sharpness Sharpness adjustment -100 to 100 849 | */ 850 | int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T *camera, int sharpness) 851 | { 852 | int ret = 0; 853 | 854 | if (!camera) 855 | return 1; 856 | 857 | if (sharpness >= -100 && sharpness <= 100) 858 | { 859 | MMAL_RATIONAL_T value = {sharpness, 100}; 860 | ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_SHARPNESS, value)); 861 | } 862 | else 863 | { 864 | vcos_log_error("Invalid sharpness value"); 865 | ret = 1; 866 | } 867 | 868 | return ret; 869 | } 870 | 871 | /** 872 | * Set the contrast adjustment for the image 873 | * @param camera Pointer to camera component 874 | * @param contrast Contrast adjustment -100 to 100 875 | * @return 876 | */ 877 | int raspicamcontrol_set_contrast(MMAL_COMPONENT_T *camera, int contrast) 878 | { 879 | int ret = 0; 880 | 881 | if (!camera) 882 | return 1; 883 | 884 | if (contrast >= -100 && contrast <= 100) 885 | { 886 | MMAL_RATIONAL_T value = {contrast, 100}; 887 | ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_CONTRAST, value)); 888 | } 889 | else 890 | { 891 | vcos_log_error("Invalid contrast value"); 892 | ret = 1; 893 | } 894 | 895 | return ret; 896 | } 897 | 898 | /** 899 | * Adjust the brightness level for images 900 | * @param camera Pointer to camera component 901 | * @param brightness Value to adjust, 0 to 100 902 | * @return 0 if successful, non-zero if any parameters out of range 903 | */ 904 | int raspicamcontrol_set_brightness(MMAL_COMPONENT_T *camera, int brightness) 905 | { 906 | int ret = 0; 907 | 908 | if (!camera) 909 | return 1; 910 | 911 | if (brightness >= 0 && brightness <= 100) 912 | { 913 | MMAL_RATIONAL_T value = {brightness, 100}; 914 | ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_BRIGHTNESS, value)); 915 | } 916 | else 917 | { 918 | vcos_log_error("Invalid brightness value"); 919 | ret = 1; 920 | } 921 | 922 | return ret; 923 | } 924 | 925 | /** 926 | * Adjust the ISO used for images 927 | * @param camera Pointer to camera component 928 | * @param ISO Value to set TODO : 929 | * @return 0 if successful, non-zero if any parameters out of range 930 | */ 931 | int raspicamcontrol_set_ISO(MMAL_COMPONENT_T *camera, int ISO) 932 | { 933 | if (!camera) 934 | return 1; 935 | 936 | return mmal_status_to_int(mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_ISO, ISO)); 937 | } 938 | 939 | /** 940 | * Adjust the metering mode for images 941 | * @param camera Pointer to camera component 942 | * @param saturation Value from following 943 | * - MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE, 944 | * - MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT, 945 | * - MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT, 946 | * - MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX 947 | * @return 0 if successful, non-zero if any parameters out of range 948 | */ 949 | int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T m_mode ) 950 | { 951 | MMAL_PARAMETER_EXPOSUREMETERINGMODE_T meter_mode = {{MMAL_PARAMETER_EXP_METERING_MODE,sizeof(meter_mode)}, 952 | m_mode}; 953 | if (!camera) 954 | return 1; 955 | 956 | return mmal_status_to_int(mmal_port_parameter_set(camera->control, &meter_mode.hdr)); 957 | } 958 | 959 | 960 | /** 961 | * Set the video stabilisation flag. Only used in video mode 962 | * @param camera Pointer to camera component 963 | * @param saturation Flag 0 off 1 on 964 | * @return 0 if successful, non-zero if any parameters out of range 965 | */ 966 | int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T *camera, int vstabilisation) 967 | { 968 | if (!camera) 969 | return 1; 970 | 971 | return mmal_status_to_int(mmal_port_parameter_set_boolean(camera->control, MMAL_PARAMETER_VIDEO_STABILISATION, vstabilisation)); 972 | } 973 | 974 | /** 975 | * Adjust the exposure compensation for images (EV) 976 | * @param camera Pointer to camera component 977 | * @param exp_comp Value to adjust, -10 to +10 978 | * @return 0 if successful, non-zero if any parameters out of range 979 | */ 980 | int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T *camera, int exp_comp) 981 | { 982 | if (!camera) 983 | return 1; 984 | 985 | return mmal_status_to_int(mmal_port_parameter_set_int32(camera->control, MMAL_PARAMETER_EXPOSURE_COMP , exp_comp)); 986 | } 987 | 988 | 989 | /** 990 | * Set exposure mode for images 991 | * @param camera Pointer to camera component 992 | * @param mode Exposure mode to set from 993 | * - MMAL_PARAM_EXPOSUREMODE_OFF, 994 | * - MMAL_PARAM_EXPOSUREMODE_AUTO, 995 | * - MMAL_PARAM_EXPOSUREMODE_NIGHT, 996 | * - MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW, 997 | * - MMAL_PARAM_EXPOSUREMODE_BACKLIGHT, 998 | * - MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT, 999 | * - MMAL_PARAM_EXPOSUREMODE_SPORTS, 1000 | * - MMAL_PARAM_EXPOSUREMODE_SNOW, 1001 | * - MMAL_PARAM_EXPOSUREMODE_BEACH, 1002 | * - MMAL_PARAM_EXPOSUREMODE_VERYLONG, 1003 | * - MMAL_PARAM_EXPOSUREMODE_FIXEDFPS, 1004 | * - MMAL_PARAM_EXPOSUREMODE_ANTISHAKE, 1005 | * - MMAL_PARAM_EXPOSUREMODE_FIREWORKS, 1006 | * 1007 | * @return 0 if successful, non-zero if any parameters out of range 1008 | */ 1009 | int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMODE_T mode) 1010 | { 1011 | MMAL_PARAMETER_EXPOSUREMODE_T exp_mode = {{MMAL_PARAMETER_EXPOSURE_MODE,sizeof(exp_mode)}, mode}; 1012 | 1013 | if (!camera) 1014 | return 1; 1015 | 1016 | return mmal_status_to_int(mmal_port_parameter_set(camera->control, &exp_mode.hdr)); 1017 | } 1018 | 1019 | 1020 | /** 1021 | * Set the aWB (auto white balance) mode for images 1022 | * @param camera Pointer to camera component 1023 | * @param awb_mode Value to set from 1024 | * - MMAL_PARAM_AWBMODE_OFF, 1025 | * - MMAL_PARAM_AWBMODE_AUTO, 1026 | * - MMAL_PARAM_AWBMODE_SUNLIGHT, 1027 | * - MMAL_PARAM_AWBMODE_CLOUDY, 1028 | * - MMAL_PARAM_AWBMODE_SHADE, 1029 | * - MMAL_PARAM_AWBMODE_TUNGSTEN, 1030 | * - MMAL_PARAM_AWBMODE_FLUORESCENT, 1031 | * - MMAL_PARAM_AWBMODE_INCANDESCENT, 1032 | * - MMAL_PARAM_AWBMODE_FLASH, 1033 | * - MMAL_PARAM_AWBMODE_HORIZON, 1034 | * @return 0 if successful, non-zero if any parameters out of range 1035 | */ 1036 | int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_AWBMODE_T awb_mode) 1037 | { 1038 | MMAL_PARAMETER_AWBMODE_T param = {{MMAL_PARAMETER_AWB_MODE,sizeof(param)}, awb_mode}; 1039 | 1040 | if (!camera) 1041 | return 1; 1042 | 1043 | return mmal_status_to_int(mmal_port_parameter_set(camera->control, ¶m.hdr)); 1044 | } 1045 | 1046 | /** 1047 | * Set the image effect for the images 1048 | * @param camera Pointer to camera component 1049 | * @param imageFX Value from 1050 | * - MMAL_PARAM_IMAGEFX_NONE, 1051 | * - MMAL_PARAM_IMAGEFX_NEGATIVE, 1052 | * - MMAL_PARAM_IMAGEFX_SOLARIZE, 1053 | * - MMAL_PARAM_IMAGEFX_POSTERIZE, 1054 | * - MMAL_PARAM_IMAGEFX_WHITEBOARD, 1055 | * - MMAL_PARAM_IMAGEFX_BLACKBOARD, 1056 | * - MMAL_PARAM_IMAGEFX_SKETCH, 1057 | * - MMAL_PARAM_IMAGEFX_DENOISE, 1058 | * - MMAL_PARAM_IMAGEFX_EMBOSS, 1059 | * - MMAL_PARAM_IMAGEFX_OILPAINT, 1060 | * - MMAL_PARAM_IMAGEFX_HATCH, 1061 | * - MMAL_PARAM_IMAGEFX_GPEN, 1062 | * - MMAL_PARAM_IMAGEFX_PASTEL, 1063 | * - MMAL_PARAM_IMAGEFX_WATERCOLOUR, 1064 | * - MMAL_PARAM_IMAGEFX_FILM, 1065 | * - MMAL_PARAM_IMAGEFX_BLUR, 1066 | * - MMAL_PARAM_IMAGEFX_SATURATION, 1067 | * - MMAL_PARAM_IMAGEFX_COLOURSWAP, 1068 | * - MMAL_PARAM_IMAGEFX_WASHEDOUT, 1069 | * - MMAL_PARAM_IMAGEFX_POSTERISE, 1070 | * - MMAL_PARAM_IMAGEFX_COLOURPOINT, 1071 | * - MMAL_PARAM_IMAGEFX_COLOURBALANCE, 1072 | * - MMAL_PARAM_IMAGEFX_CARTOON, 1073 | * @return 0 if successful, non-zero if any parameters out of range 1074 | */ 1075 | int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T *camera, MMAL_PARAM_IMAGEFX_T imageFX) 1076 | { 1077 | MMAL_PARAMETER_IMAGEFX_T imgFX = {{MMAL_PARAMETER_IMAGE_EFFECT,sizeof(imgFX)}, imageFX}; 1078 | 1079 | if (!camera) 1080 | return 1; 1081 | 1082 | return mmal_status_to_int(mmal_port_parameter_set(camera->control, &imgFX.hdr)); 1083 | } 1084 | 1085 | /* TODO :what to do with the image effects parameters? 1086 | MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imfx_param = {{MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,sizeof(imfx_param)}, 1087 | imageFX, 0, {0}}; 1088 | mmal_port_parameter_set(camera->control, &imfx_param.hdr); 1089 | */ 1090 | 1091 | /** 1092 | * Set the colour effect for images (Set UV component) 1093 | * @param camera Pointer to camera component 1094 | * @param colourFX Contains enable state and U and V numbers to set (e.g. 128,128 = Black and white) 1095 | * @return 0 if successful, non-zero if any parameters out of range 1096 | */ 1097 | int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T *camera, const MMAL_PARAM_COLOURFX_T *colourFX) 1098 | { 1099 | MMAL_PARAMETER_COLOURFX_T colfx = {{MMAL_PARAMETER_COLOUR_EFFECT,sizeof(colfx)}, 0, 0, 0}; 1100 | 1101 | if (!camera) 1102 | return 1; 1103 | 1104 | colfx.enable = colourFX->enable; 1105 | colfx.u = colourFX->u; 1106 | colfx.v = colourFX->v; 1107 | 1108 | return mmal_status_to_int(mmal_port_parameter_set(camera->control, &colfx.hdr)); 1109 | 1110 | } 1111 | 1112 | 1113 | /** 1114 | * Set the rotation of the image 1115 | * @param camera Pointer to camera component 1116 | * @param rotation Degree of rotation (any number, but will be converted to 0,90,180 or 270 only) 1117 | * @return 0 if successful, non-zero if any parameters out of range 1118 | */ 1119 | int raspicamcontrol_set_rotation(MMAL_COMPONENT_T *camera, int rotation) 1120 | { 1121 | int ret; 1122 | int my_rotation = ((rotation % 360 ) / 90) * 90; 1123 | 1124 | ret = mmal_port_parameter_set_int32(camera->output[0], MMAL_PARAMETER_ROTATION, my_rotation); 1125 | mmal_port_parameter_set_int32(camera->output[1], MMAL_PARAMETER_ROTATION, my_rotation); 1126 | mmal_port_parameter_set_int32(camera->output[2], MMAL_PARAMETER_ROTATION, my_rotation); 1127 | 1128 | return ret; 1129 | } 1130 | 1131 | /** 1132 | * Set the flips state of the image 1133 | * @param camera Pointer to camera component 1134 | * @param hflip If true, horizontally flip the image 1135 | * @param vflip If true, vertically flip the image 1136 | * 1137 | * @return 0 if successful, non-zero if any parameters out of range 1138 | */ 1139 | int raspicamcontrol_set_flips(MMAL_COMPONENT_T *camera, int hflip, int vflip) 1140 | { 1141 | MMAL_PARAMETER_MIRROR_T mirror = {{MMAL_PARAMETER_MIRROR, sizeof(MMAL_PARAMETER_MIRROR_T)}, MMAL_PARAM_MIRROR_NONE}; 1142 | 1143 | if (hflip && vflip) 1144 | mirror.value = MMAL_PARAM_MIRROR_BOTH; 1145 | else 1146 | if (hflip) 1147 | mirror.value = MMAL_PARAM_MIRROR_HORIZONTAL; 1148 | else 1149 | if (vflip) 1150 | mirror.value = MMAL_PARAM_MIRROR_VERTICAL; 1151 | 1152 | mmal_port_parameter_set(camera->output[0], &mirror.hdr); 1153 | mmal_port_parameter_set(camera->output[1], &mirror.hdr); 1154 | return mmal_port_parameter_set(camera->output[2], &mirror.hdr); 1155 | } 1156 | 1157 | /** 1158 | * Set the ROI of the sensor to use for captures/preview 1159 | * @param camera Pointer to camera component 1160 | * @param rect Normalised coordinates of ROI rectangle 1161 | * 1162 | * @return 0 if successful, non-zero if any parameters out of range 1163 | */ 1164 | int raspicamcontrol_set_ROI(MMAL_COMPONENT_T *camera, PARAM_FLOAT_RECT_T rect) 1165 | { 1166 | MMAL_PARAMETER_INPUT_CROP_T crop = {{MMAL_PARAMETER_INPUT_CROP, sizeof(MMAL_PARAMETER_INPUT_CROP_T)}}; 1167 | 1168 | crop.rect.x = (65536 * rect.x); 1169 | crop.rect.y = (65536 * rect.y); 1170 | crop.rect.width = (65536 * rect.w); 1171 | crop.rect.height = (65536 * rect.h); 1172 | 1173 | return mmal_port_parameter_set(camera->control, &crop.hdr); 1174 | } 1175 | 1176 | 1177 | /** 1178 | * Asked GPU how much memory it has allocated 1179 | * 1180 | * @return amount of memory in MB 1181 | */ 1182 | /*static int raspicamcontrol_get_mem_gpu(void) 1183 | { 1184 | char response[80] = ""; 1185 | int gpu_mem = 0; 1186 | if (vc_gencmd(response, sizeof response, "get_mem gpu") == 0) 1187 | vc_gencmd_number_property(response, "gpu", &gpu_mem); 1188 | return gpu_mem; 1189 | }*/ 1190 | 1191 | /** 1192 | * Ask GPU about its camera abilities 1193 | * @param supported None-zero if software supports the camera 1194 | * @param detected None-zero if a camera has been detected 1195 | */ 1196 | /*static void raspicamcontrol_get_camera(int *supported, int *detected) 1197 | { 1198 | char response[80] = ""; 1199 | if (vc_gencmd(response, sizeof response, "get_camera") == 0) 1200 | { 1201 | if (supported) 1202 | vc_gencmd_number_property(response, "supported", supported); 1203 | if (detected) 1204 | vc_gencmd_number_property(response, "detected", detected); 1205 | } 1206 | }*/ 1207 | 1208 | /** 1209 | * Check to see if camera is supported, and we have allocated enough meooryAsk GPU about its camera abilities 1210 | * @param supported None-zero if software supports the camera 1211 | * @param detected None-zero if a camera has been detected 1212 | */ 1213 | void raspicamcontrol_check_configuration(int min_gpu_mem) 1214 | { 1215 | int gpu_mem = 256;//raspicamcontrol_get_mem_gpu(); 1216 | int supported = 1, detected = 1; 1217 | //raspicamcontrol_get_camera(&supported, &detected); 1218 | if (!supported) 1219 | vcos_log_error("Camera is not enabled in this build. Try running \"sudo raspi-config\" and ensure that \"camera\" has been enabled\n"); 1220 | else if (gpu_mem < min_gpu_mem) 1221 | 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); 1222 | else if (!detected) 1223 | vcos_log_error("Camera is not detected. Please check carefully the camera module is installed correctly\n"); 1224 | else 1225 | vcos_log_error("Failed to run camera app. Please check for firmware updates\n"); 1226 | } 1227 | 1228 | --------------------------------------------------------------------------------