public CamData GetCamData() { CamData data = new CamData(); data.setIntProp(_servoPos[Constants.PITCH_SERVO_ADDR], CamData.SERVO_PITCH); data.setIntProp(_servoPos[Constants.YAW_SERVO_ADDR], CamData.SERVO_YAW); data.setDoubleProp(_eulerAngles[0], CamData.EULER_ROLL); data.setDoubleProp(_eulerAngles[1], CamData.EULER_PITCH); data.setDoubleProp(_eulerAngles[2], CamData.EULER_YAW); if (_bStabilizationChanged) { data.setBoolProp(_bStabilizationActive, CamData.STAB_ACTIVE); _bStabilizationChanged = false; } return data; }
public void SetCamData(CamData data) { if (data.propChanged[CamData.SERVO_PITCH]) { _servoPos[Constants.PITCH_SERVO_ADDR] = data.servoPitch; _servoChanged[Constants.PITCH_SERVO_ADDR] = true; Logger.instance().log("Servo pitch set to " + data.servoPitch); } if(data.propChanged[CamData.SERVO_YAW]) { _servoPos[Constants.YAW_SERVO_ADDR] = data.servoYaw; _servoChanged[Constants.YAW_SERVO_ADDR] = true; Logger.instance().log("Servo yaw set to " + data.servoYaw); } if (data.propChanged[CamData.STAB_KI]) { _ki = data.ki; _kiChanged = true; Logger.instance().log("Ki set to " + data.ki); } if(data.propChanged[CamData.STAB_KP]) { _kp = data.kp; _kpChanged = true; Logger.instance().log("Kp set to " + data.kp); } if(data.propChanged[CamData.STAB_ACTIVE] && data.stabActive != _bStabilizationActive) { _bStabilizationActive = data.stabActive; Logger.instance().log("Stabilization " + (data.stabActive ? "activated" : "deactivated")); } if (data.propChanged[CamData.CAM_FPS]) { _frameRate = data.fps; _frameRateChanged = true; Logger.instance().log("Frame rate set to " + data.fps); } if (data.propChanged[CamData.CAM_EXP_TIME]) { _expTime = data.expTime; _expTimeChanged = true; Logger.instance().log("Exposure time set to " + data.expTime); } }
/** * Update the pitch, yaw and roll and send angle modification if necessary */ protected override void work() { try { // init Ki and Kp with values from server _Ki = _satService.getKi(); _Kp = _satService.getKp(); _fps = _goalFps = _satService.getFrameRate(); _expTime = _goalExpTime = _satService.getExposureTime(); // init stabilization mode _stabilizeMode = _satService.GetStablizationActive(); Console.WriteLine("Write console orientation fetcher"); while (_go && IsAlive()) { CamData data = _satService.GetCamData(); // update euler angles double[] euler = new double[] { data.eulerPitch, data.eulerYaw, data.eulerRoll }; lock (this) { _pitch.push(euler[0]); _yaw.push(euler[1]); _roll.push(euler[2]); } // update servo angles _servoPitch = data.servoPitch; _servoYaw = data.servoYaw; bool goal = _stabilizeModeGoal, curr = _stabilizeMode, server = data.stabActive; updateStabMode(goal, curr, server); // prepare CamData object to return to the servo with new orders CamData orders = new CamData(); // set stabilize mode if (sendStabOrder(goal, curr, server)) orders.setBoolProp(goal, CamData.STAB_ACTIVE); // if servo angles invalid : send request for changing them if (Math.Abs(_servoPitch - _goal_pitch) > SERVO_EQUAL_THRESHOLD && !_stabilizeMode) orders.setIntProp(_goal_pitch, CamData.SERVO_PITCH); if (Math.Abs(_servoYaw - _goal_yaw) > SERVO_EQUAL_THRESHOLD && !_stabilizeMode) orders.setIntProp(_goal_yaw, CamData.SERVO_YAW); if (Math.Abs(_KiGoal - _Ki) > EQUAL_THRESHOLD) { _Ki = _KiGoal; orders.setDoubleProp(_Ki, CamData.STAB_KI); } if (Math.Abs(_KpGoal - _Kp) > EQUAL_THRESHOLD) { _Kp = _KpGoal; orders.setDoubleProp(_Kp, CamData.STAB_KP); } if (Math.Abs(_goalFps - _fps) > EQUAL_THRESHOLD) { _fps = _goalFps; orders.setDoubleProp(_fps, CamData.CAM_FPS); } if (Math.Abs(_goalExpTime - _expTime) > EQUAL_THRESHOLD) { _expTime = _goalExpTime; orders.setDoubleProp(_expTime, CamData.CAM_EXP_TIME); } if(orders.hasChanged()) // send only if there are changes _satService.SetCamData(orders); Thread.Sleep(200); } } catch(Exception e) { Console.Error.Write("Error occurred in OrientationFetcher : {0}", e.Message); } Console.WriteLine("Leave orientation fetcher"); }