Mercurial > public > ostc4
annotate Discovery/Src/tInfoCompass.c @ 1046:1d7c7a36df15 GasConsumption
Bugfix OSTC5 BT and enabling fast mode:
The OSTC5 BT was operating at default speed of 115200. To enable the faster communication some init steps have been added to set speed to 460800. Having the UART enabled while the module was shut down caused problems during initialisation. To avoid these the BT UART is now initialized after the the module is powered on and deinitialized while the module is switched off.
| author | Ideenmodellierer |
|---|---|
| date | Fri, 14 Nov 2025 18:54:20 +0100 |
| parents | 65d35e66efb9 |
| children |
| rev | line source |
|---|---|
| 38 | 1 /////////////////////////////////////////////////////////////////////////////// |
| 2 /// -*- coding: UTF-8 -*- | |
| 3 /// | |
| 4 /// \file Discovery/Src/tInfoCompass.c | |
| 5 /// \brief there is only compass_DX_f, compass_DY_f, compass_DZ_f output during this mode | |
| 6 /// \author heinrichs weikamp gmbh | |
| 7 /// \date 23-Feb-2015 | |
| 8 /// | |
| 9 /// \details | |
| 10 /// | |
| 11 /// $Id$ | |
| 12 /////////////////////////////////////////////////////////////////////////////// | |
| 13 /// \par Copyright (c) 2014-2018 Heinrichs Weikamp gmbh | |
| 14 /// | |
| 15 /// This program is free software: you can redistribute it and/or modify | |
| 16 /// it under the terms of the GNU General Public License as published by | |
| 17 /// the Free Software Foundation, either version 3 of the License, or | |
| 18 /// (at your option) any later version. | |
| 19 /// | |
| 20 /// This program is distributed in the hope that it will be useful, | |
| 21 /// but WITHOUT ANY WARRANTY; without even the implied warranty of | |
| 22 /// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
| 23 /// GNU General Public License for more details. | |
| 24 /// | |
| 25 /// You should have received a copy of the GNU General Public License | |
| 26 /// along with this program. If not, see <http://www.gnu.org/licenses/>. | |
| 27 ////////////////////////////////////////////////////////////////////////////// | |
| 28 | |
| 29 /* Includes ------------------------------------------------------------------*/ | |
|
243
b7b481df4f22
debug: add SPI error counter to compass calibration
Jan Mulder <jlmulder@xs4all.nl>
parents:
38
diff
changeset
|
30 |
| 1007 | 31 |
|
243
b7b481df4f22
debug: add SPI error counter to compass calibration
Jan Mulder <jlmulder@xs4all.nl>
parents:
38
diff
changeset
|
32 #include "gfx_fonts.h" |
|
b7b481df4f22
debug: add SPI error counter to compass calibration
Jan Mulder <jlmulder@xs4all.nl>
parents:
38
diff
changeset
|
33 #include "tHome.h" |
|
b7b481df4f22
debug: add SPI error counter to compass calibration
Jan Mulder <jlmulder@xs4all.nl>
parents:
38
diff
changeset
|
34 #include "tInfo.h" |
| 38 | 35 #include "tInfoCompass.h" |
| 36 | |
| 37 #include <string.h> | |
| 1007 | 38 #include <math.h> |
| 39 | |
| 40 #define PI 3.14159265358979323846 | |
| 41 | |
| 42 #define PITCH (0) | |
| 43 #define ROLL (1) | |
| 44 #define YAW (2) | |
| 38 | 45 |
| 46 /* Private variables ---------------------------------------------------------*/ | |
| 47 | |
| 1007 | 48 static uint16_t tInfoCompassTimeout = 0; |
| 49 static int16_t minMaxCompassDX[3][2] = { 0 }; | |
| 50 | |
| 51 static axisIndicator_t axis[3]; | |
| 52 static uint8_t activeAxis = PITCH; | |
| 53 static uint32_t checkTick = 0; | |
| 38 | 54 |
| 55 /* Exported functions --------------------------------------------------------*/ | |
| 56 void openInfo_Compass(void) | |
| 57 { | |
| 1007 | 58 uint16_t angle = 0; |
| 59 uint8_t axisIndex = 0; | |
| 38 | 60 set_globalState(StICOMPASS); |
| 61 tInfoCompassTimeout = settingsGetPointer()->timeoutInfoCompass; | |
| 62 tInfoCompassTimeout *= 10; | |
| 63 | |
| 1007 | 64 for(axisIndex = 0; axisIndex < 3; axisIndex++) |
| 65 { | |
| 66 memset(axis[axisIndex].check,0, 360); | |
| 67 } | |
| 68 for(angle = 170; angle < 360; angle++) | |
| 69 { | |
| 70 axis[PITCH].check[angle] = 1; /* pitch only from -90 to +90 */ | |
| 71 } | |
| 72 axis[YAW].coord.x = 100; | |
| 73 axis[YAW].coord.y = 70; | |
| 74 axis[PITCH].coord.x = 100; | |
| 75 axis[PITCH].coord.y = 300; | |
| 76 axis[ROLL].coord.x = 100; | |
| 77 axis[ROLL].coord.y = 200; | |
| 78 | |
| 79 axis[YAW].eclipse.x = 50; | |
| 80 axis[YAW].eclipse.y = 50; | |
| 81 axis[PITCH].eclipse.x = 20; | |
| 82 axis[PITCH].eclipse.y = 50; | |
| 83 axis[ROLL].eclipse.x = 50; | |
| 84 axis[ROLL].eclipse.y = 20; | |
| 85 | |
| 38 | 86 for(int i = 0; i<3;i ++) |
| 87 { | |
| 88 minMaxCompassDX[i][0] = 999; | |
| 89 minMaxCompassDX[i][1] = -999; | |
| 90 } | |
| 1007 | 91 checkTick = HAL_GetTick(); |
| 92 | |
| 93 activeAxis = PITCH; | |
| 38 | 94 } |
| 95 | |
| 1007 | 96 void getElipsePoint(uint16_t elipseX , int16_t elipseY, int16_t degree, int16_t *x, int16_t *y) |
| 97 { | |
| 98 double rad = degree * (PI / 180.0); | |
| 99 *x = (int16_t) (elipseX * cos(rad)); | |
| 100 *y = (int16_t) (elipseY * sin(rad)); | |
| 101 } | |
| 38 | 102 // =============================================================================== |
| 103 // refreshInfo_Compass | |
| 104 /// @brief there is only compass_DX_f, compass_DY_f, compass_DZ_f output during this mode | |
| 105 /// the accel is not called during this process | |
| 106 // =============================================================================== | |
|
243
b7b481df4f22
debug: add SPI error counter to compass calibration
Jan Mulder <jlmulder@xs4all.nl>
parents:
38
diff
changeset
|
107 void refreshInfo_Compass(GFX_DrawCfgScreen s) |
| 38 | 108 { |
| 1007 | 109 static int16_t cursorAngle = 0; |
| 110 | |
| 111 int16_t angle = 0.0; | |
| 112 int16_t drawX = 0; | |
| 113 int16_t drawY = 0; | |
| 114 uint8_t color = 0; | |
| 115 point_t start; | |
| 116 point_t stop; | |
| 117 point_t center; | |
| 118 | |
| 119 int8_t offset = 0; | |
| 120 uint8_t axisIndex = 0; | |
| 121 int16_t index = 0; | |
| 122 uint8_t textIndex = 0; | |
|
243
b7b481df4f22
debug: add SPI error counter to compass calibration
Jan Mulder <jlmulder@xs4all.nl>
parents:
38
diff
changeset
|
123 |
|
b7b481df4f22
debug: add SPI error counter to compass calibration
Jan Mulder <jlmulder@xs4all.nl>
parents:
38
diff
changeset
|
124 tHome_show_lost_connection_count(&s); |
| 38 | 125 tInfoCompassTimeout--; |
| 126 if(tInfoCompassTimeout == 0) | |
| 127 { | |
| 128 exitInfo(); | |
| 129 return; | |
| 130 } | |
| 131 | |
| 132 char text[80]; | |
| 133 | |
| 134 int16_t compassValues[3]; | |
| 135 | |
| 136 compassValues[0] = stateUsed->lifeData.compass_DX_f; | |
| 137 compassValues[1] = stateUsed->lifeData.compass_DY_f; | |
| 138 compassValues[2] = stateUsed->lifeData.compass_DZ_f; | |
| 139 | |
| 1007 | 140 /* draw indicator */ |
| 141 for(axisIndex = 0; axisIndex < 3; axisIndex++) | |
| 142 { | |
| 143 switch(axisIndex) | |
| 144 { | |
| 145 default: | |
| 146 case YAW: index = (uint16_t)(stateUsed->lifeData.compass_heading); | |
| 147 textIndex = snprintf(text,80,"yaw %.1f", stateUsed->lifeData.compass_heading); | |
| 148 | |
| 149 if((tInfoCompassTimeout < 50) && (activeAxis == YAW)) | |
| 150 { | |
| 151 snprintf(&text[textIndex],80,"\023\t(%i, %i)", minMaxCompassDX[YAW][0], minMaxCompassDX[YAW][1]); | |
| 152 } | |
| 153 | |
| 154 start.x = axis[axisIndex].coord.x - 1; | |
| 155 start.y = axis[axisIndex].coord.y; | |
| 156 stop.x = axis[axisIndex].coord.x + 1; | |
| 157 stop.y = axis[axisIndex].coord.y; | |
| 158 break; | |
| 159 case PITCH: index = (uint16_t)(stateUsed->lifeData.compass_pitch + 90); | |
| 160 textIndex = snprintf(text,80,"pitch %.1f", stateUsed->lifeData.compass_pitch); | |
| 161 if((tInfoCompassTimeout < 50) && (activeAxis == YAW)) | |
| 162 { | |
| 163 snprintf(&text[textIndex],80,"\023\t(%i, %i)", minMaxCompassDX[PITCH][0], minMaxCompassDX[PITCH][1]); | |
| 164 } | |
| 165 start.x = axis[axisIndex].coord.x - 30; | |
| 166 start.y = axis[axisIndex].coord.y; | |
| 167 stop.x = axis[axisIndex].coord.x + 30; | |
| 168 stop.y = axis[axisIndex].coord.y; | |
| 169 | |
| 170 break; | |
| 171 case ROLL: index = (uint16_t)(stateUsed->lifeData.compass_roll + 180); | |
| 172 textIndex = snprintf(text,80,"roll %.1f", stateUsed->lifeData.compass_roll); | |
| 173 if((tInfoCompassTimeout < 50) && (activeAxis == YAW)) | |
| 174 { | |
| 175 snprintf(&text[textIndex],80,"\023\t(%i, %i)", minMaxCompassDX[ROLL][0], minMaxCompassDX[ROLL][1]); | |
| 176 } | |
| 177 start.x = axis[axisIndex].coord.x; | |
| 178 start.y = axis[axisIndex].coord.y - 30; | |
| 179 stop.x = axis[axisIndex].coord.x; | |
| 180 stop.y = axis[axisIndex].coord.y +30; | |
| 181 break; | |
| 182 } | |
| 183 if(settingsGetPointer()->FlipDisplay) | |
| 184 { | |
| 185 start.x = 800 - start.x; | |
| 186 stop.x = 800 - stop.x; | |
| 187 start.y = 480 - start.y; | |
| 188 stop.y = 480 - stop.y; | |
| 189 } | |
| 190 if(activeAxis == axisIndex) /* only check one axis at a time. reason: yaw will be unstable at the beginning of calibration */ | |
| 191 { | |
| 192 for(offset = -6; offset <= 6; offset++) /* it is hard to hit every single angle and the resolution is not needed */ | |
| 193 { | |
| 194 if(( (index + offset) >= 0) && (index + offset < 360)) | |
| 195 { | |
| 196 axis[axisIndex].check[index + offset] = 1; /* => check surrounding angles as well */ | |
| 197 } | |
| 198 } | |
| 199 } | |
| 200 if(axisIndex == activeAxis) | |
| 201 { | |
| 202 color = CLUT_InfoCompass; | |
| 203 getElipsePoint(axis[axisIndex].eclipse.x,axis[axisIndex].eclipse.y,cursorAngle, &drawX, &drawY); | |
| 204 center.x = axis[axisIndex].coord.x + drawX; | |
| 205 center.y = axis[axisIndex].coord.y + drawY; | |
| 206 | |
| 207 t_Info_draw_circle(center, 4, CLUT_Font020); | |
| 208 cursorAngle += 15; | |
| 209 if(cursorAngle >= 360) | |
| 210 { | |
| 211 cursorAngle = 0; | |
| 212 } | |
| 213 } | |
| 214 else | |
| 215 { | |
| 216 color = CLUT_Font021; | |
| 217 } | |
| 218 tInfo_write_content_simple( 200, 600, 480 - axis[axisIndex].coord.y - 35 , &FontT42, text, color); | |
| 219 tInfo_draw_colorline(start, stop, CLUT_Font020); | |
| 220 for(angle = 0; angle < 360; angle = angle + 3) | |
| 221 { | |
| 222 if(axis[axisIndex].check[angle]) | |
| 223 { | |
| 224 color = CLUT_NiceGreen; | |
| 225 } | |
| 226 else | |
| 227 { | |
| 228 color = CLUT_WarningRed; | |
| 229 } | |
| 230 getElipsePoint(axis[axisIndex].eclipse.x,axis[axisIndex].eclipse.y,angle, &drawX, &drawY); | |
| 231 tInfo_drawPixel(axis[axisIndex].coord.x + drawX, axis[axisIndex].coord.y + drawY,color); | |
| 232 if((axisIndex == PITCH) && (angle == 180)) /* pitch only from -90 to +90 */ | |
| 233 { | |
| 234 break; | |
| 235 } | |
| 236 } | |
| 237 | |
| 238 } | |
| 239 | |
| 38 | 240 for(int i = 0; i<3;i ++) |
| 241 { | |
| 242 // do not accept zero | |
| 243 if(minMaxCompassDX[i][0] == 0) | |
| 244 minMaxCompassDX[i][0] = compassValues[i]; | |
| 245 | |
| 246 // do not accept zero | |
| 247 if(minMaxCompassDX[i][1] == 0) | |
| 248 minMaxCompassDX[i][1] = compassValues[i]; | |
| 249 | |
| 250 if(compassValues[i] < minMaxCompassDX[i][0]) | |
| 251 minMaxCompassDX[i][0] = compassValues[i]; | |
| 252 | |
| 253 if(compassValues[i] > minMaxCompassDX[i][1]) | |
| 254 minMaxCompassDX[i][1] = compassValues[i]; | |
| 255 } | |
| 256 | |
| 257 snprintf(text,80,"Time left: %u s",(tInfoCompassTimeout+9)/10); | |
|
243
b7b481df4f22
debug: add SPI error counter to compass calibration
Jan Mulder <jlmulder@xs4all.nl>
parents:
38
diff
changeset
|
258 tInfo_write_content_simple( 20,800, 25, &FontT42, text, CLUT_InfoCompass); |
| 38 | 259 |
| 1007 | 260 |
| 261 if(time_elapsed_ms(checkTick, HAL_GetTick() > 1000)) | |
| 38 | 262 { |
| 1007 | 263 index = 0; |
| 264 for(angle = 0; angle < 360; angle++) | |
| 265 { | |
| 266 if(axis[activeAxis].check[angle]) | |
| 267 { | |
| 268 index++; | |
| 269 } | |
| 270 } | |
| 271 if(index > 350) | |
| 272 { | |
| 273 if(activeAxis < 2) | |
| 274 { | |
| 275 activeAxis++; | |
| 276 } | |
| 277 else | |
| 278 { | |
| 279 if(tInfoCompassTimeout > 50) | |
| 280 { | |
| 281 tInfoCompassTimeout = 50; /* reduce exit time to five seconds */ | |
| 282 } | |
| 283 } | |
| 284 } | |
| 285 | |
| 286 | |
| 287 checkTick = HAL_GetTick(); | |
| 38 | 288 } |
| 289 } |