This repository has been archived by the owner on Jun 18, 2022. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 104
/
RaspiCamera.cpp
executable file
·324 lines (258 loc) · 8.76 KB
/
RaspiCamera.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
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
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
#include "capture/RaspiCamera.h"
#include <signal.h>
#include <sched.h>
using namespace IL;
struct State {
Camera* camera;
VideoEncode* preview_encode;
VideoEncode* record_encode;
bool recording;
bool running;
pthread_t preview_thid;
pthread_t record_thid;
} state = { nullptr, nullptr, nullptr, false, false };
std::ofstream file;
static int HighPri(const int pri)
{
struct sched_param sched;
memset(&sched, 0, sizeof(sched));
if (pri > sched_get_priority_max(SCHED_RR))
{
sched.sched_priority = sched_get_priority_max( SCHED_RR );
}
else
{
sched.sched_priority = pri;
}
return sched_setscheduler(0, SCHED_RR, &sched);
}
void* preview_thread(void* self)
{
kerberos::RaspiCamera * capture = (kerberos::RaspiCamera *) self;
// Retrieve OMX buffer
capture->data_buffer = state.camera->outputPorts()[70].buffer->pBuffer;
capture->mjpeg_data_buffer = state.preview_encode->outputPorts()[201].buffer->pBuffer;
HighPri(99); // Mark the thread as high priority
// ATTENTION : Each loop must take less time than it takes to the camera to take one frame
// otherwise it will cause underflow which can lead to camera stalling
// a good solution is to implement frame skipping (measure time between to loops, if this time
// is too big, just skip image processing and MJPEG sendout)
BINFO << "RaspiCamera: Entering preview thread.";
while (state.running)
{
capture->incrementHealth();
// Get YUV420 image from preview port, this is a blocking call
// If zero-copy is activated, we don't pass any buffer
capture->data_length = state.camera->getOutputData(70, nullptr);
}
BINFO << "RaspiCamera: Exiting preview thread.";
return nullptr;
}
void* record_thread(void* self)
{
kerberos::RaspiCamera * capture = (kerberos::RaspiCamera *) self;
uint8_t* data = new uint8_t[65536*4];
BINFO << "RaspiCamera: Entering record thread.";
while(state.running)
{
// Consume h264 data, this is a blocking call
int32_t datalen = state.record_encode->getOutputData(data);
if(datalen > 0 && state.recording)
{
file.write((char*) data, datalen);
file.flush();
}
}
BINFO << "RaspiCamera: Exiting record thread.";
return nullptr;
}
namespace kerberos
{
void RaspiCamera::setup(kerberos::StringMap &settings)
{
int width = std::atoi(settings.at("captures.RaspiCamera.frameWidth").c_str());
int height = std::atoi(settings.at("captures.RaspiCamera.frameHeight").c_str());
int angle = std::atoi(settings.at("captures.RaspiCamera.angle").c_str());
int delay = std::atoi(settings.at("captures.RaspiCamera.delay").c_str());
m_framerate = std::atoi(settings.at("captures.RaspiCamera.framerate").c_str());
m_sharpness = std::atoi(settings.at("captures.RaspiCamera.sharpness").c_str());
m_brightness = std::atoi(settings.at("captures.RaspiCamera.brightness").c_str());
m_contrast = std::atoi(settings.at("captures.RaspiCamera.contrast").c_str());
m_saturation = std::atoi(settings.at("captures.RaspiCamera.saturation").c_str());
// Initialize executor
tryToUpdateCapture.setAction(this, &RaspiCamera::update);
tryToUpdateCapture.setInterval("once at 1000 calls");
// Save width and height in settings.
Capture::setup(settings, width, height, angle);
setImageSize(width, height);
setRotation(angle);
setDelay(delay);
m_onBoardRecording = true;
m_hardwareMJPEGEncoding = true;
// Open camera
open();
}
void RaspiCamera::grab(){}
Image RaspiCamera::retrieve()
{
// Delay camera for some time..
usleep(m_delay*1000);
Image image;
try
{
cv::Mat mYUV(m_frameHeight + m_frameHeight / 2, m_frameWidth, CV_8UC1, (void*) data_buffer);
cvtColor(mYUV, image.getImage(), CV_YUV2BGR_I420, 3);
}
catch(cv::Exception & ex)
{
throw OpenCVException(ex.msg.c_str());
}
return image;
}
int32_t RaspiCamera::retrieveRAW(uint8_t * data)
{
try
{
if(data_length > 0)
{
// Send it to the MJPEG encoder
state.preview_encode->fillInput(200, data_buffer, data_length, false, true);
}
int length = 0;
while((length = state.preview_encode->getOutputData(data, false)) > 0)
{
mjpeg_data_length = length;
}
return mjpeg_data_length;
}
catch(cv::Exception & ex)
{
throw OpenCVException(ex.msg.c_str());
}
}
Image * RaspiCamera::takeImage()
{
// Delay camera for some time..
usleep(m_delay*1000);
Image * image = new Image();
try
{
cv::Mat mYUV(m_frameHeight + m_frameHeight / 2, m_frameWidth, CV_8UC1, (void*) data_buffer);
cvtColor(mYUV, image->getImage(), CV_YUV2BGR_I420, 3);
}
catch(cv::Exception & ex)
{
throw OpenCVException(ex.msg.c_str());
}
return image;
}
void RaspiCamera::setImageSize(int width, int height)
{
Capture::setImageSize(width, height);
// .. set size
}
void RaspiCamera::setRotation(int angle)
{
Capture::setRotation(angle);
}
void RaspiCamera::setDelay(int msec)
{
Capture::setDelay(msec);
}
void RaspiCamera::open()
{
LINFO << "Capture: Trying to open Raspberry Pi camera module.";
LINFO << "Capture: (Warning) If you see a OMX_GetHandle error, this means that you don't have a working RPi camera module attached.";
LINFO << "Capture: (Warning) You can change the capture device with the configuration files.";
// Initialize hardware
bcm_host_init();
// Create components
state.camera = new Camera(m_frameWidth, m_frameHeight, 0, false, 0, false);
state.preview_encode = new VideoEncode(16192, VideoEncode::CodingMJPEG, false, false);
state.record_encode = new VideoEncode(4096, VideoEncode::CodingAVC, false, false);
// Setup camera
state.camera->setFramerate(m_framerate);
state.camera->setBrightness(m_brightness);
state.camera->setSaturation(m_saturation);
state.camera->setContrast(m_contrast);
state.camera->setSharpness(m_sharpness);
//state.camera->setExposureControl(Camera::ExposureControlOff);
if(m_angle > 0)
{
state.camera->setRotation(m_angle);
}
// Copy preview port definition to the encoder to help it handle incoming data
Component::CopyPort( &state.camera->outputPorts()[70], &state.preview_encode->inputPorts()[200] );
// Tunnel video port to AVC encoder
state.camera->SetupTunnelVideo( state.record_encode );
// Prepare components for next step
state.camera->SetState(Component::StateIdle);
state.preview_encode->SetState(Component::StateIdle);
state.record_encode->SetState(Component::StateIdle);
// Allocate buffers that will be processed manually
state.camera->AllocateOutputBuffer(70);
state.preview_encode->AllocateInputBuffer(200);
// Start components
state.camera->SetState(Component::StateExecuting);
state.preview_encode->SetState(Component::StateExecuting);
state.record_encode->SetState(Component::StateExecuting);
// Start capturing
state.camera->SetCapturing(true);
state.running = true;
state.recording = false;
// Start threads
pthread_create(&state.preview_thid, nullptr, &preview_thread, this);
pthread_detach(state.preview_thid);
pthread_create(&state.record_thid, nullptr, &record_thread, this);
pthread_detach(state.record_thid);
LINFO << "Capture: Succesfully opened Raspberry Pi camera module.";
}
void RaspiCamera::stopThreads()
{
state.running = false;
// -------------------------
// Cancel the record thread.
pthread_join(state.record_thid, nullptr);
// -------------------------
// Cancel the preview thread.
pthread_join(state.preview_thid, nullptr);
}
RaspiCamera::~RaspiCamera()
{
Component::DestroyAll();
}
void RaspiCamera::close()
{
stopThreads();
}
void RaspiCamera::update(){}
bool RaspiCamera::isOpened()
{
return state.running;
}
void RaspiCamera::startRecord(std::string path)
{
// Restart record_encode component before recording
state.record_encode->SetState(Component::StateIdle);
state.record_encode->SetState(Component::StateExecuting);
// Create a new file to which the recording is written
file.open(path, std::ofstream::out | std::ofstream::binary);
// Write some headers
const std::map< uint32_t, uint8_t* > headers = state.record_encode->headers();
if(headers.size() > 0)
{
for(auto hdr : headers)
{
file.write((char*) hdr.second, hdr.first);
file.flush();
}
}
// Enable recording
state.recording = true;
}
void RaspiCamera::stopRecord()
{
file.close();
state.recording = false;
}
}