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; } }