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); }
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); }
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; }
operator float() const { return fix16_to_float(value); }
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; }
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; }
// 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); }