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