Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <stageprofi.h>
00019 #include <ros/ros.h>
00020 #include <boost/cstdint.hpp>
00021 #include <boost/integer.hpp>
00022 #include <algorithm>
00023
00024 StageProfi::StageProfi(SerialIO* serialIO, unsigned int leds, int led_offset)
00025 {
00026 _serialIO = serialIO;
00027 _num_leds = leds;
00028 _led_offset = led_offset;
00029 }
00030
00031 StageProfi::~StageProfi()
00032 {
00033 }
00034
00035 bool StageProfi::init()
00036 {
00037 bool ret = false;
00038 const char init_data[] = { 'C', '?' };
00039 int init_len = sizeof(init_data) / sizeof(init_data[0]);
00040
00041 char init_buf[2];
00042
00043 memcpy(&init_buf, init_data, init_len);
00044 if (_serialIO->sendData(init_buf, 2) == 2)
00045 {
00046 std::string rec;
00047 if(_serialIO->readData(rec, 1) == 1)
00048 ret = true;
00049 }
00050 else
00051 ROS_ERROR("Sending init command to stageprofi failed");
00052 return ret;
00053 }
00054
00055 void StageProfi::setColor(color::rgba color)
00056 {
00057 color::rgba color_tmp = color;
00058
00059 color_tmp.r *= color.a;
00060 color_tmp.g *= color.a;
00061 color_tmp.b *= color.a;
00062
00063 color_tmp.r = fabs(color_tmp.r * 255);
00064 color_tmp.g = fabs(color_tmp.g * 255);
00065 color_tmp.b = fabs(color_tmp.b * 255);
00066
00067 unsigned int num_channels = _num_leds * 3;
00068 char channelbuffer[num_channels];
00069
00070 for (int i = 0; i < _num_leds; i++)
00071 {
00072 channelbuffer[i * 3] = (int) color_tmp.r;
00073 channelbuffer[i * 3 + 1] = (int) color_tmp.g;
00074 channelbuffer[i * 3 + 2] = (int) color_tmp.b;
00075 }
00076
00077 uint16_t index = 0;
00078 while (index < num_channels)
00079 {
00080 unsigned int size = std::min((unsigned int) MAX_CHANNELS,
00081 num_channels - index);
00082 if (sendDMX(index, channelbuffer + index, size))
00083 index += size;
00084 else
00085 {
00086 ROS_ERROR("Sending color to stageprofi failed");
00087 this->recover();
00088 break;
00089 }
00090 }
00091 m_sigColorSet(color);
00092 }
00093
00094 void StageProfi::setColorMulti(std::vector<color::rgba> &colors)
00095 {
00096 color::rgba color_tmp;
00097 unsigned int num_channels = _num_leds * 3;
00098 char channelbuffer[num_channels];
00099
00100 std::vector<color::rgba> color_out = colors;
00101
00102 if(_led_offset != 0)
00103 {
00104 if(_led_offset > 0)
00105 std::rotate(color_out.begin(), color_out.begin()+_led_offset, color_out.end());
00106 else
00107 std::rotate(color_out.begin(), color_out.begin()+color_out.size()+_led_offset, color_out.end());
00108 }
00109
00110 for (int i = 0; i < _num_leds || i < colors.size(); i++)
00111 {
00112 color_tmp.r = color_out[i].r * color_out[i].a;
00113 color_tmp.g = color_out[i].g * color_out[i].a;
00114 color_tmp.b = color_out[i].b * color_out[i].a;
00115
00116 color_tmp.r = fabs(color_tmp.r * 255);
00117 color_tmp.g = fabs(color_tmp.g * 255);
00118 color_tmp.b = fabs(color_tmp.b * 255);
00119
00120 channelbuffer[i * 3] = (int) color_tmp.r;
00121 channelbuffer[i * 3 + 1] = (int) color_tmp.g;
00122 channelbuffer[i * 3 + 2] = (int) color_tmp.b;
00123 }
00124
00125 uint16_t index = 0;
00126 while (index < num_channels)
00127 {
00128 unsigned int size = std::min((unsigned int) MAX_CHANNELS,
00129 num_channels - index);
00130 if (sendDMX(index, channelbuffer + index, size))
00131 index += size;
00132 else
00133 {
00134 ROS_ERROR("Sending color to stageprofi failed");
00135 this->recover();
00136 break;
00137 }
00138 }
00139 m_sigColorSet(colors[0]);
00140 }
00141
00142 bool StageProfi::sendDMX(uint16_t start, const char* buf, unsigned int length)
00143 {
00144 bool ret = false;
00145 std::string recv;
00146 char msg[MAX_CHANNELS + HEADER_SIZE];
00147
00148 unsigned int len = std::min((unsigned int) MAX_CHANNELS, length);
00149
00150 msg[0] = 0xFF;
00151 msg[1] = start & 0xFF;
00152 msg[2] = (start >> 8) & 0xFF;
00153 msg[3] = len;
00154
00155 memcpy(msg + HEADER_SIZE, buf, len);
00156
00157 const int bytes_to_send = len + HEADER_SIZE;
00158
00159
00160 ret = _serialIO->sendData(msg, bytes_to_send) == bytes_to_send;
00161
00162 ret &= _serialIO->readData(recv, 1) == 1;
00163 return ret;
00164 }
00165
00166 bool StageProfi::recover()
00167 {
00168 ROS_WARN("Trying to recover stagedriver");
00169 if(_serialIO->recover() && this->init())
00170 return true;
00171 else
00172 return false;
00173 }