//-------------------------------------------------------------------------------------- // 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; } } }