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);
}
/* 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;
}
Fix16 sind() { return Fix16(fix16_sin(fix16_deg_to_rad(value))); }
Fix16 sin() { return Fix16(fix16_sin(value)); }