david fernandez / Mbed 2 deprecated Nucleo_i2c_master_David_4_

Multithread approach to 6DOF Mag Tracker

Dependencies:   mbed mbed-rtos

Mag_track.h@0:9621bbc04b9b, 2020-05-21 (annotated)

Committer:
darkraxx
Date:
Thu May 21 13:10:58 2020 +0000
Revision:
0:9621bbc04b9b
Multithread approach Mag Track

Who changed what in which revision?

UserRevisionLine numberNew contents of line
darkraxx 0:9621bbc04b9b 1 //=====================================================================================================
darkraxx 0:9621bbc04b9b 2 // Mag_track.h
darkraxx 0:9621bbc04b9b 3 //=====================================================================================================
darkraxx 0:9621bbc04b9b 4 //
darkraxx 0:9621bbc04b9b 5 // Implementation of Magnetic tracking algorithm.
darkraxx 0:9621bbc04b9b 6 //
darkraxx 0:9621bbc04b9b 7 // Date Author Notes
darkraxx 0:9621bbc04b9b 8 // 30/01/2020 DAFG Initial release
darkraxx 0:9621bbc04b9b 9 //
darkraxx 0:9621bbc04b9b 10 //=====================================================================================================
darkraxx 0:9621bbc04b9b 11 #ifndef Mag_track_h
darkraxx 0:9621bbc04b9b 12 #define Mag_track_h
darkraxx 0:9621bbc04b9b 13 //----------------------------------------------------------------------------------------------------
darkraxx 0:9621bbc04b9b 14 // Variable declaration
darkraxx 0:9621bbc04b9b 15
darkraxx 0:9621bbc04b9b 16 // maximum number of inputs that can be handled
darkraxx 0:9621bbc04b9b 17 // in one function call
darkraxx 0:9621bbc04b9b 18 #define MAX_INPUT_LEN 16
darkraxx 0:9621bbc04b9b 19 // maximum length of filter than can be handled
darkraxx 0:9621bbc04b9b 20 #define MAX_FLT_LEN 84
darkraxx 0:9621bbc04b9b 21 // buffer to hold all of the input samples
darkraxx 0:9621bbc04b9b 22 #define BUFFER_LEN (MAX_FLT_LEN + MAX_INPUT_LEN)
darkraxx 0:9621bbc04b9b 23 extern int16_t lp_insamp[ 3*BUFFER_LEN ]; // Array to hold input samples LP
darkraxx 0:9621bbc04b9b 24 #define FILTER_LEN_LP 82
darkraxx 0:9621bbc04b9b 25 // fs = 800 Hz; low pass fc 10-35 Hz
darkraxx 0:9621bbc04b9b 26 extern int16_t coeffs_lp[FILTER_LEN_LP];
darkraxx 0:9621bbc04b9b 27 extern int16_t insamp[ 3*BUFFER_LEN ]; // Array to hold input samples BP
darkraxx 0:9621bbc04b9b 28 #define FILTER_LEN_BP 71
darkraxx 0:9621bbc04b9b 29 // fs = 800 Hz; Band pass 85-115 Hz
darkraxx 0:9621bbc04b9b 30 extern int16_t coeffs_bp[ FILTER_LEN_BP ];
darkraxx 0:9621bbc04b9b 31
darkraxx 0:9621bbc04b9b 32 extern Serial PC;
darkraxx 0:9621bbc04b9b 33 extern DigitalOut myled;
darkraxx 0:9621bbc04b9b 34
darkraxx 0:9621bbc04b9b 35 //---------------------------------------------------------------------------------------------------
darkraxx 0:9621bbc04b9b 36 // Function declarations
darkraxx 0:9621bbc04b9b 37
darkraxx 0:9621bbc04b9b 38 /////// FIR init
darkraxx 0:9621bbc04b9b 39 void firFixedInit( void );
darkraxx 0:9621bbc04b9b 40 /////// the FIR filter function
darkraxx 0:9621bbc04b9b 41 void firFixed( int16_t *coeffs, int16_t *input, int16_t *output,
darkraxx 0:9621bbc04b9b 42 int length, int filterLength, int bias, int16_t *buffer );
darkraxx 0:9621bbc04b9b 43 /////// SIGN FUNCTION
darkraxx 0:9621bbc04b9b 44 int sign(int16_t x);
darkraxx 0:9621bbc04b9b 45 /////// QUATERNION product FUNCTION
darkraxx 0:9621bbc04b9b 46 void Qprod(float *p, float *q, float *r );
darkraxx 0:9621bbc04b9b 47 /////// QUATERNION rotation FUNCTION
darkraxx 0:9621bbc04b9b 48 void Qrotate(float *v, float *q, float *out );
darkraxx 0:9621bbc04b9b 49 /////// Sensor set_up function
darkraxx 0:9621bbc04b9b 50 void sensor_setup();
darkraxx 0:9621bbc04b9b 51 /////// MPU sensor axes alignment and offset correction
darkraxx 0:9621bbc04b9b 52 void MPUrot_offset(int16_t *Data, char *rawData, int16_t *offset);
darkraxx 0:9621bbc04b9b 53 /////// Magnetic sensor offset correction
darkraxx 0:9621bbc04b9b 54 void Mag_offset(int16_t *M_read, char *data, int16_t *offset);
darkraxx 0:9621bbc04b9b 55 /////// Sensors reading
darkraxx 0:9621bbc04b9b 56 void Read_Mag(int16_t *M_read);
darkraxx 0:9621bbc04b9b 57 void Read_Acc(int16_t *A_read);
darkraxx 0:9621bbc04b9b 58 void Read_Gyr(int16_t *G_read);
darkraxx 0:9621bbc04b9b 59 /////// Madgwick Quaternion update high-level function
darkraxx 0:9621bbc04b9b 60 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);
darkraxx 0:9621bbc04b9b 61 /////// Filtered magnetic signals envelope extraction
darkraxx 0:9621bbc04b9b 62 void Env_extraction(int16_t *E, int16_t *buffer_rot, int16_t *max, int16_t *p_i, double *R, double *ph, int16_t *S);
darkraxx 0:9621bbc04b9b 63 #endif
darkraxx 0:9621bbc04b9b 64 //=====================================================================================================
darkraxx 0:9621bbc04b9b 65 // End of file
darkraxx 0:9621bbc04b9b 66 //=====================================================================================================