aleks/hoverboard-sideboard-hack-STM
1
0
Fork 0
mirror of https://github.com/EFeru/hoverboard-sideboard-hack-STM.git synced 2025-07-27 09:19:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added sensor orientation matrix

Browse Source 
- set the matrix in mpu6050.c according to sensor installation
This commit is contained in:
EmanuelFeru 2020-02-23 14:54:54 +01:00
parent cefe805dbf
commit e21f61ec2e
2 changed files with 46 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Inc/mpu6050.h
View File

@ -122,6 +122,8 @@ int mpu_run_self_test(long *gyro, long *accel);
int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug); int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug);
void mpu_start_self_test(void); void mpu_start_self_test(void);
void mpu_setup_gyro(void); void mpu_setup_gyro(void);
unsigned short inv_row_2_scale(const signed char *row);
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
/* MPU configuration */ /* MPU configuration */
int mpu_config(void); int mpu_config(void);

50
Src/mpu6050.c
View File

@ -44,6 +44,11 @@
*/ */
static signed char MPU_ORIENTATION[9] = {1, 0, 0, // [-] MPU Sensor orientation matrix: set this according to the sensor installation
0, 1, 0,
0, 0, 1};
#if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250 #if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
#error Which gyro are you using? Define MPUxxxx in config.h #error Which gyro are you using? Define MPUxxxx in config.h
#endif #endif
@ -3273,13 +3278,9 @@ void mpu_start_self_test(void)
} }
/**
* @}
*/
//========================================================================================= //=========================================================================================
//=========================================================================================
struct hal_s { struct hal_s {
unsigned char lp_accel_mode; unsigned char lp_accel_mode;
unsigned char sensors; unsigned char sensors;
@ -3332,7 +3333,44 @@ void mpu_setup_gyro(void)
} }
} }
unsigned short inv_row_2_scale(const signed char *row)
{
unsigned short b;
if (row[0] > 0)
b = 0;
else if (row[0] < 0)
b = 4;
else if (row[1] > 0)
b = 1;
else if (row[1] < 0)
b = 5;
else if (row[2] > 0)
b = 2;
else if (row[2] < 0)
b = 6;
else
b = 7; // error
return b;
}
/*
* This function converts the Sensor orientation matric to a scalar
*/
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{
unsigned short scalar;
scalar = inv_row_2_scale(mtx);
scalar |= inv_row_2_scale(mtx + 3) << 3;
scalar |= inv_row_2_scale(mtx + 6) << 6;
return scalar;
}
/**
* @}
*/
/* =========================== MPU-6050 Configuration =========================== */ /* =========================== MPU-6050 Configuration =========================== */
int mpu_config(void) int mpu_config(void)
@ -3400,7 +3438,7 @@ int mpu_config(void)
consoleLog(" FAIL (DMP) --\r\n"); consoleLog(" FAIL (DMP) --\r\n");
return -1; return -1;
} }
// dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_pdata.orientation)); dmp_set_orientation(inv_orientation_matrix_to_scalar(MPU_ORIENTATION));
dmp_register_tap_cb(mpu_tap_func); dmp_register_tap_cb(mpu_tap_func);
dmp_register_android_orient_cb(mpu_android_orient_func); dmp_register_android_orient_cb(mpu_android_orient_func);
/* /*