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