int processData(sensor_data *data) { printf("HI, we're in processData\n"); //This is broken, serves as a delay static int start = SS_NUM; static int counter = 0; static bool biasSet = false; bool ss_flag = false; // initial data collection if (start) { start--; return 0; } // need to establish bias first if(!biasSet) { if (checkIfSS()) { radians_curr = getAbsPos(); gyro_bias = getGyroBias(); acc_bias = getAccBias(); ss_flag = true; biasSet = true; } else { return 0; } } // periodically check if we are stationary if (counter > SENSOR_FREQ/SS_CHECK_FREQ) { counter = 0; if(checkIfSS()){ radians_curr = getAbsPos(); gyro_bias = getGyroBias(); acc_bias = getAccBias(); ss_flag = true; } } // update current tilt if (!ss_flag) { radians_curr.roll += (data->rotX)/SENSOR_FREQ; radians_curr.pitch += (data->rotY)/SENSOR_FREQ; } counter++; return 1; }
// Detect takeoff for optical flow navigation void NavEKF2_core::detectOptFlowTakeoff(void) { if (motorsArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) { const AP_InertialSensor &ins = _ahrs->get_ins(); Vector3f angRateVec; Vector3f gyroBias; getGyroBias(gyroBias); bool dual_ins = ins.get_gyro_health(0) && ins.get_gyro_health(1); if (dual_ins) { angRateVec = (ins.get_gyro(0) + ins.get_gyro(1)) * 0.5f - gyroBias; } else { angRateVec = ins.get_gyro() - gyroBias; } takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rngAtStartOfFlight + 0.1f))); } }
// Detect takeoff for optical flow navigation void NavEKF2_core::detectOptFlowTakeoff(void) { if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) { // we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff const AP_InertialSensor &ins = _ahrs->get_ins(); Vector3f angRateVec; Vector3f gyroBias; getGyroBias(gyroBias); bool dual_ins = ins.get_gyro_health(0) && ins.get_gyro_health(1); if (dual_ins) { angRateVec = (ins.get_gyro(0) + ins.get_gyro(1)) * 0.5f - gyroBias; } else { angRateVec = ins.get_gyro() - gyroBias; } takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rangeDataNew.rng > (rngAtStartOfFlight + 0.1f))); } else if (onGround) { // we are confidently on the ground so set the takeoff detected status to false takeOffDetected = false; } }