31 void CRaePID::loadConfig_sensorSpecific(
37 configSource.
read_string(iniSection,
"COM_port_PID",
"COM1",
true);
40 configSource.
read_string(iniSection,
"COM_port_PID",
"/dev/tty0",
true);
43 com_bauds = configSource.
read_int(iniSection,
"baudRate", 9600,
false);
45 pose_x = configSource.
read_float(iniSection,
"pose_x", 0,
true);
46 pose_y = configSource.
read_float(iniSection,
"pose_y", 0,
true);
47 pose_z = configSource.
read_float(iniSection,
"pose_z", 0,
true);
48 pose_roll = configSource.
read_float(iniSection,
"pose_roll", 0,
true);
49 pose_pitch = configSource.
read_float(iniSection,
"pose_pitch", 0,
true);
50 pose_yaw = configSource.
read_float(iniSection,
"pose_yaw", 0,
true);
56 bool CRaePID::tryToOpenTheCOM()
58 if (COM.isOpen())
return true;
61 cout <<
"[CRaePID] Opening " << com_port <<
" @ " << com_bauds << endl;
67 COM.setConfig(com_bauds, 0, 8, 1);
69 COM.setTimeouts(50, 1, 100, 1, 20);
76 catch (std::exception& e)
78 std::cerr <<
"[CRaePID::tryToOpenTheCOM] Error opening or configuring "
87 std::cerr <<
"[CRaePID::tryToOpenTheCOM] Error opening or configuring "
98 void CRaePID::doProcess()
101 if (!tryToOpenTheCOM())
107 bool have_reading =
false;
109 bool time_out =
false;
111 while (!have_reading)
118 power_reading = COM.ReadString(500, &time_out);
123 std::this_thread::sleep_for(10ms);
133 const float readnum = atof(power_reading.c_str());
134 const float val_ppm = readnum / 1000;
154 cout <<
"Firmware version: " << endl;
156 size_t B_written = COM.Write(
"F", 1);
160 bool time_out =
false;
161 std::string s_read = COM.ReadString(2000, &time_out);
162 if (time_out) s_read =
"Time_out";
173 return COM.ReadString();
183 return COM.ReadString();
193 return COM.ReadString();
196 bool CRaePID::switchPower()
204 reading = COM.ReadString();
206 if (strcmp(reading.c_str(),
"Sleep...") == 0)
220 reading = COM.ReadString();
225 std::stringstream readings_str(reading);
229 std::istream_iterator<std::string> it(readings_str);
230 std::istream_iterator<std::string> endit;
231 std::vector<std::string> measurements_text(it, endit);
237 for (
size_t k = 0; k < measurements_text.size(); k++)
239 const float readnum = atof(measurements_text[k].c_str());
240 const float val_ppm = readnum / 1000.f;
261 reading = COM.ReadString();
266 std::stringstream readings_str(reading);
270 std::istream_iterator<std::string> it(readings_str);
271 std::istream_iterator<std::string> endit;
272 std::vector<std::string> errors_text(it, endit);
275 if ((strcmp(errors_text[0].c_str(),
"0") == 0) &&
276 (strcmp(errors_text[1].c_str(),
"0") == 0))
285 errorString = reading;
290 void CRaePID::getLimits(
float&
min,
float& max)
298 reading = COM.ReadString();
303 std::stringstream readings_str(reading);
307 std::istream_iterator<std::string> it(readings_str);
308 std::istream_iterator<std::string> endit;
309 std::vector<std::string> readings_text(it, endit);
312 max = atof(readings_text[0].c_str());
313 min = atof(readings_text[1].c_str());