Magnetometer calibration

#define FROM_MGAUSS_TO_UT50                         (0.1f/50.0f)

static TaskHandle_t sensor_values_task_handle;

bool calEnabled = true;
uint32_t local_timestamp = 50;

int maxQuality = 0;

void print_sensor_values (void *pvParameters)
{
    while (true) {
        if (calEnabled) {
            MMC_Input_t data_in;
            MMC_Output_t data_out;

            if (magAvailable()) {
                readMag();

                data_in.TimeStamp = local_timestamp;
                data_in.Mag[0] = calcMag(mx) * 1000 * FROM_MGAUSS_TO_UT50;
                data_in.Mag[1] = calcMag(my) * 1000 * FROM_MGAUSS_TO_UT50;
                data_in.Mag[2] = calcMag(mz) * 1000 * FROM_MGAUSS_TO_UT50;

                MotionMC_Update(&data_in);

                MotionMC_GetCalParams(&data_out);

                if (maxQuality < data_out.CalQuality) {
                    maxQuality = data_out.CalQuality;
                    calEnabled = false;
                }
            }
        } else {
            // At least poor quality
        }

        local_timestamp += 50;
        vTaskDelay(50); // 20 Hz
    }
}

int main(void)
{
    /* Set all the default settings for the sensor. */
    setup_LSM9DS1();

    /*Create the I2C connection and start to get data from the sensor. */
    begin_LSM9DS1();

    char * temp = (char *)malloc(sizeof(char) * 35);
    uint8_t res = MotionMC_GetLibVersion(temp);

    MotionMC_Initialize(50, 1);

    BaseType_t xReturned = xTaskCreate(print_sensor_values, "SNSR", 1400, NULL, 2, &sensor_values_task_handle);

    // Start FreeRTOS scheduler.
    vTaskStartScheduler();
}