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