void CommandProcess()
{
// Read incoming control messages
if (Serial_available() >= 2)
{
char start=Serial_read();
if (start == '@') {// Start of new control message
int command = Serial_read(); // Commands
if (command == 'h') {//Hook AHRS Stack Device
// Read ID
char id[2];
id[0] = GetChar();
id[1] = GetChar();
// Reply with synch message
printf("@HOOK");
Serial_write(id, 2);
}
else if (command == 'v') {//Check Version
CheckVersion();
}
else if (command == 'c') {// A 'c'calibration command
SensorCalibration();
}
else if (command == 'b') {// 'b'lock erase flash
FlashControl();
}
else if (command == 'p') {// Set 'p'id command
SetPID();
}
else if (command == 'm') {// Set report 'm'ode
char mode = GetChar();
if (mode == 'e') {// Report AHRS by 'e'uler angle
report_mode = REPORT_AHRS_EULER;
}
else if (mode == 'q') {// Report // Report AHRS by 'q'quaternion
report_mode = REPORT_AHRS_QUATERNION;
}
else if (mode == 'r') {// Report sensor 'r'aw data
report_mode = REPORT_SENSORS_RAW;
}
else if (mode == 'c') {// Report sensor 'c'alibrated data
report_mode = REPORT_SENSORS_CALIBRATED;
}
else if (mode == 'm') {// Report 'm'otor power distribution
report_mode = REPORT_MOTOR_POWER;
}
else if (mode == 'v') {// Report 'v'elocity
report_mode = REPORT_VELOCITY;
}
else if (mode == 's') {// Report 's'tatus
report_status();
}
else if (mode == 'p') {// Report controller 'p'id
char type = GetChar();
if (type == 'p') // 'p'id
report_mode = REPORT_PID;
else if (type == 'r') //'r'ate pid
report_mode = REPORT_RATE_PID;
else if (type == 'a') //'a'ltitude hold pid
report_mode = REPORT_ALTHOLD_PID;
}
}
else if (command == 'f') {// Set report 'f'ormat
char format = GetChar();
if (format == 'b') {// Report 'b'inary format
report_format = REPORT_FORMAT_BINARY;
}
else if (format == 't') {// Report 't'ext format
report_format = REPORT_FORMAT_TEXT;
}
}
else if (command == 's') {// 's'tream output control
char mode = GetChar();
if (mode == 's') {// 's'tart stream
stream_mode = STREAM_START;
}
else if (mode == 'p') {// 'p'ause stream
stream_mode = STREAM_PAUSE;
}
else if (mode == 't') {// 't'oggle stream
if(stream_mode==STREAM_START)
stream_mode = STREAM_PAUSE;
else
stream_mode = STREAM_START;
}
}
}
else {
if (report_format == REPORT_FORMAT_TEXT) {
printf("Unknown command.\n");
}
} // Skip character
}
}
DataWrapper::DataWrapper(std::string istrm, std::string sstrm, std::string cfg, std::string lname)
{
m_ok = true;
string cam_spec_name = string(CONFIG_DIR) + string("CameraSpecs.cfg");
string sen_calib_name = string(CONFIG_DIR) + string("SensorCalibration.cfg");
debug << "opening camera config: " << cam_spec_name << std::endl;
if( ! camera_data.LoadFromConfigFile(cam_spec_name.c_str())) {
errorlog << "DataWrapper::DataWrapper() - failed to load camera specifications: " << cam_spec_name << std::endl;
m_ok = false;
}
debug << "opening sensor calibration config: " << sen_calib_name << std::endl;
if( ! sensor_calibration.ReadSettings(sen_calib_name)) {
errorlog << "DataWrapper::DataWrapper() - failed to load sensor calibration: " << sen_calib_name << ". Using default values." << std::endl;
sensor_calibration = SensorCalibration();
}
kinematics_horizon.setLine(0, 1, 0);
numFramesDropped = numFramesProcessed = 0;
streamname = istrm;
debug << "openning image stream: " << streamname << std::endl;
imagestrm.open(streamname.c_str());
using_sensors = !sstrm.empty();
sensorstreamname = sstrm;
if(m_ok && using_sensors) {
debug << "openning sensor stream: " << sensorstreamname << std::endl;
sensorstrm.open(sensorstreamname.c_str());
if(!sensorstrm.is_open()) {
std::cerr << "Error : Failed to read sensors from: " << sensorstreamname << " defaulting to sensors off." << std::endl;
using_sensors = false;
}
}
if(!imagestrm.is_open()) {
errorlog << "DataWrapper::DataWrapper() - failed to load stream: " << streamname << std::endl;
m_ok = false;
}
debug << "config: " << cfg << std::endl;
VisionConstants::loadFromFile(cfg);
if(!loadLUTFromFile(lname)){
errorlog << "DataWrapper::DataWrapper() - failed to load LUT: " << lname << std::endl;
m_ok = false;
}
gpio = (wiringPiSetupSys() != -1);
if(gpio) {
system("gpio -g mode 17 out");
system("gpio -g mode 18 out");
system("gpio -g mode 22 out");
system("gpio -g mode 23 out");
system("gpio export 17 out");
system("gpio export 18 out");
system("gpio export 22 out");
system("gpio export 23 out");
}
else {
std::cerr << "Failed to setup wiringPi - GPIO pins unavailable" << std::endl;
}
}
