calibrate

C++ implementation

node {

    int buffersize = 500; //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
    int acel_deadzone = 8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)
    int giro_deadzone = 1; //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)

    int16_t ax, ay, az, gx, gy, gz;

    int cal_state = 0;
    int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz;
    int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;

    MPU6050* mpu; // local copy

    void meansensors() {
        Number buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;
        Number i = 0;

        while (i < (buffersize + 101)) {
            mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
            if (i > 100 && i <= (buffersize + 100)) { //First 100 measures are discarded
                buff_ax = buff_ax + ax;
                buff_ay = buff_ay + ay;
                buff_az = buff_az + az;
                buff_gx = buff_gx + gx;
                buff_gy = buff_gy + gy;
                buff_gz = buff_gz + gz;
            }
            if (i == (buffersize + 100)) {
                mean_ax = buff_ax / buffersize;
                mean_ay = buff_ay / buffersize;
                mean_az = buff_az / buffersize;
                mean_gx = buff_gx / buffersize;
                mean_gy = buff_gy / buffersize;
                mean_gz = buff_gz / buffersize;
            }
            i++;
            delay(2); //Needed so we don't get repeated measures
        }
    }

    void calibration() {
        ax_offset = -mean_ax / 8;
        ay_offset = -mean_ay / 8;
        az_offset = (16384.0 - mean_az) / 8;

        gx_offset = -mean_gx / 4;
        gy_offset = -mean_gy / 4;
        gz_offset = -mean_gz / 4;

        while (1) {
            int ready = 0;
            mpu->setXAccelOffset(ax_offset);
            mpu->setYAccelOffset(ay_offset);
            mpu->setZAccelOffset(az_offset);

            mpu->setXGyroOffset(gx_offset);
            mpu->setYGyroOffset(gy_offset);
            mpu->setZGyroOffset(gz_offset);

            meansensors();

            if (abs(mean_ax) <= acel_deadzone)
                ready++;
            else
                ax_offset = ax_offset - mean_ax / acel_deadzone;
            if (abs(mean_ay) <= acel_deadzone)
                ready++;
            else
                ay_offset = ay_offset - mean_ay / acel_deadzone;
            if (abs(16384.0 - mean_az) <= acel_deadzone)
                ready++;
            else
                az_offset = az_offset + (16384.0 - mean_az) / acel_deadzone;
            if (abs(mean_gx) <= giro_deadzone)
                ready++;
            else
                gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);
            if (abs(mean_gy) <= giro_deadzone)
                ready++;
            else
                gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);
            if (abs(mean_gz) <= giro_deadzone)
                ready++;
            else
                gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);

            XOD_DEBUG_SERIAL.print("Calibrated: ");
            XOD_DEBUG_SERIAL.print(ready);
            XOD_DEBUG_SERIAL.println("/6");

            if (ready == 6)
                break;
        }
    }

    void evaluate(Context ctx) {

        mpu = getValue<input_DEV>(ctx);
        bool error = mpu->testConnection();
        if (error) {
            XOD_DEBUG_SERIAL.println("MPU6050 connection successful");
        } else {
            XOD_DEBUG_SERIAL.println("MPU6050 connection failed");
        }

        if (isInputDirty<input_DO>(ctx)) {
            if (cal_state == 0) {
                XOD_DEBUG_SERIAL.println("Reading sensors for first time");
                meansensors();
                cal_state++;
                delay(500);
            }
            if (cal_state == 1) {
                XOD_DEBUG_SERIAL.println("Calculating offsets");
                calibration();
                cal_state++;
                delay(500);
            }
            if (cal_state == 2) {
                XOD_DEBUG_SERIAL.println("Finished");
                meansensors();
                emitValue<output_GXoff>(ctx, gx_offset);
                emitValue<output_GYoff>(ctx, gy_offset);
                emitValue<output_GZoff>(ctx, gz_offset);
                emitValue<output_AXoff>(ctx, ax_offset);
                emitValue<output_AYoff>(ctx, ay_offset);
                emitValue<output_AZoff>(ctx, az_offset);
                emitValue<output_DONE>(ctx, 1);
            }
        }
    }

}