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