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