void FlightControl::mainLoop()
{
sei();
// Get measurements
gz = AccelGyro.getRotationZ();
loopCounter++;
flightPlan();
switch(flightMode) {
case Idle: idleMode(); // Do nothing. Stationary landed.
break;
case Hover: hoverMode(); // Maintain altitude. Can go forward in this mode.
break;
case TakeOff: takeOffMode(); // Increase throttle till height is achieved
break;
case Land: landMode(); // Decrease throttle to land smoothly
break;
case Turn: turnMode(); // Execute open loop turn
break;
}
// Altitude control loop
if (altPIDenabled) {
if (alt > 200) {
alt = 20;
}
if (loopCounter % ALT_PID_FREQ_DIV == 0) {
alt = altLP.filter(AltSensor.value());
int16_t err = altSetPoint - alt;
if (err > 0)
altInput = altPID->loop(altSetPoint,alt);
else
altInput = alt2PID->loop(altSetPoint,alt);
throttle = THROTTLE_SET + altInput;
throttle = (throttle>THROTTLE_MAX) ? THROTTLE_MAX:throttle;
throttle = (throttle<THROTTLE_MIN) ? THROTTLE_MIN:throttle;
}
}
// Yaw Control loop
if (yawPIDenabled) {
yawInput = yawPID->loop(gyroSetPoint,gz);
topRotDuty = throttle - yawInput;
botRotDuty = throttle + yawInput;
}
// Set motor duty cycles
setTopRotorDuty(topRotDuty);
setBottomRotorDuty(botRotDuty);
}
inline void FlightControl::landMode() {
throttle = throttle - THROTTLE_LAND_DELTA;
throttle = (throttle<DUTY_MIN) ? DUTY_MIN:throttle;
alt = altLP.filter(AltSensor.value());
if (throttle==DUTY_MIN) {
idle();
}
if (alt < 25 || alt > 200) {
idle();
}
if (throttle < 300)
yawPIDenabled = false;
}
void sensorEvent(MASensor a) {
// If the type of sensor data received is from the accelerometer
if (a.type == SENSOR_TYPE_ACCELEROMETER) {
// Filter the accelerometer gravity vector.
Vector3 f = mFilter.filter(Vector3(a.values[0], a.values[1], a.values[2]));
// And calculate the rotations. We don't pass the compass angle, just the
// accelerometer gravity vector.
mRotation = convertAccelerometerAndCompassDataToRadians(f, 0.0f);
// Set the rotation.
mRenderer->setRotation(
convertRadiansToDegrees(mRotation.x),
convertRadiansToDegrees(mRotation.y),
convertRadiansToDegrees(mRotation.z)
);
}
}
KeyDetectionResult KeyFinder::findKey(const AudioData& originalAudio, const Parameters& params){
KeyDetectionResult result;
AudioData* workingAudio = new AudioData(originalAudio);
workingAudio->reduceToMono();
// TODO: there is presumably some good maths to determine filter frequencies
float lpfCutoff = params.getLastFrequency() * 1.05;
float dsCutoff = params.getLastFrequency() * 1.10;
unsigned int downsampleFactor = (int)floor( workingAudio->getFrameRate() / 2 / dsCutoff );
// get filter
LowPassFilter* lpf = lpfFactory.getLowPassFilter(160, workingAudio->getFrameRate(), lpfCutoff, 2048);
// feeding in the downsampleFactor for a shortcut
lpf->filter(workingAudio, downsampleFactor);
// note we don't delete the LPF; it's stored in the factory for reuse
Downsampler ds;
ds.downsample(workingAudio, downsampleFactor);
SpectrumAnalyser* sa = new SpectrumAnalyser(workingAudio->getFrameRate(), params, &ctFactory);
// run spectral analysis
Chromagram* ch = sa->chromagram(workingAudio);
delete workingAudio;
delete sa;
// reduce chromagram
ch->reduceTuningBins(params);
result.fullChromagram = Chromagram(*ch);
ch->reduceToOneOctave(params);
result.oneOctaveChromagram = Chromagram(*ch);
// get harmonic change signal
Segmentation* segmenter = Segmentation::getSegmentation(params);
result.harmonicChangeSignal = segmenter->getRateOfChange(*ch, params);
// get track segmentation
std::vector<unsigned int> segmentBoundaries = segmenter->getSegments(result.harmonicChangeSignal, params);
segmentBoundaries.push_back(ch->getHops()); // sentinel
delete segmenter;
// get key estimates for each segment
KeyClassifier hc(params);
std::vector<float> keyWeights(24); // TODO: not ideal using int cast of key_t enum. Hash?
for (int s = 0; s < (signed)segmentBoundaries.size()-1; s++){
KeyDetectionSegment segment;
segment.firstHop = segmentBoundaries[s];
segment.lastHop = segmentBoundaries[s+1] - 1;
// collapse segment's time dimension
std::vector<float> segmentChroma(ch->getBins());
for (unsigned int hop = segment.firstHop; hop <= segment.lastHop; hop++) {
for (unsigned int bin = 0; bin < ch->getBins(); bin++) {
float value = ch->getMagnitude(hop, bin);
segmentChroma[bin] += value;
segment.energy += value;
}
}
segment.key = hc.classify(segmentChroma);
if(segment.key != SILENCE){
keyWeights[segment.key] += segment.energy;
}
result.segments.push_back(segment);
}
delete ch;
// get global key
result.globalKeyEstimate = SILENCE;
float mostCommonKeyWeight = 0.0;
for (int k = 0; k < (signed)keyWeights.size(); k++){
if(keyWeights[k] > mostCommonKeyWeight){
mostCommonKeyWeight = keyWeights[k];
result.globalKeyEstimate = (key_t)k;
}
}
return result;
}