//--------------------------------------------------------------------------------------
// Func: ResetCenter
// Desc: Utility for resetting tracker center. This is only called during tracker
// startup. Button press resets are handled during filtering. (Needs refactor)
//--------------------------------------------------------------------------------------
void ResetCenter()
{
resetValues = 0;
// Not sure what Dennis is doing here. Giving it
// time to stabilize, since this is called at setup time?
for (unsigned char k = 0; k < 250; k++)
{
UpdateSensors();
GyroCalc();
AccelCalc();
MagCalc();
FilterSensorData();
}
tiltStart = accAngle[0];
panStart = magAngle[2];
rollStart = accAngle[1];
UpdateSensors();
GyroCalc();
AccelCalc();
MagCalc();
FilterSensorData();
panAngle = magAngle[2];
tiltAngle = accAngle[0];
rollAngle = accAngle[1];
TrackerStarted = 1;
}
//--------------------------------------------------------------------------------------
// Func: Filter
// Desc: Filters / merges sensor data.
//--------------------------------------------------------------------------------------
void FilterSensorData()
{
int temp = 0;
// Used to set initial values.
if (resetValues == 1)
{
resetValues = 0;
tiltStart = 0;
panStart = 0;
rollStart = 0;
UpdateSensors();
GyroCalc();
AccelCalc();
MagCalc();
panAngle = 0;
tiltStart = accAngle[0];
panStart = magAngle[2];
rollStart = accAngle[1];
}
// Simple FilterSensorData, uses mainly gyro-data, but uses accelerometer to compensate for drift
rollAngle = (rollAngle + ((gyroRaw[0] - gyroOff[0]) * cos((tiltAngle - 90) / 57.3) + (gyroRaw[2] - gyroOff[2]) * sin((tiltAngle - 90) / 57.3)) / (SAMPLERATE * SCALING_FACTOR)) * gyroWeightTiltRoll + accAngle[1] * (1 - gyroWeightTiltRoll);
tiltAngle = (tiltAngle + ((gyroRaw[1] - gyroOff[1]) * cos((rollAngle - 90) / 57.3) + (gyroRaw[2] - gyroOff[2]) * sin((rollAngle - 90) / 57.3) * -1) / (SAMPLERATE * SCALING_FACTOR)) * gyroWeightTiltRoll + accAngle[0] * (1 - gyroWeightTiltRoll);
panAngle = (panAngle + ((gyroRaw[2] - gyroOff[2]) * cos((tiltAngle - 90) / 57.3) + (((gyroRaw[0] - gyroOff[0]) * -1) * (sin((tiltAngle - 90) / 57.3)) ) + ( ((gyroRaw[1] - gyroOff[1]) * 1) * (sin((rollAngle - 90) / 57.3)))) / (SAMPLERATE * SCALING_FACTOR)) * GyroWeightPan + magAngle[2] * (1 - GyroWeightPan);
if (TrackerStarted)
{
// All low-pass filters
tiltAngleLP = tiltAngle * tiltRollBeta + (1 - tiltRollBeta) * lastTiltAngle;
lastTiltAngle = tiltAngleLP;
rollAngleLP = rollAngle * tiltRollBeta + (1 - tiltRollBeta) * lastRollAngle;
lastRollAngle = rollAngleLP;
panAngleLP = panAngle * panBeta + (1 - panBeta) * lastPanAngle;
lastPanAngle = panAngleLP;
float panAngleTemp = panAngleLP * panInverse * panFactor;
if ( (panAngleTemp > -panMinPulse) && (panAngleTemp < panMaxPulse) )
{
temp = servoPanCenter + panAngleTemp;
channel_value[htChannels[0]] = (int)temp;
}
float tiltAngleTemp = (tiltAngleLP - tiltStart) * tiltInverse * tiltFactor;
if ( (tiltAngleTemp > -tiltMinPulse) && (tiltAngleTemp < tiltMaxPulse) )
{
temp = servoTiltCenter + tiltAngleTemp;
channel_value[htChannels[1]] = temp;
}
float rollAngleTemp = (rollAngleLP - rollStart) * rollInverse * rollFactor;
if ( (rollAngleTemp > -rollMinPulse) && (rollAngleTemp < rollMaxPulse) )
{
temp = servoRollCenter + rollAngleTemp;
channel_value[htChannels[2]] = temp;
}
}
}
//--------------------------------------------------------------------------------------
// Func: Filter
// Desc: Filters / merges sensor data.
//--------------------------------------------------------------------------------------
void FilterSensorData()
{
signed int temp = 0;
// Used to set initial values.
if (resetValues == 1)
{
#if FATSHARK_HT_MODULE
digitalWrite(BUZZER, HIGH);
#endif
resetValues = 0;
tiltStart = 0;
panStart = 0;
rollStart = 0;
UpdateSensors();
GyroCalc();
AccelCalc();
MagCalc();
panAngle = 0;
tiltStart = accAngle[0];
panStart = magAngle[2];
rollStart = accAngle[1];
#if FATSHARK_HT_MODULE
digitalWrite(BUZZER, LOW);
#endif
}
// Simple FilterSensorData, uses mainly gyro-data, but uses accelerometer to compensate for drift
rollAngle = (rollAngle + ((gyroRaw[0] - gyroOff[0]) * cos((tiltAngle - 90) / 57.3) + (gyroRaw[2] - gyroOff[2]) * sin((tiltAngle - 90) / 57.3)) / (SAMPLERATE * SCALING_FACTOR)) * gyroWeightTiltRoll + accAngle[1] * (1 - gyroWeightTiltRoll);
tiltAngle = (tiltAngle + ((gyroRaw[1] - gyroOff[1]) * cos((rollAngle - 90) / 57.3) + (gyroRaw[2] - gyroOff[2]) * sin((rollAngle - 90) / 57.3) * -1) / (SAMPLERATE * SCALING_FACTOR)) * gyroWeightTiltRoll + accAngle[0] * (1 - gyroWeightTiltRoll);
panAngle = (panAngle + ((gyroRaw[2] - gyroOff[2]) * cos((tiltAngle - 90) / 57.3) + (((gyroRaw[0] - gyroOff[0]) * -1) * (sin((tiltAngle - 90) / 57.3)) ) + ( ((gyroRaw[1] - gyroOff[1]) * 1) * (sin((rollAngle - 90) / 57.3)))) / (SAMPLERATE * SCALING_FACTOR)) * GyroWeightPan + magAngle[2] * (1 - GyroWeightPan);
if (TrackerStarted)
{
// All low-pass filters
tiltAngleLP = tiltAngle * tiltRollBeta + (1 - tiltRollBeta) * lastTiltAngle;
lastTiltAngle = tiltAngleLP;
rollAngleLP = rollAngle * tiltRollBeta + (1 - tiltRollBeta) * lastRollAngle;
lastRollAngle = rollAngleLP;
panAngleLP = panAngle * panBeta + (1 - panBeta) * lastPanAngle;
lastPanAngle = panAngleLP;
panAngleTemp = panAngleLP * panInverse * panFactor;
/* temp=constrain((servoPanCenter+panAngleTemp),panMinPulse,panMaxPulse);
channel_value[htChannels[0]]=(int)temp;
*/
temp = servoPanCenter + (int)panAngleTemp;
if ( (temp > panMinPulse) && (temp < panMaxPulse) )
{
channel_value[htChannels[0]] = temp;
}
float tiltAngleTemp = (tiltAngleLP - tiltStart) * tiltInverse * tiltFactor;
//temp=constrain((servoTiltCenter+tiltAngleTemp),tiltMinPulse,tiltMaxPulse);
//channel_value[htChannels[1]]=(int)temp;
temp = servoTiltCenter + (int)tiltAngleTemp;
if ( (temp > tiltMinPulse) && (temp < tiltMaxPulse) )
{
channel_value[htChannels[1]] = temp;
}
float rollAngleTemp = (rollAngleLP - rollStart) * rollInverse * rollFactor;
//temp=constrain((servoRollCenter+rollAngleTemp),rollMinPulse,rollMaxPulse);
//channel_value[htChannels[2]]=(int)temp;
temp = servoRollCenter + (int)rollAngleTemp;
if ( (temp > rollMinPulse) && (temp < rollMaxPulse) )
{
channel_value[htChannels[2]] = temp;
}
}
}
