david fernandez
/
Nucleo_i2c_master_David_4_
Multithread approach to 6DOF Mag Tracker
Mag_track.h
- Committer:
- darkraxx
- Date:
- 2020-05-21
- Revision:
- 0:9621bbc04b9b
File content as of revision 0:9621bbc04b9b:
//===================================================================================================== // Mag_track.h //===================================================================================================== // // Implementation of Magnetic tracking algorithm. // // Date Author Notes // 30/01/2020 DAFG Initial release // //===================================================================================================== #ifndef Mag_track_h #define Mag_track_h //---------------------------------------------------------------------------------------------------- // Variable declaration // maximum number of inputs that can be handled // in one function call #define MAX_INPUT_LEN 16 // maximum length of filter than can be handled #define MAX_FLT_LEN 84 // buffer to hold all of the input samples #define BUFFER_LEN (MAX_FLT_LEN + MAX_INPUT_LEN) extern int16_t lp_insamp[ 3*BUFFER_LEN ]; // Array to hold input samples LP #define FILTER_LEN_LP 82 // fs = 800 Hz; low pass fc 10-35 Hz extern int16_t coeffs_lp[FILTER_LEN_LP]; extern int16_t insamp[ 3*BUFFER_LEN ]; // Array to hold input samples BP #define FILTER_LEN_BP 71 // fs = 800 Hz; Band pass 85-115 Hz extern int16_t coeffs_bp[ FILTER_LEN_BP ]; extern Serial PC; extern DigitalOut myled; //--------------------------------------------------------------------------------------------------- // Function declarations /////// FIR init void firFixedInit( void ); /////// the FIR filter function void firFixed( int16_t *coeffs, int16_t *input, int16_t *output, int length, int filterLength, int bias, int16_t *buffer ); /////// SIGN FUNCTION int sign(int16_t x); /////// QUATERNION product FUNCTION void Qprod(float *p, float *q, float *r ); /////// QUATERNION rotation FUNCTION void Qrotate(float *v, float *q, float *out ); /////// Sensor set_up function void sensor_setup(); /////// MPU sensor axes alignment and offset correction void MPUrot_offset(int16_t *Data, char *rawData, int16_t *offset); /////// Magnetic sensor offset correction void Mag_offset(int16_t *M_read, char *data, int16_t *offset); /////// Sensors reading void Read_Mag(int16_t *M_read); void Read_Acc(int16_t *A_read); void Read_Gyr(int16_t *G_read); /////// Madgwick Quaternion update high-level function void QUpdate(int32_t *q, float *m, float *a, float *g, int16_t *Mag, int16_t *Acc, int16_t *Gyr, float *Q_init, int32_t *Q_out); /////// Filtered magnetic signals envelope extraction void Env_extraction(int16_t *E, int16_t *buffer_rot, int16_t *max, int16_t *p_i, double *R, double *ph, int16_t *S); #endif //===================================================================================================== // End of file //=====================================================================================================