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; }
/** * 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"); }