10
10
#include < sstream>
11
11
#include < regex>
12
12
#include < cctype>
13
-
13
+ # include < stdio.h >
14
14
15
15
#ifdef _WIN32
16
16
#include " PacketManager/WindowsSerial.h"
@@ -75,7 +75,6 @@ Status DeviceControllerImpl::QueryFirmwareVersion(std::string& version)
75
75
{
76
76
// clear output version string to avoid returning garbage
77
77
version.clear ();
78
-
79
78
try
80
79
{
81
80
std::string version_in_progress;
@@ -109,24 +108,30 @@ Status DeviceControllerImpl::QueryFirmwareVersion(std::string& version)
109
108
}
110
109
}
111
110
111
+ // parse lines of module versions .e.g.
112
+ // ASDISP : 18.9.24.0
113
+ // NNLED : 15.9.24.0
114
+ static const std::regex module_regex {R"( (\w+) : ([\d\.]+))" };
112
115
std::stringstream ss (buffer);
113
116
std::string line;
114
117
while (std::getline (ss, line, ' \n ' ))
115
- {
116
- static const std::regex module_regex {
117
- R"( (OPFW|NNLED|DNET|RECOG|YOLO|AS2DLR|NNLAS|NNLEDR|SPOOFS|ASDISP) : ([\d\.]+))" };
118
+ {
118
119
std::smatch match;
119
-
120
120
auto match_ok = std::regex_search (line, match, module_regex);
121
121
122
122
if (match_ok)
123
123
{
124
+ auto version_number = match[2 ].str ();
125
+ if (version_number == " 0.0.0.0" ) // ignore, unused module
126
+ {
127
+ continue ;
128
+ }
124
129
if (!version_in_progress.empty ())
125
130
version_in_progress += ' |' ;
126
131
127
132
version_in_progress += match[1 ].str ();
128
133
version_in_progress += ' :' ;
129
- version_in_progress += match[ 2 ]. str () ;
134
+ version_in_progress += version_number ;
130
135
}
131
136
}
132
137
@@ -301,7 +306,9 @@ Status DeviceControllerImpl::Ping()
301
306
}
302
307
303
308
// create a ping data packet with random data
304
- char random_data[sizeof (DataMessage::data)];
309
+ // create a ping data packet with random data
310
+ char random_data[512 ];
311
+ static_assert (sizeof (random_data) <= sizeof (DataMessage::data), " Random data size exceeds max allowed" );
305
312
Randomizer::Instance ().GenerateRandom ((unsigned char *)random_data, sizeof (random_data));
306
313
DataPacket ping_packet {MsgId::Ping, random_data, sizeof (random_data)};
307
314
@@ -388,8 +395,8 @@ Status DeviceControllerImpl::FetchLog(std::string& result)
388
395
{
389
396
result.erase (pos);
390
397
}
391
-
392
- // expect the START_OF_LOG token
398
+
399
+ // expect the START_OF_LOG token
393
400
pos = result.find (start_token);
394
401
if (pos != std::string::npos)
395
402
{
@@ -405,9 +412,9 @@ Status DeviceControllerImpl::FetchLog(std::string& result)
405
412
// make sure it ends with '\n'
406
413
if (!result.empty () && result.back () != ' \n ' )
407
414
{
408
- result.push_back (' \n ' );
415
+ result.push_back (' \n ' );
409
416
}
410
-
417
+
411
418
LOG_DEBUG (LOG_TAG, " Got %zu log bytes" , result.size ());
412
419
return Status::Ok;
413
420
}
@@ -422,4 +429,84 @@ Status DeviceControllerImpl::FetchLog(std::string& result)
422
429
return Status::Error;
423
430
}
424
431
}
432
+
433
+ Status DeviceControllerImpl::GetColorGains (int & red, int & blue)
434
+ {
435
+ const char * const cmd = PacketManager::Commands::get_color_gains;
436
+ auto send_status = _serial->SendBytes (cmd, ::strlen (cmd));
437
+ if (send_status != PacketManager::SerialStatus::Ok)
438
+ {
439
+ LOG_ERROR (LOG_TAG, " Failed sending cm command" );
440
+ return ToStatus (send_status);
441
+ }
442
+
443
+ // receive data until no more is available
444
+ constexpr size_t max_buffer_size = 128 ;
445
+ char buffer[max_buffer_size] = {0 };
446
+ for (size_t i = 0 ; i < max_buffer_size - 1 ; ++i)
447
+ {
448
+ auto status = _serial->RecvBytes (&buffer[i], 1 );
449
+ // timeout is legal for the final byte, because we do not know the expected data size
450
+ if (status == PacketManager::SerialStatus::RecvTimeout)
451
+ break ;
452
+
453
+ // other error are still not accepted
454
+ if (status != PacketManager::SerialStatus::Ok)
455
+ {
456
+ LOG_ERROR (LOG_TAG, " Failed reading serial number data" );
457
+ return ToStatus (status);
458
+ }
459
+ }
460
+
461
+ // extract red blue numbers e.g [123 511]
462
+ try
463
+ {
464
+ std::string input (buffer);
465
+ std::smatch matches;
466
+ static const std::regex pattern {R"( \[(\d+)\s(\d+)\])" };
467
+ if (std::regex_search (input, matches, pattern))
468
+ {
469
+ red = std::stoi (matches[1 ].str ());
470
+ blue = std::stoi (matches[2 ].str ());
471
+ return Status::Ok;
472
+ }
473
+ else
474
+ {
475
+ return Status::Error;
476
+ }
477
+ }
478
+ catch (const std::exception& ex)
479
+ {
480
+ LOG_EXCEPTION (LOG_TAG, ex);
481
+ return Status::Error;
482
+ }
483
+ catch (...)
484
+ {
485
+ LOG_ERROR (LOG_TAG, " Unknown exception in GetColorGains" );
486
+ return Status::Error;
487
+ }
488
+ }
489
+
490
+ Status DeviceControllerImpl::SetColorGains (int red, int blue)
491
+ {
492
+ constexpr int max_value = 511 ;
493
+ constexpr int min_value = 0 ;
494
+ if (red < min_value || red > max_value || blue < min_value || blue > max_value)
495
+ {
496
+ LOG_ERROR (LOG_TAG, " Invalid color gain values" );
497
+ return Status::Error;
498
+ }
499
+
500
+ char buf[64 ];
501
+ const char * const cmd = PacketManager::Commands::set_color_gains;
502
+ snprintf (buf, sizeof (buf), cmd, red, blue);
503
+ auto send_status = _serial->SendBytes (buf, ::strlen (buf));
504
+ if (send_status != PacketManager::SerialStatus::Ok)
505
+ {
506
+ LOG_ERROR (LOG_TAG, " Failed sending cm command" );
507
+ }
508
+ return ToStatus (send_status);
509
+ }
510
+
511
+
425
512
} // namespace RealSenseID
0 commit comments