void Normalize(void) { fix16_t error = 0; fix16_t temporary[3][3]; fix16_t renorm = 0; error = -fix16_mul(Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0]), const_fix16_from_dbl(.5)); Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 renorm = fix16_mul(const_fix16_from_dbl(.5), fix16_sadd(fix16_from_int(3), -Vector_Dot_Product(&temporary[0][0],&temporary[0][0]))); Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); renorm = fix16_mul(const_fix16_from_dbl(.5), fix16_sadd(fix16_from_int(3), -Vector_Dot_Product(&temporary[1][0],&temporary[1][0]))); Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); renorm = fix16_mul(const_fix16_from_dbl(.5), fix16_sadd(fix16_from_int(3), - Vector_Dot_Product(&temporary[2][0],&temporary[2][0]))); Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); }
void module_init(void) { // init module/param descriptor #ifdef ARCH_BFIN pTapeData = (tapeData*)SDRAM_ADDRESS; #else pTapeData = (tapeData*)malloc(sizeof(tapeData)); // debug file dbgFile = fopen( "tape_dbg.txt", "w"); #endif gModuleData = &(pTapeData->super); gModuleData->numParams = eParamNumParams; gModuleData->paramDesc = (ParamDesc*)malloc(eParamNumParams * sizeof(ParamDesc)); strcpy(gModuleData->paramDesc[eParamAmp].label, "amp"); strcpy(gModuleData->paramDesc[eParamDry].label, "dry"); strcpy(gModuleData->paramDesc[eParamTime].label, "time"); strcpy(gModuleData->paramDesc[eParamRate].label, "rate"); strcpy(gModuleData->paramDesc[eParamFb].label, "feedback"); strcpy(gModuleData->paramDesc[eParamAmpSmooth].label, "amp smoothing"); strcpy(gModuleData->paramDesc[eParamTimeSmooth].label, "time smoothing"); // init params amp = FIX16_ONE >> 1; time = FIX16_ONE << 1; rate = FIX16_ONE; fb = 0; // init smoothers ampLp = (filter_1p_fix16*)malloc(sizeof(filter_1p_fix16)); filter_1p_fix16_init( ampLp, SAMPLERATE, fix16_from_int(32), amp ); timeLp = (filter_1p_fix16*)malloc(sizeof(filter_1p_fix16)); filter_1p_fix16_init( timeLp, SAMPLERATE, fix16_from_int(32), time ); rateLp = (filter_1p_fix16*)malloc(sizeof(filter_1p_fix16)); filter_1p_fix16_init( rateLp, SAMPLERATE, fix16_from_int(32), time ); // init buffer and taps buffer_init(&echoBuf, pTapeData->echoBufData, ECHO_BUF_SIZE, SAMPLERATE); buffer_tap_init(&tapRd, &echoBuf); buffer_tap_init(&tapWr, &echoBuf); // set rate and offset buffer_tap_set_rate(&tapRd, rate); buffer_tap_set_rate(&tapWr, rate); buffer_tap_sync(&tapRd, &tapWr, time); }
void calculate_heading_hmc5843( fix16_t roll, fix16_t pitch) { // float Head_X; // float Head_Y; // float cos_roll; // float sin_roll; // float cos_pitch; // float sin_pitch; // // // cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM? // sin_roll = sin(roll); // cos_pitch = cos(pitch); // sin_pitch = sin(pitch); // // Tilt compensated Magnetic field X component: // Head_X = hmc5843_mag_x*cos_pitch+ // hmc5843_mag_y*sin_roll*sin_pitch+ // hmc5843_mag_z*cos_roll*sin_pitch; // // Tilt compensated Magnetic field Y component: // Head_Y = hmc5843_mag_y*cos_roll- // hmc5843_mag_z*sin_roll; // // Magnetic Heading // //hmc_5843_heading = atan2(-Head_Y,Head_X); fix16_t cos_roll = fix16_cos(roll); fix16_t sin_roll = fix16_sin(roll); fix16_t cos_pitch = fix16_cos(pitch); fix16_t sin_pitch = fix16_sin(pitch); hmc5843_head_x = fix16_sadd(fix16_mul(fix16_from_int(hmc5843_mag_x),cos_pitch), fix16_sadd(fix16_mul(fix16_from_int(hmc5843_mag_y),fix16_mul(sin_roll,sin_pitch)), fix16_mul(fix16_from_int(hmc5843_mag_z),fix16_mul(cos_roll,sin_pitch)))); hmc5843_head_y = fix16_sadd(fix16_mul(fix16_from_int(hmc5843_mag_y),cos_roll), -fix16_mul(fix16_from_int(hmc5843_mag_z),sin_roll)); hmc5843_heading = fix16_atan2(-hmc5843_head_y,hmc5843_head_x); }
static inline void _set_fixed_point_scroll(fix16_t *scroll, fix16_t amount, uint16_t *in, uint16_t *dn) { int32_t integral; integral = fix16_to_int(amount); fix16_t fractional; fractional = fix16_fractional(amount); *in = integral; *dn = fractional & 0xFF00; *scroll = fix16_add(fix16_from_int(*in), fractional); }
/* A basic single-frequency DFT, useful when you are interested in just a single signal. */ static cell AMX_NATIVE_CALL amx_dft(AMX *amx, const cell *params) { // dft(input{}, Fixed: &real, Fixed: &imag, Fixed: period, count); uint8_t *input = (uint8_t*)params[1]; int count = params[5]; fix16_t period = params[4]; fix16_t *realp = (fix16_t*)params[2]; fix16_t *imagp = (fix16_t*)params[3]; // Round the count to a multiple of period int multiple = fix16_from_int(count) / period; count = fix16_to_int(fix16_mul(fix16_from_int(multiple), period)); fix16_t real = 0; fix16_t imag = 0; fix16_t step = fix16_div(2 * fix16_pi, period); fix16_t angle = 0; for (int i = 0; i < count; i++) { // We scale by 256 to achieve a good compromise between precision and // range. fix16_t value = input[INPUT_INDEX(i)] * 256; // Calculate value * (cos(angle) - i * sin(angle)) and add to sum. real += fix16_mul(value, fix16_cos(angle)); imag += fix16_mul(value, -fix16_sin(angle)); angle += step; } fix16_t scale = count * 256; *realp = fix16_div(real, scale); *imagp = fix16_div(imag, scale); return 0; }
// --------------------------------------------------------------------------------------------------------------------- void ripple_init(FXState& state) { EffectData* data = (EffectData*)state.store; Fix16 zero = fix16_from_int(0); data->dropFrequency = 3; // percent data->damping = 0.92f; for(int i = 0; i < EffectData::BufferSize * 2; ++i) { data->buffer[i] = zero; } data->source = data->buffer; data->destination = &data->buffer[EffectData::BufferSize]; }
// read fract32 buffer_tapN_read(bufferTapN *tap) { fract32 a, b; fix16 tmp; #if 1 if(tap->divCount == 0) { return tap->buf->data[tap->idx]; } else { // interpolate during phase-division a = tap->buf->data[tap->idx]; if( (tap->idx + 1) >= tap->loop) { b = tap->buf->data[0]; } else { b = tap->buf->data[ tap->idx + 1 ]; } tmp = FRACT_FIX16( sub_fr1x32(b, a) ); tmp = fix16_mul(tmp, fix16_from_int(tap->divCount)); return add_fr1x32(a, FIX16_FRACT_TRUNC(tmp)); } #else return tap->buf->data[tap->idx]; #endif }
void Matrix_update( int gx , int gy , int gz , int ax , int ay , int az ) { Gyro_Vector[0] = Gyro_Scaled_X(fix16_from_int(-gx)); Gyro_Vector[1] = Gyro_Scaled_X(fix16_from_int(gy)); Gyro_Vector[2] = Gyro_Scaled_X(fix16_from_int(-gz)); /* Accel_Vector[0]=fix16_sadd(fix16_mul(Accel_Vector[0],const_fix16_from_dbl(0.6)), fix16_mul(fix16_from_int(ax),const_fix16_from_dbl(0.4))); Accel_Vector[1]=fix16_sadd(fix16_mul(Accel_Vector[0],const_fix16_from_dbl(0.6)), fix16_mul(fix16_from_int(ay),const_fix16_from_dbl(0.4))); Accel_Vector[2]=fix16_sadd(fix16_mul(Accel_Vector[0],const_fix16_from_dbl(0.6)), fix16_mul(fix16_from_int(az),const_fix16_from_dbl(0.4))); */ /* NO LOW PASS FILTER */ Accel_Vector[0] = fix16_from_int(ax); Accel_Vector[1] = fix16_from_int(-ay); Accel_Vector[2] = fix16_from_int(az); Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional #if OUTPUTMODE==1 // corrected mode Update_Matrix[0][0] = 0; Update_Matrix[0][1] = -fix16_mul(G_Dt, Omega_Vector[2]);//-z Update_Matrix[0][2] = fix16_mul(G_Dt, Omega_Vector[1]);//y Update_Matrix[1][0] = fix16_mul(G_Dt, Omega_Vector[2]);//z Update_Matrix[1][1] = 0; Update_Matrix[1][2] = -fix16_mul(G_Dt, Omega_Vector[0]);//-x Update_Matrix[2][0] = -fix16_mul(G_Dt, Omega_Vector[1]);//-y Update_Matrix[2][1] = fix16_mul(G_Dt, Omega_Vector[0]);//x Update_Matrix[2][2] = 0; #endif Matrix_Multiply(DCM_Matrix, Update_Matrix, Temporary_Matrix); Matrix_Addto(DCM_Matrix, Temporary_Matrix); }
const Fix16 smul(const int16_t other) const { Fix16 ret = fix16_smul(value, fix16_from_int(other)); return ret; }
const Fix16 ssub(const int16_t other) const { Fix16 ret = fix16_sadd(value, -fix16_from_int(other)); return ret; }
Fix16 & operator-=(const int16_t rhs) { value -= fix16_from_int(rhs); return *this; }
const int operator> (const int16_t other) const { return (value > fix16_from_int(other)); }
Fix16(const int16_t inValue) { value = fix16_from_int(inValue); }
namespace vfx { struct InterfaceStage { enum Enum { IS_GUI, IS_FADETO, IS_FX, IS_FADEFROM }; }; DisplayMode::Enum gCurrentDisplayMode = DisplayMode::plasma; InterfaceStage::Enum gCurrentInterfaceStage = InterfaceStage::IS_GUI; DisplayMode::Enum gNextDisplayMode = DisplayMode::plasma; int16_t fadeTick = 0; // --------------------------------------------------------------------------------------------------------------------- void init(FXState& state) { state.counter = 0; state.rng.reseed(MurmurHash2(__TIMESTAMP__, sizeof(__TIMESTAMP__), 0xb33f)); } static int32_t dialA = 0; static int32_t dialB = 0; static Fix16 vTargetA = fix16_from_int(0), vCurA = fix16_from_int(0); static Fix16 vTargetB = fix16_from_int(0), vCurB = fix16_from_int(0); static int32_t ticksSinceAdjust = 0; struct gui_entry { const unsigned char* gly; DisplayMode::Enum mode; }; static const gui_entry radioGUI[] = { {glyph_noise, DisplayMode::noise}, {glyph_plasma, DisplayMode::plasma}, {glyph_flame, DisplayMode::flame}, {glyph_meta, DisplayMode::meta}, {glyph_ripple, DisplayMode::ripple}, {glyph_cube, DisplayMode::cube}, {glyph_spark, DisplayMode::spark}, {glyph_eq, DisplayMode::eq}, {glyph_storm, DisplayMode::stars}, {glyph_qst, DisplayMode::ident}, }; static const int16_t radioGUICount = sizeof(radioGUI) / sizeof(gui_entry); // --------------------------------------------------------------------------------------------------------------------- bool tick(const FrameInput& input, FXState& state, FrameOutput& output) { switch (gCurrentInterfaceStage) { case InterfaceStage::IS_GUI: { output.clear(); if (input.dialChange[1] == 0) ticksSinceAdjust ++; else ticksSinceAdjust = 0; const int16_t radioLen = sizeof(radioGUI) / sizeof(gui_entry); Fix16 max_target = fix16_from_int(radioLen); Fix16 max_value = fix16_from_int(radioLen - 1); Fix16 dial16 = fix16_from_int( input.dialChange[1] ); Fix16 pt05 = fix16_from_float(-1.0f); vTargetA += dial16 * fix16_from_float(0.2f); if (vTargetA < fix16_neg_one) vTargetA = fix16_neg_one; if (vTargetA > max_target) vTargetA = max_target; vCurA += (vTargetA - vCurA) * fix16_from_float(0.035f); if (vCurA < fix16_zero) vCurA = fix16_zero; if (vCurA > max_value) vCurA = max_value; if (ticksSinceAdjust > 30) { vTargetA -= fpart(vTargetA); } int16_t off = ipart(vCurA).asInt(); if (off < 0) off = 0; if (off > radioLen - 1) off = radioLen - 1; const gui_entry* guient[3] = {&radioGUI[off], &radioGUI[off], &radioGUI[off]}; if (off - 1 >= 0) guient[2] = &radioGUI[off - 1]; if (off + 1 <= radioLen - 1) guient[1] = &radioGUI[off + 1]; Fix16 charASlide = fpart(vCurA); charASlide *= Fix16(-16.0f); int16_t slideAInt = charASlide.asInt(); IconGlyph(output.frame, guient[0]->gly, slideAInt, 0, true); IconGlyph(output.frame, guient[1]->gly, 16 + slideAInt, 0, false); if (off > 0) IconGlyph(output.frame, guient[2]->gly, slideAInt - 16, 0, false); if (fadeTick < 15) { fadeTick ++; output.fade(15 - fadeTick); } if (input.dialClick) { gCurrentInterfaceStage = InterfaceStage::IS_FADETO; gNextDisplayMode = guient[0]->mode; } } break; case InterfaceStage::IS_FADETO: { output.fade(1); fadeTick --; if (fadeTick <= 0) { vTargetA = vCurA; gCurrentDisplayMode = gNextDisplayMode; for(int i = 0; i < Constants::MemoryPool; ++i) state.store[i] = 0xFF; DisplayMode::doInitFor(gCurrentDisplayMode, state); fadeTick = 0; gCurrentInterfaceStage = InterfaceStage::IS_FX; } } break; case InterfaceStage::IS_FX: { DisplayMode::doTickFor(gCurrentDisplayMode, input, output, state); if (fadeTick < 15) { fadeTick ++; output.fade(15 - fadeTick); } if (input.dialClick) { gCurrentInterfaceStage = InterfaceStage::IS_FADEFROM; } } break; case InterfaceStage::IS_FADEFROM: { DisplayMode::doTickFor(gCurrentDisplayMode, input, output, state); output.fade(15 - fadeTick); fadeTick --; if (fadeTick <= 0) { gCurrentInterfaceStage = InterfaceStage::IS_GUI; fadeTick = 0; } } break; } // /* dial --; if (dial <= 0) { // clear the frame output.clear(); for (int y=0; y<Constants::FrameHeight; y++) { for (int x=0; x<Constants::FrameWidth; x++) { int32_t RR = state.rng.genInt32(-4, 4); if (RR<0) RR =0; int32_t GG = state.rng.genInt32(-4, 4); if (GG<0) GG =0; setLED(output.frame, x, y, RR, GG); } } dial = 6; } */ /* byte red, green; for (int y=0; y<Constants::FrameHeight; y++) { for (int x=0; x<Constants::FrameWidth; x++) { pixel &LEDpixel = output.frame[y * Constants::FrameWidth + x]; DecodeByte(LEDpixel, red, green); if (red > 0) red --; if (green > 0) green --; LEDpixel = red | (green << 4); } } vTargetA += fix16_from_int( input.dialChange[1] ); vTargetB += fix16_from_int( input.dialChange[2] ); vCurA += (vTargetA - vCurA) * fix16_from_float(0.05f); vCurB += (vTargetB - vCurB) * fix16_from_float(0.05f); Fix16 xo = fix16_from_float(8.0f); Fix16 yo = fix16_from_float(8.0f); Fix16 ss1 = fix16_sin(vCurA * fix16_from_float(0.1f)); Fix16 cc1 = fix16_cos(vCurA * fix16_from_float(0.1f)); Fix16 ss2 = fix16_sin(vCurB * fix16_from_float(0.1f)); Fix16 cc2 = fix16_cos(vCurB * fix16_from_float(0.1f)); Fix16 rad1(-2.0f), rad2(12.0f); draw::WuLine( output.frame, xo + (ss1 * rad1), yo + (cc1 * rad1), xo + (ss1 * rad2), yo + (cc1 * rad2), Red); draw::WuLine( output.frame, xo + (ss2 * rad1), yo + (cc2 * rad1), xo + (ss2 * rad2), yo + (cc2 * rad2), Green); */ return true; } } // namespace vfx
// --------------------------------------------------------------------------------------------------------------------- bool tick(const FrameInput& input, FXState& state, FrameOutput& output) { switch (gCurrentInterfaceStage) { case InterfaceStage::IS_GUI: { output.clear(); if (input.dialChange[1] == 0) ticksSinceAdjust ++; else ticksSinceAdjust = 0; const int16_t radioLen = sizeof(radioGUI) / sizeof(gui_entry); Fix16 max_target = fix16_from_int(radioLen); Fix16 max_value = fix16_from_int(radioLen - 1); Fix16 dial16 = fix16_from_int( input.dialChange[1] ); Fix16 pt05 = fix16_from_float(-1.0f); vTargetA += dial16 * fix16_from_float(0.2f); if (vTargetA < fix16_neg_one) vTargetA = fix16_neg_one; if (vTargetA > max_target) vTargetA = max_target; vCurA += (vTargetA - vCurA) * fix16_from_float(0.035f); if (vCurA < fix16_zero) vCurA = fix16_zero; if (vCurA > max_value) vCurA = max_value; if (ticksSinceAdjust > 30) { vTargetA -= fpart(vTargetA); } int16_t off = ipart(vCurA).asInt(); if (off < 0) off = 0; if (off > radioLen - 1) off = radioLen - 1; const gui_entry* guient[3] = {&radioGUI[off], &radioGUI[off], &radioGUI[off]}; if (off - 1 >= 0) guient[2] = &radioGUI[off - 1]; if (off + 1 <= radioLen - 1) guient[1] = &radioGUI[off + 1]; Fix16 charASlide = fpart(vCurA); charASlide *= Fix16(-16.0f); int16_t slideAInt = charASlide.asInt(); IconGlyph(output.frame, guient[0]->gly, slideAInt, 0, true); IconGlyph(output.frame, guient[1]->gly, 16 + slideAInt, 0, false); if (off > 0) IconGlyph(output.frame, guient[2]->gly, slideAInt - 16, 0, false); if (fadeTick < 15) { fadeTick ++; output.fade(15 - fadeTick); } if (input.dialClick) { gCurrentInterfaceStage = InterfaceStage::IS_FADETO; gNextDisplayMode = guient[0]->mode; } } break; case InterfaceStage::IS_FADETO: { output.fade(1); fadeTick --; if (fadeTick <= 0) { vTargetA = vCurA; gCurrentDisplayMode = gNextDisplayMode; for(int i = 0; i < Constants::MemoryPool; ++i) state.store[i] = 0xFF; DisplayMode::doInitFor(gCurrentDisplayMode, state); fadeTick = 0; gCurrentInterfaceStage = InterfaceStage::IS_FX; } } break; case InterfaceStage::IS_FX: { DisplayMode::doTickFor(gCurrentDisplayMode, input, output, state); if (fadeTick < 15) { fadeTick ++; output.fade(15 - fadeTick); } if (input.dialClick) { gCurrentInterfaceStage = InterfaceStage::IS_FADEFROM; } } break; case InterfaceStage::IS_FADEFROM: { DisplayMode::doTickFor(gCurrentDisplayMode, input, output, state); output.fade(15 - fadeTick); fadeTick --; if (fadeTick <= 0) { gCurrentInterfaceStage = InterfaceStage::IS_GUI; fadeTick = 0; } } break; } // /* dial --; if (dial <= 0) { // clear the frame output.clear(); for (int y=0; y<Constants::FrameHeight; y++) { for (int x=0; x<Constants::FrameWidth; x++) { int32_t RR = state.rng.genInt32(-4, 4); if (RR<0) RR =0; int32_t GG = state.rng.genInt32(-4, 4); if (GG<0) GG =0; setLED(output.frame, x, y, RR, GG); } } dial = 6; } */ /* byte red, green; for (int y=0; y<Constants::FrameHeight; y++) { for (int x=0; x<Constants::FrameWidth; x++) { pixel &LEDpixel = output.frame[y * Constants::FrameWidth + x]; DecodeByte(LEDpixel, red, green); if (red > 0) red --; if (green > 0) green --; LEDpixel = red | (green << 4); } } vTargetA += fix16_from_int( input.dialChange[1] ); vTargetB += fix16_from_int( input.dialChange[2] ); vCurA += (vTargetA - vCurA) * fix16_from_float(0.05f); vCurB += (vTargetB - vCurB) * fix16_from_float(0.05f); Fix16 xo = fix16_from_float(8.0f); Fix16 yo = fix16_from_float(8.0f); Fix16 ss1 = fix16_sin(vCurA * fix16_from_float(0.1f)); Fix16 cc1 = fix16_cos(vCurA * fix16_from_float(0.1f)); Fix16 ss2 = fix16_sin(vCurB * fix16_from_float(0.1f)); Fix16 cc2 = fix16_cos(vCurB * fix16_from_float(0.1f)); Fix16 rad1(-2.0f), rad2(12.0f); draw::WuLine( output.frame, xo + (ss1 * rad1), yo + (cc1 * rad1), xo + (ss1 * rad2), yo + (cc1 * rad2), Red); draw::WuLine( output.frame, xo + (ss2 * rad1), yo + (cc2 * rad1), xo + (ss2 * rad2), yo + (cc2 * rad2), Green); */ return true; }
void imu_get_rates(fix16_t rate[]){ gyro_get_data(); rate[0] = fix16_from_int(imu.gyroADC[ROLL]); rate[1] = fix16_from_int(imu.gyroADC[PITCH]); rate[2] = fix16_from_int(imu.gyroADC[YAW]); }
void Drift_correction(fix16_t head_x, fix16_t head_y) { fix16_t errorCourse; static fix16_t Scaled_Omega_P[3]; static fix16_t Scaled_Omega_I[3]; Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); errorRollPitch[0] = constrain(errorRollPitch[0],-fix16_from_int(1000),fix16_from_int(1000)); errorRollPitch[1] = constrain(errorRollPitch[1],-fix16_from_int(1000),fix16_from_int(1000)); errorRollPitch[2] = constrain(errorRollPitch[2],-fix16_from_int(1000),fix16_from_int(1000)); Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH); Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); #ifdef IsMAG errorCourse= fix16_sadd(fix16_mul(DCM_Matrix[0][0], head_x), fix16_mul(DCM_Matrix[1][0],head_y)); Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. errorYaw[0] = constrain(errorYaw[0],-fix16_from_int(50),fix16_from_int(50)); errorYaw[1] = constrain(errorYaw[1],-fix16_from_int(50),fix16_from_int(50)); errorYaw[2] = constrain(errorYaw[2],-fix16_from_int(50),fix16_from_int(50)); Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I #endif }
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; }