Mercurial > public > ostc4
annotate Discovery/Src/tInfoCompass.c @ 1022:ca713e199f22 GasConsumption
Add model ID 0x44 for the OSTC5 to the 0x60 (HARDWARE2) endpoint. The OSTC4 stays at model ID 0x43.
(mikeller)
| author | heinrichsweikamp |
|---|---|
| date | Sun, 29 Jun 2025 13:45:56 +0200 |
| 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 } |