Mercurial > public > ostc4
annotate Discovery/Src/data_central.c @ 664:667093daa937 Betatest
Stability improvment bluetooth startup:
The previous implementation expected a default setup of the Bluetooth module. Deviations from the default expectation caused the init function to stop. The new implementation is able to fix wrong baud rate setting (reset baudrate to default 115200).
In addition the function evaluating the answers of the module is not able to derive the status out of a data stream.
| author | Ideenmodellierer |
|---|---|
| date | Tue, 21 Dec 2021 19:36:41 +0100 |
| parents | 1b995079c045 |
| children | 8775d3dc6325 |
| rev | line source |
|---|---|
| 38 | 1 /** |
| 2 ****************************************************************************** | |
| 3 * @copyright heinrichs weikamp | |
| 4 * @file data_central.c | |
| 5 * @author heinrichs weikamp gmbh | |
| 6 * @date 10-November-2014 | |
| 7 * @version V1.0.2 | |
| 8 * @since 10-Nov-2014 | |
| 9 * @brief All the data EXCEPT | |
| 10 * - settings (settings.c) | |
| 11 * feste Werte, die nur an der Oberfl�che ge�ndert werden | |
| 12 * - dataIn and dataOut (data_exchange.h and data_exchange_main.c) | |
| 13 * Austausch mit Small CPU | |
| 14 * @bug | |
| 15 * @warning | |
| 16 @verbatim | |
| 17 ============================================================================== | |
| 18 ##### SDiveState Real and Sim ##### | |
| 19 ============================================================================== | |
| 20 [..] SDiveSettings | |
| 21 copy of parts of Settings that are necessary during the dive | |
| 22 and could be modified during the dive without post dive changes. | |
| 23 | |
| 24 [..] SLifeData | |
| 25 written in DataEX_copy_to_LifeData(); | |
| 26 block 1 "lifedata" set by SmallCPU in stateReal | |
| 27 block 2 "actualGas" set by main CPU from user input and send to Small CPU | |
| 28 block 3 "calculated data" set by main CPU based on "lifedata" | |
| 29 | |
| 30 [..] SVpm | |
| 31 | |
| 32 [..] SEvents | |
| 33 | |
| 34 [..] SDecoinfo | |
| 35 | |
| 36 [..] mode | |
| 37 set by SmallCPU in stateReal, can be surface, dive, ... | |
| 38 | |
| 39 [..] data_old__lost_connection_to_slave | |
| 40 set by DataEX_copy_to_LifeData(); | |
| 41 | |
| 42 ============================================================================== | |
| 43 ##### SDiveState Deco ##### | |
| 44 ============================================================================== | |
| 45 [..] kjbkldafj�lasdfjasdf | |
| 46 | |
| 47 ============================================================================== | |
| 48 ##### decoLock ##### | |
| 49 ============================================================================== | |
| 50 [..] The handler that synchronizes the data between IRQ copy and main deco loop | |
| 51 | |
| 52 | |
| 53 @endverbatim | |
| 54 ****************************************************************************** | |
| 55 * @attention | |
| 56 * | |
| 57 * <h2><center>© COPYRIGHT(c) 2015 heinrichs weikamp</center></h2> | |
| 58 * | |
| 59 ****************************************************************************** | |
| 60 */ | |
| 61 | |
| 62 /* Includes ------------------------------------------------------------------*/ | |
| 63 #include <string.h> | |
|
539
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
64 #include <math.h> |
| 38 | 65 #include "data_central.h" |
| 66 #include "calc_crush.h" | |
| 67 #include "decom.h" | |
| 68 #include "stm32f4xx_hal.h" | |
| 69 #include "settings.h" | |
| 70 #include "data_exchange_main.h" | |
| 71 #include "ostc.h" // for button adjust on hw testboard 1 | |
| 72 #include "tCCR.h" | |
| 73 #include "crcmodel.h" | |
| 662 | 74 #include "configuration.h" |
| 38 | 75 |
|
225
2bb1db22b5f5
cleanup: random set of cleanups
Jan Mulder <jlmulder@xs4all.nl>
parents:
149
diff
changeset
|
76 static SDiveState stateReal = { 0 }; |
| 38 | 77 SDiveState stateSim = { 0 }; |
| 78 SDiveState stateDeco = { 0 }; | |
| 79 | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
80 static SDevice stateDevice = |
| 38 | 81 { |
| 82 /* max is 0x7FFFFFFF, min is 0x80000000 but also defined in stdint.h :-) */ | |
| 83 | |
| 84 /* count, use 0 */ | |
| 85 .batteryChargeCompleteCycles.value_int32 = 0, | |
| 86 .batteryChargeCycles.value_int32 = 0, | |
| 87 .diveCycles.value_int32 = 0, | |
| 88 .hoursOfOperation.value_int32 = 0, | |
| 89 | |
| 90 /* max values, use min. */ | |
| 91 .temperatureMaximum.value_int32 = INT32_MIN, | |
| 92 .depthMaximum.value_int32 = INT32_MIN, | |
| 93 | |
| 94 /* min values, use max. */ | |
| 95 .temperatureMinimum.value_int32 = INT32_MAX, | |
| 96 .voltageMinimum.value_int32 = INT32_MAX, | |
| 97 }; | |
| 98 | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
99 static SVpmRepetitiveData stateVPM = |
| 38 | 100 { |
| 101 .repetitive_variables_not_valid = 1, | |
| 102 .is_data_from_RTE_CPU = 0, | |
| 103 }; | |
| 104 | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
105 const SDiveState *stateUsed = &stateReal; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
106 SDiveState *stateUsedWrite = &stateReal; |
| 38 | 107 |
|
539
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
108 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
109 #define COMPASS_FRACTION (4.0f) /* delay till value changes to new actual */ |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
110 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
111 static float compass_compensated = 0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
112 |
| 38 | 113 void set_stateUsedToReal(void) |
| 114 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
115 stateUsed = stateUsedWrite = &stateReal; |
| 38 | 116 } |
| 117 | |
| 118 void set_stateUsedToSim(void) | |
| 119 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
120 stateUsed = stateUsedWrite = &stateSim; |
| 38 | 121 } |
| 122 | |
| 123 _Bool is_stateUsedSetToSim(void) | |
| 124 { | |
|
225
2bb1db22b5f5
cleanup: random set of cleanups
Jan Mulder <jlmulder@xs4all.nl>
parents:
149
diff
changeset
|
125 return stateUsed == &stateSim; |
| 38 | 126 } |
| 127 | |
| 128 const SDiveState * stateRealGetPointer(void) | |
| 129 { | |
| 130 return &stateReal; | |
| 131 } | |
| 132 | |
| 133 SDiveState * stateRealGetPointerWrite(void) | |
| 134 { | |
| 135 return &stateReal; | |
| 136 } | |
| 137 | |
| 138 | |
| 139 const SDiveState * stateSimGetPointer(void) | |
| 140 { | |
| 141 return &stateSim; | |
| 142 } | |
| 143 | |
| 144 | |
| 145 SDiveState * stateSimGetPointerWrite(void) | |
| 146 { | |
| 147 return &stateSim; | |
| 148 } | |
| 149 | |
| 150 | |
| 151 const SDevice * stateDeviceGetPointer(void) | |
| 152 { | |
| 153 return &stateDevice; | |
| 154 } | |
| 155 | |
| 156 | |
| 157 SDevice * stateDeviceGetPointerWrite(void) | |
| 158 { | |
| 159 return &stateDevice; | |
| 160 } | |
| 161 | |
| 162 | |
| 163 const SVpmRepetitiveData * stateVpmRepetitiveDataGetPointer(void) | |
| 164 { | |
| 165 return &stateVPM; | |
| 166 } | |
| 167 | |
| 168 | |
| 169 SVpmRepetitiveData * stateVpmRepetitiveDataGetPointerWrite(void) | |
| 170 { | |
| 171 return &stateVPM; | |
| 172 } | |
| 173 | |
| 174 | |
| 175 uint32_t time_elapsed_ms(uint32_t ticksstart,uint32_t ticksnow) | |
| 176 { | |
| 177 if(ticksstart <= ticksnow) | |
| 178 return ticksnow - ticksstart; | |
| 179 else | |
| 180 return 0xFFFFFFFF - ticksstart + ticksnow; | |
| 181 } | |
| 182 | |
| 183 | |
| 184 uint8_t decoLock = DECO_CALC_undefined; | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
185 |
|
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
186 static int descent_rate_meter_per_min = 20; |
|
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
187 static int max_depth = 70; |
|
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
188 static int bottom_time = 10; |
| 38 | 189 |
| 190 _Bool vpm_crush(SDiveState* pDiveState); | |
| 191 void setSimulationValues(int _ascent_rate_meter_per_min, int _descent_rate_meter_per_min, int _max_depth, int _bottom_time ) | |
| 192 { | |
| 193 descent_rate_meter_per_min = _descent_rate_meter_per_min; | |
| 194 max_depth = _max_depth; | |
| 195 bottom_time = _bottom_time; | |
| 196 } | |
| 197 | |
| 198 int current_second(void) { | |
| 199 | |
| 200 return HAL_GetTick() / 1000; | |
| 201 } | |
| 202 | |
| 203 #define OXY_ONE_SIXTIETH_PART 0.0166667f | |
| 204 | |
| 205 uint8_t calc_MOD(uint8_t gasId) | |
| 206 { | |
| 207 int16_t oxygen, maxppO2, result; | |
| 208 SSettings *pSettings; | |
| 209 | |
| 210 pSettings = settingsGetPointer(); | |
| 211 | |
| 212 oxygen = (int16_t)(pSettings->gas[gasId].oxygen_percentage); | |
| 213 | |
| 214 if(pSettings->gas[gasId].note.ub.deco > 0) | |
| 215 maxppO2 =(int16_t)(pSettings->ppO2_max_deco); | |
| 216 else | |
| 217 maxppO2 =(int16_t)(pSettings->ppO2_max_std); | |
| 218 | |
| 219 result = 10 * maxppO2; | |
| 220 result /= oxygen; | |
| 221 result -= 10; | |
| 222 | |
| 223 if(result < 0) | |
| 224 return 0; | |
| 225 | |
| 226 if(result > 255) | |
| 227 return 255; | |
| 228 | |
| 229 return result; | |
| 230 } | |
| 231 | |
| 232 float get_ambiant_pressure_simulation(long dive_time_seconds, float surface_pressure_bar ) | |
| 233 { | |
| 234 static | |
| 235 long descent_time; | |
| 236 float depth_meter; | |
| 237 | |
| 238 descent_time = 60 * max_depth / descent_rate_meter_per_min; | |
| 239 | |
| 240 if(dive_time_seconds <= descent_time) | |
| 241 { | |
| 242 depth_meter = ((float)(dive_time_seconds * descent_rate_meter_per_min)) / 60; | |
| 243 return surface_pressure_bar + depth_meter / 10; | |
| 244 } | |
| 245 //else if(dive_time_seconds <= (descent_time + bottom_time * 60)) | |
| 246 return surface_pressure_bar + max_depth / 10; | |
| 247 | |
| 248 | |
| 249 | |
| 250 } | |
| 251 | |
| 252 void UpdateLifeDataTest(SDiveState * pDiveState) | |
| 253 { | |
| 254 static int last_second = -1; | |
| 255 int now = current_second(); | |
| 256 if(last_second == now) | |
| 257 return; | |
| 258 last_second = now; | |
| 259 | |
| 260 pDiveState->lifeData.dive_time_seconds += 1; | |
| 261 pDiveState->lifeData.pressure_ambient_bar = get_ambiant_pressure_simulation(pDiveState->lifeData.dive_time_seconds,pDiveState->lifeData.pressure_surface_bar); | |
| 262 | |
| 263 pDiveState->lifeData.depth_meter = (pDiveState->lifeData.pressure_ambient_bar - pDiveState->lifeData.pressure_surface_bar) * 10.0f; | |
| 264 if(pDiveState->lifeData.max_depth_meter < pDiveState->lifeData.depth_meter) | |
| 265 pDiveState->lifeData.max_depth_meter = pDiveState->lifeData.depth_meter; | |
| 266 decom_tissues_exposure(1, &pDiveState->lifeData); | |
| 267 pDiveState->lifeData.ppO2 = decom_calc_ppO2( pDiveState->lifeData.pressure_ambient_bar, &pDiveState->lifeData.actualGas); | |
| 268 decom_oxygen_calculate_cns(& pDiveState->lifeData.cns, pDiveState->lifeData.ppO2); | |
| 269 | |
| 270 vpm_crush(pDiveState); | |
| 271 } | |
| 272 | |
| 273 | |
| 274 _Bool vpm_crush(SDiveState* pDiveState) | |
| 275 { | |
| 276 int i = 0; | |
| 277 static float starting_ambient_pressure = 0; | |
| 278 static float ending_ambient_pressure = 0; | |
| 279 static float time_calc_begin = -1; | |
| 280 static float initial_helium_pressure[16]; | |
| 281 static float initial_nitrogen_pressure[16]; | |
| 282 ending_ambient_pressure = pDiveState->lifeData.pressure_ambient_bar * 10; | |
| 283 | |
| 284 if((pDiveState->lifeData.dive_time_seconds <= 4) || (starting_ambient_pressure >= ending_ambient_pressure)) | |
| 285 { | |
| 286 time_calc_begin = pDiveState->lifeData.dive_time_seconds; | |
| 287 starting_ambient_pressure = pDiveState->lifeData.pressure_ambient_bar * 10; | |
| 288 for( i = 0; i < 16; i++) | |
| 289 { | |
| 290 initial_helium_pressure[i] = pDiveState->lifeData.tissue_helium_bar[i] * 10; | |
| 291 initial_nitrogen_pressure[i] = pDiveState->lifeData.tissue_nitrogen_bar[i] * 10; | |
| 292 } | |
| 293 return false; | |
| 294 } | |
| 295 if(pDiveState->lifeData.dive_time_seconds - time_calc_begin >= 4) | |
| 296 { | |
| 297 if(ending_ambient_pressure > starting_ambient_pressure + 0.5f) | |
| 298 { | |
| 299 float rate = (ending_ambient_pressure - starting_ambient_pressure) * 60 / 4; | |
| 300 calc_crushing_pressure(&pDiveState->lifeData, &pDiveState->vpm, initial_helium_pressure, initial_nitrogen_pressure, starting_ambient_pressure, rate); | |
| 301 | |
| 302 time_calc_begin = pDiveState->lifeData.dive_time_seconds; | |
| 303 starting_ambient_pressure = pDiveState->lifeData.pressure_ambient_bar * 10; | |
| 304 for( i = 0; i < 16; i++) | |
| 305 { | |
| 306 initial_helium_pressure[i] = pDiveState->lifeData.tissue_helium_bar[i] * 10; | |
| 307 initial_nitrogen_pressure[i] = pDiveState->lifeData.tissue_nitrogen_bar[i] * 10; | |
| 308 } | |
| 309 | |
| 310 return true; | |
| 311 } | |
| 312 | |
| 313 } | |
| 314 return false; | |
| 315 }; | |
| 316 | |
| 317 | |
| 318 void createDiveSettings(void) | |
| 319 { | |
| 662 | 320 int i; |
| 38 | 321 SSettings* pSettings = settingsGetPointer(); |
| 322 | |
| 323 setActualGasFirst(&stateReal.lifeData); | |
| 324 | |
| 325 stateReal.diveSettings.compassHeading = pSettings->compassBearing; | |
| 326 stateReal.diveSettings.ascentRate_meterperminute = 10; | |
| 327 | |
| 328 stateReal.diveSettings.diveMode = pSettings->dive_mode; | |
| 329 stateReal.diveSettings.CCR_Mode = pSettings->CCR_Mode; | |
| 662 | 330 if((stateReal.diveSettings.diveMode == DIVEMODE_PSCR) && (stateReal.diveSettings.CCR_Mode == CCRMODE_FixedSetpoint)) |
| 331 { | |
| 332 /* TODO: update selection of sensor used on/off (currently sensor/fixpoint). As PSCR has no fixed setpoint change to simulated ppo2 if sensors are not active */ | |
| 333 stateReal.diveSettings.CCR_Mode = CCRMODE_Simulation; | |
| 334 } | |
| 335 | |
| 336 if(isLoopMode(stateReal.diveSettings.diveMode)) | |
| 38 | 337 stateReal.diveSettings.ccrOption = 1; |
| 338 else | |
| 339 stateReal.diveSettings.ccrOption = 0; | |
| 340 memcpy(stateReal.diveSettings.gas, pSettings->gas,sizeof(pSettings->gas)); | |
| 341 memcpy(stateReal.diveSettings.setpoint, pSettings->setpoint,sizeof(pSettings->setpoint)); | |
| 342 stateReal.diveSettings.gf_high = pSettings->GF_high; | |
| 343 stateReal.diveSettings.gf_low = pSettings->GF_low; | |
| 344 stateReal.diveSettings.input_next_stop_increment_depth_bar = ((float)pSettings->stop_increment_depth_meter) / 10.0f; | |
| 345 stateReal.diveSettings.last_stop_depth_bar = ((float)pSettings->last_stop_depth_meter) / 10.0f; | |
| 346 stateReal.diveSettings.vpm_conservatism = pSettings->VPM_conservatism.ub.standard; | |
| 347 stateReal.diveSettings.deco_type.uw = pSettings->deco_type.uw; | |
| 348 stateReal.diveSettings.fallbackOption = pSettings->fallbackToFixedSetpoint; | |
| 349 stateReal.diveSettings.ppo2sensors_deactivated = pSettings->ppo2sensors_deactivated; | |
| 350 stateReal.diveSettings.future_TTS_minutes = pSettings->future_TTS; | |
| 351 | |
| 662 | 352 stateReal.diveSettings.pscr_lung_ratio = pSettings->pscr_lung_ratio; |
| 353 stateReal.diveSettings.pscr_o2_drop = pSettings->pscr_o2_drop; | |
| 354 | |
| 355 if(stateReal.diveSettings.diveMode == DIVEMODE_PSCR) | |
| 356 { | |
| 357 for(i=0; i<5; i++) | |
| 358 { | |
| 359 stateReal.diveSettings.decogaslist[i].pscr_factor = 1.0 / stateReal.diveSettings.pscr_lung_ratio * stateReal.diveSettings.pscr_o2_drop; | |
| 360 } | |
| 361 } | |
| 362 | |
| 38 | 363 decom_CreateGasChangeList(&stateReal.diveSettings, &stateReal.lifeData); // decogaslist |
| 364 stateReal.diveSettings.internal__pressure_first_stop_ambient_bar_as_upper_limit_for_gf_low_otherwise_zero = 0; | |
| 365 | |
| 366 /* for safety */ | |
| 367 stateReal.diveSettings.input_second_to_last_stop_depth_bar = stateReal.diveSettings.last_stop_depth_bar + stateReal.diveSettings.input_next_stop_increment_depth_bar; | |
| 368 /* and the proper calc */ | |
| 662 | 369 for(i = 1; i <10; i++) |
| 38 | 370 { |
| 371 if(stateReal.diveSettings.input_next_stop_increment_depth_bar * i > stateReal.diveSettings.last_stop_depth_bar) | |
| 372 { | |
| 373 stateReal.diveSettings.input_second_to_last_stop_depth_bar = stateReal.diveSettings.input_next_stop_increment_depth_bar * i; | |
| 374 break; | |
| 375 } | |
| 376 } | |
| 377 } | |
| 378 | |
| 379 | |
| 380 void copyDiveSettingsToSim(void) | |
| 381 { | |
| 382 memcpy(&stateSim, &stateReal, sizeof(stateReal)); | |
| 383 } | |
| 384 | |
| 385 | |
| 386 void copyVpmRepetetiveDataToSim(void) | |
| 387 { | |
| 388 SDiveState * pSimData = stateSimGetPointerWrite(); | |
| 389 const SVpmRepetitiveData * pVpmData = stateVpmRepetitiveDataGetPointer(); | |
| 390 | |
| 391 if(pVpmData->is_data_from_RTE_CPU) | |
| 392 { | |
| 393 for(int i=0; i<16;i++) | |
| 394 { | |
| 395 pSimData->vpm.adjusted_critical_radius_he[i] = pVpmData->adjusted_critical_radius_he[i]; | |
| 396 pSimData->vpm.adjusted_critical_radius_n2[i] = pVpmData->adjusted_critical_radius_n2[i]; | |
| 397 | |
| 398 pSimData->vpm.adjusted_crushing_pressure_he[i] = pVpmData->adjusted_crushing_pressure_he[i]; | |
| 399 pSimData->vpm.adjusted_crushing_pressure_n2[i] = pVpmData->adjusted_crushing_pressure_n2[i]; | |
| 400 | |
| 401 pSimData->vpm.initial_allowable_gradient_he[i] = pVpmData->initial_allowable_gradient_he[i]; | |
| 402 pSimData->vpm.initial_allowable_gradient_n2[i] = pVpmData->initial_allowable_gradient_n2[i]; | |
| 403 | |
| 404 pSimData->vpm.max_actual_gradient[i] = pVpmData->max_actual_gradient[i]; | |
| 405 } | |
| 406 pSimData->vpm.repetitive_variables_not_valid = pVpmData->repetitive_variables_not_valid; | |
| 407 } | |
| 408 } | |
| 409 | |
| 410 | |
| 662 | 411 |
| 412 | |
| 38 | 413 void updateSetpointStateUsed(void) |
| 414 { | |
| 662 | 415 if(!isLoopMode(stateReal.diveSettings.diveMode)) |
| 38 | 416 { |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
417 stateUsedWrite->lifeData.actualGas.setPoint_cbar = 0; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
418 stateUsedWrite->lifeData.ppO2 = decom_calc_ppO2(stateUsed->lifeData.pressure_ambient_bar, &stateUsed->lifeData.actualGas); |
| 38 | 419 } |
| 420 else | |
| 421 { | |
| 422 if(stateUsed->diveSettings.CCR_Mode == CCRMODE_Sensors) | |
| 423 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
424 stateUsedWrite->lifeData.actualGas.setPoint_cbar = get_ppO2SensorWeightedResult_cbar(); |
| 38 | 425 } |
| 662 | 426 #ifdef ENABLE_PSCR_MODE |
| 427 if(stateUsed->diveSettings.diveMode == DIVEMODE_PSCR) /* calculate a ppO2 value based on assumptions ( transfered approach from hwos code) */ | |
| 428 { | |
| 429 stateUsedWrite->lifeData.ppo2Simulated_bar = decom_calc_SimppO2_O2based(stateUsed->lifeData.pressure_ambient_bar, stateReal.diveSettings.gas[stateUsed->lifeData.actualGas.GasIdInSettings].oxygen_percentage, stateUsed->lifeData.actualGas.pscr_factor); | |
| 430 if(stateUsed->diveSettings.CCR_Mode == CCRMODE_Simulation) | |
| 431 { | |
| 432 stateUsedWrite->lifeData.actualGas.setPoint_cbar = stateUsedWrite->lifeData.ppo2Simulated_bar * 100; | |
| 433 } | |
| 434 } | |
| 435 #endif | |
| 436 /* limit calculated value to the physically possible if needed */ | |
| 38 | 437 if((stateUsed->lifeData.pressure_ambient_bar * 100) < stateUsed->lifeData.actualGas.setPoint_cbar) |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
438 stateUsedWrite->lifeData.ppO2 = stateUsed->lifeData.pressure_ambient_bar; |
| 38 | 439 else |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
440 stateUsedWrite->lifeData.ppO2 = ((float)stateUsed->lifeData.actualGas.setPoint_cbar) / 100; |
| 38 | 441 } |
| 442 } | |
| 443 | |
| 444 void setActualGasFirst(SLifeData *lifeData) | |
| 445 { | |
| 446 SSettings* pSettings = settingsGetPointer(); | |
| 447 uint8_t start = 0; | |
| 448 uint8_t gasId = 0; | |
| 449 uint8_t setpoint_cbar = 0; | |
| 450 | |
| 662 | 451 if(isLoopMode(pSettings->dive_mode)) |
| 38 | 452 { |
| 453 setpoint_cbar = pSettings->setpoint[1].setpoint_cbar; | |
| 454 start = NUM_OFFSET_DILUENT+1; | |
| 455 } | |
| 456 else | |
| 457 { | |
| 458 setpoint_cbar = 0; | |
| 459 start = 1; | |
| 460 } | |
| 461 | |
| 462 gasId = start; | |
| 463 for(int i=start;i<=NUM_GASES+start;i++) | |
| 464 { | |
| 465 if(pSettings->gas[i].note.ub.first) | |
| 466 { | |
| 467 gasId = i; | |
| 468 break; | |
| 469 } | |
| 470 } | |
| 471 setActualGas(lifeData, gasId, setpoint_cbar); | |
| 472 } | |
| 473 | |
| 474 void setActualGasAir(SLifeData *lifeData) | |
| 475 { | |
| 476 uint8_t nitrogen; | |
| 477 nitrogen = 79; | |
| 478 lifeData->actualGas.GasIdInSettings = 0; | |
| 479 lifeData->actualGas.nitrogen_percentage = nitrogen; | |
| 480 lifeData->actualGas.helium_percentage =0; | |
| 481 lifeData->actualGas.setPoint_cbar = 0; | |
| 482 lifeData->actualGas.change_during_ascent_depth_meter_otherwise_zero = 0; | |
| 662 | 483 lifeData->actualGas.AppliedDiveMode = stateUsed->diveSettings.diveMode; |
| 38 | 484 } |
| 485 | |
| 486 | |
| 487 void setActualGas(SLifeData *lifeData, uint8_t gasId, uint8_t setpoint_cbar) | |
| 488 { | |
| 489 SSettings* pSettings = settingsGetPointer(); | |
| 490 uint8_t nitrogen; | |
| 491 | |
| 492 nitrogen = 100; | |
| 493 nitrogen -= pSettings->gas[gasId].oxygen_percentage; | |
| 494 nitrogen -= pSettings->gas[gasId].helium_percentage; | |
| 495 | |
| 496 lifeData->actualGas.GasIdInSettings = gasId; | |
| 497 lifeData->actualGas.nitrogen_percentage = nitrogen; | |
| 498 lifeData->actualGas.helium_percentage = pSettings->gas[gasId].helium_percentage; | |
| 499 lifeData->actualGas.setPoint_cbar = setpoint_cbar; | |
| 500 lifeData->actualGas.change_during_ascent_depth_meter_otherwise_zero = 0; | |
| 662 | 501 lifeData->actualGas.AppliedDiveMode = pSettings->dive_mode; |
| 502 lifeData->actualGas.pscr_factor = 1.0 / pSettings->pscr_lung_ratio * pSettings->pscr_o2_drop; | |
| 503 if(isLoopMode(pSettings->dive_mode) && (gasId > NUM_OFFSET_DILUENT)) | |
| 38 | 504 lifeData->lastDiluent_GasIdInSettings = gasId; |
| 505 } | |
| 506 | |
| 507 | |
| 508 void setActualGas_DM(SLifeData *lifeData, uint8_t gasId, uint8_t setpoint_cbar) | |
| 509 { | |
| 510 if(stateUsed->diveSettings.ccrOption && gasId < 6) | |
| 511 { | |
| 512 if(lifeData->actualGas.GasIdInSettings != gasId) | |
| 513 { | |
| 514 SSettings* pSettings = settingsGetPointer(); | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
515 stateUsedWrite->events.bailout = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
516 stateUsedWrite->events.info_bailoutO2 = pSettings->gas[gasId].oxygen_percentage; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
517 stateUsedWrite->events.info_bailoutHe = pSettings->gas[gasId].helium_percentage; |
| 38 | 518 } |
| 519 } | |
| 520 else | |
| 521 { | |
| 522 if(lifeData->actualGas.GasIdInSettings != gasId) | |
| 523 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
524 stateUsedWrite->events.gasChange = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
525 stateUsedWrite->events.info_GasChange = gasId; |
| 38 | 526 } |
| 527 if( lifeData->actualGas.setPoint_cbar != setpoint_cbar) | |
| 528 { | |
| 529 // setPoint_cbar = 255 -> change to sensor mode | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
530 stateUsedWrite->events.setpointChange = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
531 stateUsedWrite->events.info_SetpointChange = setpoint_cbar; |
| 38 | 532 } |
| 533 } | |
| 534 setActualGas(lifeData, gasId, setpoint_cbar); | |
| 535 } | |
| 536 | |
| 537 void setActualGas_ExtraGas(SLifeData *lifeData, uint8_t oxygen, uint8_t helium, uint8_t setpoint_cbar) | |
| 538 { | |
| 539 uint8_t nitrogen; | |
| 540 | |
| 541 nitrogen = 100; | |
| 542 nitrogen -= oxygen; | |
| 543 nitrogen -= helium; | |
| 544 | |
| 545 if((lifeData->actualGas.nitrogen_percentage != nitrogen) || (lifeData->actualGas.helium_percentage != helium)) | |
| 546 { | |
| 281 | 547 stateUsedWrite->events.manualGasSet = 1; |
| 548 stateUsedWrite->events.info_manualGasSetHe = helium; | |
| 549 stateUsedWrite->events.info_manualGasSetO2 = oxygen; | |
| 38 | 550 } |
| 551 if( lifeData->actualGas.setPoint_cbar != setpoint_cbar) | |
| 552 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
553 stateUsedWrite->events.setpointChange = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
554 stateUsedWrite->events.info_SetpointChange = setpoint_cbar; |
| 38 | 555 } |
| 556 lifeData->actualGas.GasIdInSettings = 0; | |
| 557 lifeData->actualGas.nitrogen_percentage = nitrogen; | |
| 558 lifeData->actualGas.helium_percentage = helium; | |
| 559 lifeData->actualGas.setPoint_cbar = setpoint_cbar; | |
| 560 lifeData->actualGas.change_during_ascent_depth_meter_otherwise_zero = 0; | |
| 662 | 561 lifeData->actualGas.AppliedDiveMode = stateUsed->diveSettings.diveMode; |
| 38 | 562 } |
| 563 | |
| 564 void setButtonResponsiveness(uint8_t *ButtonSensitivyList) | |
| 565 { | |
| 566 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 567 | |
| 568 for(int i=0; i<4; i++) | |
| 569 { | |
| 570 pDataOut->data.buttonResponsiveness[i] = settingsHelperButtonSens_translate_percentage_to_hwOS_values(ButtonSensitivyList[i]); | |
| 571 } | |
| 572 pDataOut->setButtonSensitivityNow = 1; | |
| 573 } | |
| 574 | |
| 575 | |
| 576 void setDate(RTC_DateTypeDef Sdate) | |
| 577 { | |
| 578 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 579 | |
| 580 pDataOut->data.newDate = Sdate; | |
| 581 pDataOut->setDateNow = 1; | |
| 582 } | |
| 583 | |
| 584 | |
| 585 void setTime(RTC_TimeTypeDef Stime) | |
| 586 { | |
| 587 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 588 | |
| 589 pDataOut->data.newTime = Stime; | |
| 590 pDataOut->setTimeNow = 1; | |
| 591 } | |
| 592 | |
| 593 | |
| 594 void setBatteryPercentage(uint8_t newChargePercentage) | |
| 595 { | |
| 596 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 597 | |
| 598 pDataOut->data.newBatteryGaugePercentageFloat = settingsGetPointer()->lastKnownBatteryPercentage; | |
| 599 pDataOut->setBatteryGaugeNow = 1; | |
| 600 } | |
| 601 | |
| 602 | |
| 603 void calibrateCompass(void) | |
| 604 { | |
| 605 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 606 pDataOut->calibrateCompassNow = 1; | |
| 607 } | |
| 608 | |
| 609 | |
| 610 void clearDeco(void) | |
| 611 { | |
| 612 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 613 pDataOut->clearDecoNow = 1; | |
| 614 | |
| 615 stateRealGetPointerWrite()->cnsHigh_at_the_end_of_dive = 0; | |
| 616 stateRealGetPointerWrite()->decoMissed_at_the_end_of_dive = 0; | |
| 617 } | |
| 618 | |
| 619 | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
620 static int32_t helper_days_from_civil(int32_t y, uint32_t m, uint32_t d) |
| 38 | 621 { |
| 622 y += 2000; | |
| 623 y -= m <= 2; | |
| 624 int32_t era = (y >= 0 ? y : y-399) / 400; | |
| 625 uint32_t yoe = (uint32_t)(y - era * 400); // [0, 399] | |
| 626 uint32_t doy = (153*(m + (m > 2 ? -3 : 9)) + 2)/5 + d-1; // [0, 365] | |
| 627 uint32_t doe = yoe * 365 + yoe/4 - yoe/100 + doy; // [0, 146096] | |
| 628 return era * 146097 + (int32_t)(doe) - 719468; | |
| 629 } | |
| 630 | |
| 631 | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
632 static uint8_t helper_weekday_from_days(int32_t z) |
| 38 | 633 { |
| 634 return (uint8_t)(z >= -4 ? (z+4) % 7 : (z+5) % 7 + 6); | |
| 635 } | |
| 636 | |
| 637 | |
| 638 void setWeekday(RTC_DateTypeDef *sDate) | |
| 639 { | |
| 640 uint8_t day; | |
| 641 // [0, 6] -> [Sun, Sat] | |
| 642 day = helper_weekday_from_days(helper_days_from_civil(sDate->Year, sDate->Month, sDate->Date)); | |
| 643 // [1, 7] -> [Mon, Sun] | |
| 644 if(day == 0) | |
| 645 day = 7; | |
| 646 sDate->WeekDay = day; | |
| 647 } | |
| 648 | |
| 649 | |
| 650 void translateDate(uint32_t datetmpreg, RTC_DateTypeDef *sDate) | |
| 651 { | |
| 652 datetmpreg = (uint32_t)(datetmpreg & RTC_DR_RESERVED_MASK); | |
| 653 | |
| 654 /* Fill the structure fields with the read parameters */ | |
| 655 sDate->Year = (uint8_t)((datetmpreg & (RTC_DR_YT | RTC_DR_YU)) >> 16); | |
| 656 sDate->Month = (uint8_t)((datetmpreg & (RTC_DR_MT | RTC_DR_MU)) >> 8); | |
| 657 sDate->Date = (uint8_t)(datetmpreg & (RTC_DR_DT | RTC_DR_DU)); | |
| 658 sDate->WeekDay = (uint8_t)((datetmpreg & (RTC_DR_WDU)) >> 13); | |
| 659 | |
| 660 /* Convert the date structure parameters to Binary format */ | |
| 661 sDate->Year = (uint8_t)RTC_Bcd2ToByte(sDate->Year); | |
| 662 sDate->Month = (uint8_t)RTC_Bcd2ToByte(sDate->Month); | |
| 663 sDate->Date = (uint8_t)RTC_Bcd2ToByte(sDate->Date); | |
| 664 } | |
| 665 | |
| 666 void translateTime(uint32_t tmpreg, RTC_TimeTypeDef *sTime) | |
| 667 { | |
| 668 tmpreg = (uint32_t)(tmpreg & RTC_TR_RESERVED_MASK); | |
| 669 | |
| 670 /* Fill the structure fields with the read parameters */ | |
| 671 sTime->Hours = (uint8_t)((tmpreg & (RTC_TR_HT | RTC_TR_HU)) >> 16); | |
| 672 sTime->Minutes = (uint8_t)((tmpreg & (RTC_TR_MNT | RTC_TR_MNU)) >>8); | |
| 673 sTime->Seconds = (uint8_t)(tmpreg & (RTC_TR_ST | RTC_TR_SU)); | |
| 674 sTime->TimeFormat = (uint8_t)((tmpreg & (RTC_TR_PM)) >> 16); | |
| 675 | |
| 676 /* Convert the time structure parameters to Binary format */ | |
| 677 sTime->Hours = (uint8_t)RTC_Bcd2ToByte(sTime->Hours); | |
| 678 sTime->Minutes = (uint8_t)RTC_Bcd2ToByte(sTime->Minutes); | |
| 679 sTime->Seconds = (uint8_t)RTC_Bcd2ToByte(sTime->Seconds); | |
| 680 sTime->SubSeconds = 0; | |
| 681 } | |
| 682 | |
|
270
2e58a4094770
feature, debug: make simulator write a logbook entry
Jan Mulder <jlmulder@xs4all.nl>
parents:
225
diff
changeset
|
683 void resetEvents(const SDiveState *pStateUsed) |
| 38 | 684 { |
|
270
2e58a4094770
feature, debug: make simulator write a logbook entry
Jan Mulder <jlmulder@xs4all.nl>
parents:
225
diff
changeset
|
685 memset((void *)&pStateUsed->events, 0, sizeof(SEvents)); |
| 38 | 686 } |
| 687 | |
| 688 | |
| 689 uint32_t CRC_CalcBlockCRC_moreThan768000(uint32_t *buffer1, uint32_t *buffer2, uint32_t words) | |
| 690 { | |
| 691 cm_t crc_model; | |
| 692 uint32_t word_to_do; | |
| 693 uint8_t byte_to_do; | |
| 694 int i; | |
| 695 | |
| 696 // Values for the STM32F generator. | |
| 697 | |
| 698 crc_model.cm_width = 32; // 32-bit CRC | |
| 699 crc_model.cm_poly = 0x04C11DB7; // CRC-32 polynomial | |
| 700 crc_model.cm_init = 0xFFFFFFFF; // CRC initialized to 1's | |
| 701 crc_model.cm_refin = FALSE; // CRC calculated MSB first | |
| 702 crc_model.cm_refot = FALSE; // Final result is not bit-reversed | |
| 703 crc_model.cm_xorot = 0x00000000; // Final result XOR'ed with this | |
| 704 | |
| 705 cm_ini(&crc_model); | |
| 706 | |
| 707 while (words--) | |
| 708 { | |
| 709 // The STM32F10x hardware does 32-bit words at a time!!! | |
| 710 if(words > (768000/4)) | |
| 711 word_to_do = *buffer2++; | |
| 712 else | |
| 713 word_to_do = *buffer1++; | |
| 714 | |
| 715 // Do all bytes in the 32-bit word. | |
| 716 | |
| 717 for (i = 0; i < sizeof(word_to_do); i++) | |
| 718 { | |
| 719 // We calculate a *byte* at a time. If the CRC is MSB first we | |
| 720 // do the next MS byte and vica-versa. | |
| 721 | |
| 722 if (crc_model.cm_refin == FALSE) | |
| 723 { | |
| 724 // MSB first. Do the next MS byte. | |
| 725 | |
| 726 byte_to_do = (uint8_t) ((word_to_do & 0xFF000000) >> 24); | |
| 727 word_to_do <<= 8; | |
| 728 } | |
| 729 else | |
| 730 { | |
| 731 // LSB first. Do the next LS byte. | |
| 732 | |
| 733 byte_to_do = (uint8_t) (word_to_do & 0x000000FF); | |
| 734 word_to_do >>= 8; | |
| 735 } | |
| 736 | |
| 737 cm_nxt(&crc_model, byte_to_do); | |
| 738 } | |
| 739 } | |
| 740 | |
| 741 // Return the final result. | |
| 742 | |
| 743 return (cm_crc(&crc_model)); | |
| 744 } | |
| 745 | |
| 746 | |
| 747 uint32_t CRC_CalcBlockCRC(uint32_t *buffer, uint32_t words) | |
| 748 { | |
| 749 cm_t crc_model; | |
| 750 uint32_t word_to_do; | |
| 751 uint8_t byte_to_do; | |
| 752 int i; | |
| 753 | |
| 754 // Values for the STM32F generator. | |
| 755 | |
| 756 crc_model.cm_width = 32; // 32-bit CRC | |
| 757 crc_model.cm_poly = 0x04C11DB7; // CRC-32 polynomial | |
| 758 crc_model.cm_init = 0xFFFFFFFF; // CRC initialized to 1's | |
| 759 crc_model.cm_refin = FALSE; // CRC calculated MSB first | |
| 760 crc_model.cm_refot = FALSE; // Final result is not bit-reversed | |
| 761 crc_model.cm_xorot = 0x00000000; // Final result XOR'ed with this | |
| 762 | |
| 763 cm_ini(&crc_model); | |
| 764 | |
| 765 while (words--) | |
| 766 { | |
| 767 // The STM32F10x hardware does 32-bit words at a time!!! | |
| 768 | |
| 769 word_to_do = *buffer++; | |
| 770 | |
| 771 // Do all bytes in the 32-bit word. | |
| 772 | |
| 773 for (i = 0; i < sizeof(word_to_do); i++) | |
| 774 { | |
| 775 // We calculate a *byte* at a time. If the CRC is MSB first we | |
| 776 // do the next MS byte and vica-versa. | |
| 777 | |
| 778 if (crc_model.cm_refin == FALSE) | |
| 779 { | |
| 780 // MSB first. Do the next MS byte. | |
| 781 | |
| 782 byte_to_do = (uint8_t) ((word_to_do & 0xFF000000) >> 24); | |
| 783 word_to_do <<= 8; | |
| 784 } | |
| 785 else | |
| 786 { | |
| 787 // LSB first. Do the next LS byte. | |
| 788 | |
| 789 byte_to_do = (uint8_t) (word_to_do & 0x000000FF); | |
| 790 word_to_do >>= 8; | |
| 791 } | |
| 792 | |
| 793 cm_nxt(&crc_model, byte_to_do); | |
| 794 } | |
| 795 } | |
| 796 | |
| 797 // Return the final result. | |
| 798 | |
| 799 return (cm_crc(&crc_model)); | |
| 800 } | |
| 801 | |
|
302
eba8d1eb5bef
bugfix, cleanup: keep both is_ambient_pressure_close_to_surface in sync
Jan Mulder <jlmulder@xs4all.nl>
parents:
300
diff
changeset
|
802 // This code is also in RTE. Keep it in sync when editing |
| 38 | 803 _Bool is_ambient_pressure_close_to_surface(SLifeData *lifeData) |
| 804 { | |
|
310
95928ef3986f
Make dive mode detection more advanced
Jan Mulder <jlmulder@xs4all.nl>
parents:
302
diff
changeset
|
805 if (lifeData->pressure_ambient_bar > 1.16) |
|
95928ef3986f
Make dive mode detection more advanced
Jan Mulder <jlmulder@xs4all.nl>
parents:
302
diff
changeset
|
806 return false; |
|
95928ef3986f
Make dive mode detection more advanced
Jan Mulder <jlmulder@xs4all.nl>
parents:
302
diff
changeset
|
807 else if(lifeData->pressure_ambient_bar < (lifeData->pressure_surface_bar + 0.1f)) |
| 38 | 808 return true; |
| 809 else | |
| 810 return false; | |
| 811 } | |
|
539
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
812 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
813 void compass_Inertia(float newHeading) |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
814 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
815 float newTarget = newHeading; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
816 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
817 if(settingsGetPointer()->compassInertia == 0) |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
818 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
819 compass_compensated = newHeading; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
820 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
821 else |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
822 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
823 if((compass_compensated > 270.0) && (newHeading < 90.0)) /* transition passing 0 clockwise */ |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
824 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
825 newTarget = newHeading + 360.0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
826 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
827 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
828 if((compass_compensated < 90.0) && (newHeading > 270.0)) /* transition passing 0 counter clockwise */ |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
829 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
830 newTarget = newHeading - 360.0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
831 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
832 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
833 compass_compensated = compass_compensated + ((newTarget - compass_compensated) / (COMPASS_FRACTION * (settingsGetPointer()->compassInertia))); |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
834 if(compass_compensated < 0.0) |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
835 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
836 compass_compensated += 360.0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
837 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
838 if(compass_compensated >= 360.0) |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
839 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
840 compass_compensated -= 360.0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
841 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
842 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
843 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
844 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
845 float compass_getCompensated() |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
846 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
847 return compass_compensated; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
848 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
849 |
| 662 | 850 uint8_t isLoopMode(uint8_t Mode) |
| 851 { | |
| 852 uint8_t retVal = 0; | |
| 853 if((Mode == DIVEMODE_CCR) || (Mode == DIVEMODE_PSCR)) | |
| 854 { | |
| 855 retVal = 1; | |
| 856 } | |
| 857 return retVal; | |
| 858 } |