Mercurial > public > ostc4

comparison Small_CPU/Src/compass.c @ 1007:65d35e66efb9 GasConsumption

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Improve compass calibration dialog: The previous calibration dialog showed some "magic" numbers and a 60 second count down. The new version is trying to guide the user through the calibration process: first rotate pitch, then roll and at last yaw angle. A step to the next angle is taken when enough data per angle is collected (change from red to green). To enable the yaw visualization a simple calibration is done while rotating the axis. The function behind the calibration was not modified => the suggested process can be ignored and the same handling as the with old dialog may be applied. With the new process the dialog may be left early. Anyhow it will still be left after 60 seconds and the fine calibration is performed in the same way as before.
author Ideenmodellierer
date Mon, 05 May 2025 21:02:34 +0200
parents 08af5d707c5a
children 08f76d3e9856
comparison
equal deleted inserted replaced
1006:75f958ca5d0e 1007:65d35e66efb9
82 82
83 /// Found soft-iron calibration values, deduced from already filtered values 83 /// Found soft-iron calibration values, deduced from already filtered values
84 int16_t compass_CX_f; ///< calibration value 84 int16_t compass_CX_f; ///< calibration value
85 int16_t compass_CY_f; ///< calibration value 85 int16_t compass_CY_f; ///< calibration value
86 int16_t compass_CZ_f; ///< calibration value 86 int16_t compass_CZ_f; ///< calibration value
87
88
89 float compass_DX_min;
90 float compass_DX_max;
91
92 float compass_DY_min;
93 float compass_DY_max;
94
95 float compass_DZ_min;
96 float compass_DZ_max;
97
87 98
88 99
89 /// The (filtered) components of the accelerometer sensor 100 /// The (filtered) components of the accelerometer sensor
90 int16_t accel_DX_f; ///< output from sensor 101 int16_t accel_DX_f; ///< output from sensor
91 int16_t accel_DY_f; ///< output from sensor 102 int16_t accel_DY_f; ///< output from sensor
1068 /// input is accel_DX_f, accel_DY_f, accel_DZ_f 1079 /// input is accel_DX_f, accel_DY_f, accel_DZ_f
1069 /// output is compass_pitch and compass_roll 1080 /// output is compass_pitch and compass_roll
1070 // =============================================================================== 1081 // ===============================================================================
1071 void compass_calc_roll_pitch_only(void) 1082 void compass_calc_roll_pitch_only(void)
1072 { 1083 {
1073 float sinPhi, cosPhi; 1084 float sinPhi, cosPhi, sinTeta, cosTeta;
1074 float Phi, Teta; 1085 float Phi, Teta;
1086 float mx_corr, my_corr;
1087
1088 float offsetX, offsetY, offsetZ;
1089 float scaleX, scaleY,scaleZ;
1090
1091 float local_DX = compass_DX_f;
1092 float local_DY = compass_DY_f;
1093 float local_DZ = compass_DZ_f;
1094
1095
1096 if(local_DX < compass_DX_min) compass_DX_min = local_DX;
1097 if(local_DY < compass_DY_min) compass_DY_min = local_DY;
1098 if(local_DZ < compass_DZ_min) compass_DZ_min = local_DZ;
1099
1100 if(local_DX > compass_DX_max) compass_DX_max = local_DX;
1101 if(local_DY > compass_DY_max) compass_DY_max = local_DY;
1102 if(local_DZ > compass_DZ_max) compass_DZ_max = local_DZ;
1075 1103
1076 //---- Calculate sine and cosine of roll angle Phi ----------------------- 1104 //---- Calculate sine and cosine of roll angle Phi -----------------------
1077 Phi= atan2f(accel_DY_f, accel_DZ_f) ; 1105 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
1078 compass_roll = Phi * 180.0f /PI; 1106 compass_roll = Phi * 180.0f /PI;
1079 sinPhi = sinf(Phi); 1107 sinPhi = sinf(Phi);
1080 cosPhi = cosf(Phi); 1108 cosPhi = cosf(Phi);
1081 1109
1082 //---- calculate sin and cosine of pitch angle Theta --------------------- 1110 Teta = atan2((double)accel_DX_f, sqrt((double)accel_DY_f * accel_DY_f + (double)accel_DZ_f * accel_DZ_f));
1083 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi)); 1111 compass_pitch = Teta * (180.0 / PI);
1084 compass_pitch = Teta * 180.0f /PI; 1112
1113 sinTeta = sinf(Teta);
1114 cosTeta = cosf(Teta);
1115
1116 offsetX = (compass_DX_max + compass_DX_min) / 2.0;
1117 offsetY = (compass_DY_max + compass_DY_min) / 2.0;
1118 offsetZ = (compass_DZ_max + compass_DZ_min) / 2.0;
1119
1120 scaleX = 2.0 / (compass_DX_max - compass_DX_min);
1121 scaleY = 2.0 / (compass_DY_max - compass_DY_min);
1122 scaleZ = 2.0 / (compass_DZ_max - compass_DZ_min);
1123
1124 local_DX -= offsetX;
1125 local_DX *= scaleX;
1126
1127 local_DY -= offsetY;
1128 local_DY *= scaleY;
1129
1130 local_DZ -= offsetZ;
1131 local_DZ *= scaleZ;
1132
1133 mx_corr = local_DX * cosTeta + local_DZ * sinTeta;
1134 my_corr = local_DX * sinPhi * sinTeta + local_DY * cosPhi - local_DZ * sinPhi * cosTeta;
1135
1136 compass_heading = atan2(my_corr, mx_corr) * (180.0 / PI);
1137
1138
1139 if (compass_heading < 0) {
1140 compass_heading += 360.0;
1141 }
1085 } 1142 }
1086 1143
1087 1144
1088 // =============================================================================== 1145 // ===============================================================================
1089 // compass_calc 1146 // compass_calc
1170 g->Su = g->Sv = g->Sw = 0.0; 1227 g->Su = g->Sv = g->Sw = 0.0;
1171 g->Suu = g->Svv = g->Sww = g->Suv = g->Suw = g->Svw = 0.0; 1228 g->Suu = g->Svv = g->Sww = g->Suv = g->Suw = g->Svw = 0.0;
1172 g->Suuu = g->Svvv = g->Swww = 0.0; 1229 g->Suuu = g->Svvv = g->Swww = 0.0;
1173 g->Suuv = g->Suuw = g->Svvu = g->Svvw = g->Swwu = g->Swwv = 0.0; 1230 g->Suuv = g->Suuw = g->Svvu = g->Svvw = g->Swwu = g->Swwv = 0.0;
1174 compass_CX_f = compass_CY_f = compass_CZ_f = 0.0; 1231 compass_CX_f = compass_CY_f = compass_CZ_f = 0.0;
1232
1233 compass_DX_min = 10000;
1234 compass_DY_min = 10000;
1235 compass_DZ_min = 10000;
1236
1237 compass_DX_max = -10000;
1238 compass_DY_max = -10000;
1239 compass_DZ_max = -10000;
1175 } 1240 }
1176 1241
1177 1242
1178 // =============================================================================== 1243 // ===============================================================================
1179 // compass_add_calibration 1244 // compass_add_calibration