Ejemplo n.º 1
0
void kalman_gravity_demo_lambda()
{
    // initialize the filter
    kalman_gravity_init();

    mf16 *x = kalman_get_state_vector(&kf);
    mf16 *z = kalman_get_observation_vector(&kfm);

    // forcibly increase uncertainty in every prediction step by ~20% (1/lambda^2)
    const fix16_t lambda = F16(0.9);

    // filter!
    uint_fast16_t i;
    for (i = 0; i < MEAS_COUNT; ++i)
    {
        // prediction.
        kalman_predict_tuned(&kf, lambda);

        // measure ...
        fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]);
        matrix_set(z, 0, 0, measurement);

        // update
        kalman_correct(&kf, &kfm);
    }

    // fetch estimated g
    const fix16_t g_estimated = x->data[2][0];
    const float value = fix16_to_float(g_estimated);
    assert(value > 9.7 && value < 10);
}
Ejemplo n.º 2
0
void kalman_gravity_demo()
{
    // initialize the filter
    kalman_gravity_init();

    mf16 *x = kalman_get_state_vector_uc(&kf);
    mf16 *z = kalman_get_observation_vector(&kfm);
    
    // filter!
    uint_fast16_t i;
    for (i = 0; i < MEAS_COUNT; ++i)
    {
        // prediction.
        kalman_predict_uc(&kf);

        // measure ...
        fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]);
        matrix_set(z, 0, 0, measurement);

        // update
        kalman_correct_uc(&kf, &kfm);
    }

    // fetch estimated g
    const fix16_t g_estimated = x->data[2][0];
    const float value = fix16_to_float(g_estimated);
    assert(value > 9.7 && value < 10);
}
Ejemplo n.º 3
0
int main(int argc, char** argv) {
    printf("libfixmath test tool\n");

    hiclock_init();

	uintptr_t args = (1 <<  8);
	uintptr_t iter = (1 <<  8);
	uintptr_t pass = (1 <<  8);

	uintptr_t i;
	srand(time(NULL));

	hiclock_t fix_duration = 0;
	hiclock_t flt_duration = 0;
	fix16_t   fix_error = 0;

	uintptr_t k;
	for(k = 0; k < pass; k++) {
		fix16_t fix_args[args];
		for(i = 0; i < args; i++)
			fix_args[i] = (rand() ^ (rand() << 16));
		fix16_t fix_result[args];
		hiclock_t fix_start = hiclock();
		for(i = 0; i < iter; i++) {
			uintptr_t j;
			for(j = 0; j < args; j++)
				fix_result[j] = fix_func(fix_args[j]);
		}
		hiclock_t fix_end = hiclock();

		float   flt_args[args];
		for(i = 0; i < args; i++)
			flt_args[i] = fix16_to_float(fix_args[i]);
		float   flt_result[args];
		hiclock_t flt_start = hiclock();
		for(i = 0; i < iter; i++) {
			uintptr_t j;
			for(j = 0; j < args; j++)
				flt_result[j] = flt_func(flt_args[j]);
		}
		hiclock_t flt_end = hiclock();

		for(i = 0; i < args; i++)
			fix_error += abs(fix16_from_float(flt_result[i]) - fix_result[i]);
		flt_duration += (flt_end - flt_start);
		fix_duration += (fix_end - fix_start);
	}

	printf("%16s: %08"PRIuHICLOCK" @ %"PRIu32"Hz\n", flt_func_str, flt_duration, HICLOCKS_PER_SEC);
	printf("%16s: %08"PRIuHICLOCK" @ %"PRIu32"Hz\n", fix_func_str, fix_duration, HICLOCKS_PER_SEC);
	printf("      Difference: %08"PRIiHICLOCK" (% 3.2f%%)\n", (flt_duration - fix_duration), ((fix_duration * 100.0) / flt_duration));
	printf("           Error: %f%%\n", ((fix16_to_dbl(fix_error) * 100.0) / (args * pass)));

    return EXIT_SUCCESS;
}
Ejemplo n.º 4
0
		operator float()   const { return fix16_to_float(value); }
Ejemplo n.º 5
0
int main()
{
    int i;
    interface_init();
    
    start_timing();
    print_value("Timestamp bias", end_timing());
    
    for (i = 0; i < TESTCASES1_COUNT; i++)
    {
        fix16_t input = testcases1[i].a;
        fix16_t result;
        fix16_t expected = testcases1[i].sqrt;
        MEASURE(sqrt_cycles, result = fix16_sqrt(input));
        
        if (input > 0 && delta(result, expected) > max_delta)
        {
            print_value("Failed SQRT, i", i);
            print_value("Failed SQRT, input", input);
            print_value("Failed SQRT, output", result);
            print_value("Failed SQRT, expected", expected);
        }
        
        expected = testcases1[i].exp;
        MEASURE(exp_cycles, result = fix16_exp(input));
        
        if (delta(result, expected) > 400)
        {
            print_value("Failed EXP, i", i);
            print_value("Failed EXP, input", input);
            print_value("Failed EXP, output", result);
            print_value("Failed EXP, expected", expected);
        }
    }
    PRINT(sqrt_cycles, "fix16_sqrt");
    PRINT(exp_cycles, "fix16_exp");

    for (i = 0; i < TESTCASES2_COUNT; i++)
    {
        fix16_t a = testcases2[i].a;
        fix16_t b = testcases2[i].b;
        volatile fix16_t result;
        
        fix16_t expected = testcases2[i].add;
        MEASURE(add_cycles, result = fix16_add(a, b));
        if (delta(result, expected) > max_delta)
        {
            print_value("Failed ADD, i", i);
            print_value("Failed ADD, a", a);
            print_value("Failed ADD, b", b);
            print_value("Failed ADD, output", result);
            print_value("Failed ADD, expected", expected);
        }
        
        expected = testcases2[i].sub;
        MEASURE(sub_cycles, result = fix16_sub(a, b));
        if (delta(result, expected) > max_delta)
        {
            print_value("Failed SUB, i", i);
            print_value("Failed SUB, a", a);
            print_value("Failed SUB, b", b);
            print_value("Failed SUB, output", result);
            print_value("Failed SUB, expected", expected);
        }
        
        expected = testcases2[i].mul;
        MEASURE(mul_cycles, result = fix16_mul(a, b));
        if (delta(result, expected) > max_delta)
        {
            print_value("Failed MUL, i", i);
            print_value("Failed MUL, a", a);
            print_value("Failed MUL, b", b);
            print_value("Failed MUL, output", result);
            print_value("Failed MUL, expected", expected);
        }
        
        if (b != 0)
        {
            expected = testcases2[i].div;
            MEASURE(div_cycles, result = fix16_div(a, b));
            if (delta(result, expected) > max_delta)
            {
                print_value("Failed DIV, i", i);
                print_value("Failed DIV, a", a);
                print_value("Failed DIV, b", b);
                print_value("Failed DIV, output", result);
                print_value("Failed DIV, expected", expected);
            }
        }
    }
    PRINT(add_cycles, "fix16_add");
    PRINT(sub_cycles, "fix16_sub");
    PRINT(mul_cycles, "fix16_mul");
    PRINT(div_cycles, "fix16_div");
    
    /* Compare with floating point performance */
#ifndef NO_FLOAT
    for (i = 0; i < TESTCASES1_COUNT; i++)
    {
        float input = fix16_to_float(testcases1[i].a);
        volatile float result;
        MEASURE(float_sqrtf_cycles, result = sqrtf(input));
    }
    PRINT(float_sqrtf_cycles, "float sqrtf");
    
    for (i = 0; i < TESTCASES2_COUNT; i++)
    {
        float a = fix16_to_float(testcases2[i].a);
        float b = fix16_to_float(testcases2[i].b);
        volatile float result;
        MEASURE(float_add_cycles, result = a + b);
        MEASURE(float_sub_cycles, result = a - b);
        MEASURE(float_mul_cycles, result = a * b);
        
        if (b != 0)
        {
            MEASURE(float_div_cycles, result = a / b);
        }
    }
    PRINT(float_add_cycles, "float add");
    PRINT(float_sub_cycles, "float sub");
    PRINT(float_mul_cycles, "float mul");
    PRINT(float_div_cycles, "float div");
#endif    

    return 0;
}
Ejemplo n.º 6
0
int main(void)
{
    /* initialize the core clock and the systick timer */
    InitClock();
    InitSysTick();

    /* initialize the RGB led */
    LED_Init();

    /* Initialize UART0 */
    InitUart0();

    /* double rainbow all across the sky */
    DoubleFlash();

    /* initialize the I2C bus */
    I2C_Init();

#if DATA_FUSE_MODE

    /* signaling for fusion */
    FusionSignal_Init();

#endif // DATA_FUSE_MODE

    /* initialize UART fifos */
    RingBuffer_Init(&uartInputFifo, &uartInputData, UART_RX_BUFFER_SIZE);
    RingBuffer_Init(&uartOutputFifo, &uartOutputData, UART_TX_BUFFER_SIZE);

    /* initialize UART0 interrupts */
    Uart0_InitializeIrq(&uartInputFifo, &uartOutputFifo);
    Uart0_EnableReceiveIrq();

    /* initialize I2C arbiter */
    InitI2CArbiter();

    /* initialize the IMUs */
    InitHMC5883L();
    InitMPU6050();
//    InitMPU6050();

#if ENABLE_MMA8451Q
    InitMMA8451Q();
#endif

    /* Wait for the config messages to get flushed */
    //TrafficLight();
    DoubleFlash();
    RingBuffer_BlockWhileNotEmpty(&uartOutputFifo);

#if ENABLE_MMA8451Q
    /* initialize the MMA8451Q data structure for accelerometer data fetching */
    mma8451q_acc_t acc;
    MMA8451Q_InitializeData(&acc);
#endif

    /* initialize the MPU6050 data structure */
    mpu6050_sensor_t accgyrotemp, previous_accgyrotemp;
    MPU6050_InitializeData(&accgyrotemp);
    MPU6050_InitializeData(&previous_accgyrotemp);

    /* initialize the HMC5883L data structure */
    hmc5883l_data_t compass, previous_compass;
    HMC5883L_InitializeData(&compass);
    HMC5883L_InitializeData(&previous_compass);

    /* initialize HMC5883L reading */
    uint32_t lastHMCRead = 0;
    const uint32_t readHMCEvery = 1000 / 75; /* at 75Hz, data come every (1000/75Hz) ms. */

    /************************************************************************/
    /* Fetch scaler values                                                  */
    /************************************************************************/

#if DATA_FUSE_MODE

    const fix16_t mpu6050_accelerometer_scaler = mpu6050_accelerometer_get_scaler();
    const fix16_t mpu6050_gyroscope_scaler = mpu6050_gyroscope_get_scaler();
    const fix16_t hmc5883l_magnetometer_scaler = hmc5883l_magnetometer_get_scaler();

#endif // DATA_FUSE_MODE

    /************************************************************************/
    /* Prepare data fusion                                                  */
    /************************************************************************/

#if DATA_FUSE_MODE

    uint32_t last_transmit_time = 0;
    uint32_t last_fusion_time = systemTime();

    fusion_initialize();

#endif // DATA_FUSE_MODE

    /************************************************************************/
    /* Main loop                                                            */
    /************************************************************************/

    for(;;)
    {
        /* helper variables to track data freshness */
        uint_fast8_t have_gyro_data = 0;
        uint_fast8_t have_acc_data = 0;
        uint_fast8_t have_mag_data = 0;

        /************************************************************************/
        /* Determine if sensor data fetching is required                        */
        /************************************************************************/

        /* helper variables for event processing */
        int eventsProcessed = 0;
        int readMPU, readHMC;
#if ENABLE_MMA8451Q
        int readMMA;
#endif

        /* atomic detection of fresh data */
        __disable_irq();
#if ENABLE_MMA8451Q
        readMMA = poll_mma8451q;
#endif
        readMPU = poll_mpu6050;
        poll_mma8451q = 0;
        poll_mpu6050 = 0;
        __enable_irq();

        /* detection of HMC read */
        /*
         * TODO: read synchronized with MPU
         */
        readHMC = 0;
        uint32_t time = systemTime();
        if ((time - lastHMCRead) >= readHMCEvery)
        {
            readHMC = 1;
            lastHMCRead = time;
        }

        /************************************************************************/
        /* Fetching MPU6050 sensor data if required                             */
        /************************************************************************/

        /* read accelerometer/gyro */
        if (readMPU)
        {
            LED_BlueOff();

            I2CArbiter_Select(MPU6050_I2CADDR);
            MPU6050_ReadData(&accgyrotemp);

            /* mark event as detected */
            eventsProcessed = 1;

            /* check for data freshness */
            have_acc_data = (accgyrotemp.accel.x != previous_accgyrotemp.accel.x)
                            || (accgyrotemp.accel.y != previous_accgyrotemp.accel.y)
                            || (accgyrotemp.accel.z != previous_accgyrotemp.accel.z);

            have_gyro_data = (accgyrotemp.gyro.x != previous_accgyrotemp.gyro.x)
                             || (accgyrotemp.gyro.y != previous_accgyrotemp.gyro.y)
                             || (accgyrotemp.gyro.z != previous_accgyrotemp.gyro.z);

            /* loop current data --> previous data */
            previous_accgyrotemp = accgyrotemp;
        }

        /************************************************************************/
        /* Fetching HMC5883L sensor data if required                            */
        /************************************************************************/

        /* read compass data */
        if (readHMC)
        {
            I2CArbiter_Select(HMC5883L_I2CADDR);
            HMC5883L_ReadData(&compass);

            /* mark event as detected */
            eventsProcessed = 1;

            /* check for data freshness */
            have_mag_data = (compass.x != previous_compass.x)
                            || (compass.y != previous_compass.y)
                            || (compass.z != previous_compass.z);

            /* loop current data --> previous data */
            previous_compass = compass;
        }

        /************************************************************************/
        /* Fetching MMA8451Q sensor data if required                            */
        /************************************************************************/

#if ENABLE_MMA8451Q
        /* read accelerometer */
        if (readMMA)
        {
            LED_RedOff();

            I2CArbiter_Select(MMA8451Q_I2CADDR);
            MMA8451Q_ReadAcceleration14bitNoFifo(&acc);

            /* mark event as detected */
            eventsProcessed = 1;
        }
#endif

        /************************************************************************/
        /* Raw sensor data output over serial                                   */
        /************************************************************************/

#if DATA_FETCH_MODE

        /* data availability + sanity check
         * This sent me on a long bug hunt: Sometimes the interrupt would be raised
         * even if not all data registers were written. This always resulted in a
         * z data register not being fully written which, in turn, resulted in
         * extremely jumpy measurements.
         */
        if (readMPU && accgyrotemp.status != 0)
        {
            /* write data */
            uint8_t type = 0x02;
            P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)accgyrotemp.data, sizeof(accgyrotemp.data), IO_SendByte);
        }

        /* data availability + sanity check */
        if (readHMC && (compass.status & HMC5883L_SR_RDY_MASK) != 0) /* TODO: check if not in lock state */
        {
            uint8_t type = 0x03;
            P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)compass.xyz, sizeof(compass.xyz), IO_SendByte);
        }

#if ENABLE_MMA8451Q
        /* data availability + sanity check */
        if (readMMA && acc.status != 0)
        {
            uint8_t type = 0x01;
            P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)acc.xyz, sizeof(acc.xyz), IO_SendByte);
        }
#endif

#endif // DATA_FETCH_MODE

        /************************************************************************/
        /* Sensor data fusion                                                   */
        /************************************************************************/

#if DATA_FUSE_MODE

        // if there were sensor data ...
        if (eventsProcessed)
        {
            v3d gyro, acc, mag;

            // convert, calibrate and store gyroscope data
            if (have_gyro_data)
            {
                sensor_prepare_mpu6050_gyroscope_data(&gyro, accgyrotemp.gyro.x, accgyrotemp.gyro.y, accgyrotemp.gyro.z, mpu6050_gyroscope_scaler);
                fusion_set_gyroscope_v3d(&gyro);
            }

            // convert, calibrate and store accelerometer data
            if (have_acc_data)
            {
                sensor_prepare_mpu6050_accelerometer_data(&acc, accgyrotemp.accel.x, accgyrotemp.accel.y, accgyrotemp.accel.z, mpu6050_accelerometer_scaler);
                fusion_set_accelerometer_v3d(&acc);
            }

            // convert, calibrate and store magnetometer data
            if (have_mag_data)
            {
                sensor_prepare_hmc5883l_data(&mag, compass.x, compass.y, compass.z, hmc5883l_magnetometer_scaler);
                fusion_set_magnetometer_v3d(&mag);
            }

            // get the time differential
            const uint32_t current_time = systemTime();
            const fix16_t deltaT_ms = fix16_from_int(current_time - last_fusion_time);
            const fix16_t deltaT = fix16_mul(deltaT_ms, F16(0.001));

            last_fusion_time = current_time;

            FusionSignal_Predict();

            // predict the current measurements
            fusion_predict(deltaT);

            FusionSignal_Update();

            // correct the measurements
            fusion_update(deltaT);


            FusionSignal_Clear();

#if 0

            fix16_t yaw, pitch, roll;
            fusion_fetch_angles(&roll, &pitch, &yaw);

#if 0
            float yawf = fix16_to_float(yaw),
                  pitchf = fix16_to_float(pitch),
                  rollf = fix16_to_float(roll);

            IO_SendInt16((int16_t)yawf);
            IO_SendInt16((int16_t)pitchf);
            IO_SendInt16((int16_t)rollf);

            IO_SendByteUncommited('\r');
            IO_SendByte('\n');
#else
            if (current_time - last_transmit_time >= 100)
            {
                /* write data */
                uint8_t type = 42;
                fix16_t buffer[3] = { roll, pitch, yaw };
                P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);

                last_transmit_time = current_time;
            }
#endif
#else
            if (current_time - last_transmit_time >= 100)
            {
                /* write data */
                switch (output_mode)
                {
                case RPY:
                {
                    fix16_t roll, pitch, yaw;
                    fusion_fetch_angles(&roll, &pitch, &yaw);

                    /* write data */
                    uint8_t type = 42;
                    fix16_t buffer[3] = { roll, pitch, yaw };
                    P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
                    break;
                }
                case QUATERNION:
                {
                    qf16 orientation;
                    fusion_fetch_quaternion(&orientation);

                    uint8_t type = 43;
                    fix16_t buffer[4] = { orientation.a, orientation.b, orientation.c, orientation.d };
                    P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
                    break;
                }
                case QUATERNION_RPY:
                {
                    fix16_t roll, pitch, yaw;
                    fusion_fetch_angles(&roll, &pitch, &yaw);

                    qf16 orientation;
                    fusion_fetch_quaternion(&orientation);

                    uint8_t type = 44;
                    fix16_t buffer[7] = { orientation.a, orientation.b, orientation.c, orientation.d, roll, pitch, yaw };
                    P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
                    break;
                }
                case SENSORS_RAW:
                {
                    uint8_t type = 0;
                    fix16_t buffer[6] = { acc.x, acc.y, acc.z, mag.x, mag.y, mag.z };
                    P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
                    break;
                }
                }

                last_transmit_time = current_time;
            }
#endif

        }

#endif // DATA_FUSE_MODE

        /************************************************************************/
        /* Read user data input                                                 */
        /************************************************************************/

        /* as long as there is data in the buffer */
        while(!RingBuffer_Empty(&uartInputFifo))
        {
            /* light one led */
            LED_RedOn();

            /* fetch byte */
            uint8_t data = IO_ReadByte();

            output_mode = (output_mode_t)data;

            LED_RedOff();
#if 0
            /* echo to output */
            IO_SendByte(data);

            /* mark event as detected */
            eventsProcessed = 1;
#endif
        }

        /************************************************************************/
        /* Save energy if you like to                                           */
        /************************************************************************/

        /* in case of no events, allow a sleep */
        if (!eventsProcessed)
        {
            /*
             * Care must be taken with this instruction here, as it can lead
             * to a condition where after being woken up (e.g. by the SysTick)
             * and looping through, immediately before entering WFI again
             * an interrupt would yield a true condition for the branches below.
             * In this case this loop would be blocked until the next IRQ,
             * which, in case of a 1ms SysTick timer, could be too late.
             *
             * To counter this behaviour, SysTick has been speed up by factor
             * four (0.25ms).
             */
#if 0
            __WFI();
#endif
        }
    }

    return 0;
}
Ejemplo n.º 7
0
// frame calculation
static void calc_frame(void) {
    // ----- smoothers:
    // amp
    amp = filter_1p_fix16_next(ampLp);
    // time
    if(timeLp->sync) {
        ;;
    } else {
        time = filter_1p_fix16_next(timeLp);
        buffer_tap_sync(&tapRd, &tapWr, time);

#if ARCH_LINUX
        if(dbgFlag) {
            fprintf(dbgFile, "%d \t %f \r\n",
                    dbgCount,
                    fix16_to_float(time)
                   );
            dbgCount++;
        }

#endif


    }

    // rate
    //// NOTE: setting a different rate is pretty much pointless in this simple application.
    /// leaving it in just to test fractional interpolation methods.
    if(rateLp->sync) {
        ;;
    } else {
        rate = filter_1p_fix16_next(rateLp);
        buffer_tap_set_rate(&tapRd, rate);
        buffer_tap_set_rate(&tapWr, rate);
    }

    // get interpolated echo value

    echoVal = buffer_tap_read(&tapRd);

    /* // store interpolated input+fb value */
    //  buffer_tap_write(&tapWr, add_fr1x32(in0, mult_fr1x32x32(echoVal, fb ) ) );
    buffer_tap_write(&tapWr, add_fr1x32(in0 >> 1, mult_fr1x32x32(echoVal, fb ) ) );

    //FIXME: clip / saturate input buf here
    //// potentially, scale input by inverse feedback

/// test: no fb
    //buffer_tap_write(&tapWr, in0);

    /* // output */
    frameVal = add_fr1x32(
                   mult_fr1x32x32( echoVal, FIX16_FRACT_TRUNC(amp) ),
                   mult_fr1x32x32( in0,  FIX16_FRACT_TRUNC(dry) )
               );
    //// test: no dry
    //  frameVal = echoVal;
    /// FIXME: clip here

    buffer_tap_next(&tapRd);
    buffer_tap_next(&tapWr);
}