Mercurial > public > ostc4
annotate Discovery/Src/data_central.c @ 747:df0d43da1614
Added pressure compensation to CO2 detection:
A pressure compensation is needed if the ExplorIR shall be used under extended pressure conditions. The procedure recommended by the application note has been integrated. To keep things simple the focus of the CO2 measurement is not the precision, as it is needed for the decompression calculation, but the indication of a critical increase of CO2 in the breathing loop. That's why only a lookup table with 1000ppm steps has been implemented instead of calculating the polynom for every measurement.
| author | Ideenmodellierer |
|---|---|
| date | Sun, 05 Mar 2023 22:06:47 +0100 |
| parents | 8a2337c7af52 |
| children | 29d9b5bc7946 |
| 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 stateReal.diveSettings.compassHeading = pSettings->compassBearing; | |
| 324 stateReal.diveSettings.ascentRate_meterperminute = 10; | |
| 325 | |
| 326 stateReal.diveSettings.diveMode = pSettings->dive_mode; | |
| 327 stateReal.diveSettings.CCR_Mode = pSettings->CCR_Mode; | |
| 662 | 328 if((stateReal.diveSettings.diveMode == DIVEMODE_PSCR) && (stateReal.diveSettings.CCR_Mode == CCRMODE_FixedSetpoint)) |
| 329 { | |
| 330 /* 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 */ | |
| 331 stateReal.diveSettings.CCR_Mode = CCRMODE_Simulation; | |
| 332 } | |
| 333 | |
| 334 if(isLoopMode(stateReal.diveSettings.diveMode)) | |
| 38 | 335 stateReal.diveSettings.ccrOption = 1; |
| 336 else | |
| 337 stateReal.diveSettings.ccrOption = 0; | |
| 338 memcpy(stateReal.diveSettings.gas, pSettings->gas,sizeof(pSettings->gas)); | |
| 339 memcpy(stateReal.diveSettings.setpoint, pSettings->setpoint,sizeof(pSettings->setpoint)); | |
|
683
6f5a18bb25be
Bugfix: Set actual gas after basic dive settings were created:
Ideenmodellierer
parents:
682
diff
changeset
|
340 |
|
6f5a18bb25be
Bugfix: Set actual gas after basic dive settings were created:
Ideenmodellierer
parents:
682
diff
changeset
|
341 setActualGasFirst(&stateReal.lifeData); |
|
6f5a18bb25be
Bugfix: Set actual gas after basic dive settings were created:
Ideenmodellierer
parents:
682
diff
changeset
|
342 |
| 38 | 343 stateReal.diveSettings.gf_high = pSettings->GF_high; |
| 344 stateReal.diveSettings.gf_low = pSettings->GF_low; | |
| 345 stateReal.diveSettings.input_next_stop_increment_depth_bar = ((float)pSettings->stop_increment_depth_meter) / 10.0f; | |
| 346 stateReal.diveSettings.last_stop_depth_bar = ((float)pSettings->last_stop_depth_meter) / 10.0f; | |
| 347 stateReal.diveSettings.vpm_conservatism = pSettings->VPM_conservatism.ub.standard; | |
| 348 stateReal.diveSettings.deco_type.uw = pSettings->deco_type.uw; | |
| 349 stateReal.diveSettings.fallbackOption = pSettings->fallbackToFixedSetpoint; | |
| 350 stateReal.diveSettings.ppo2sensors_deactivated = pSettings->ppo2sensors_deactivated; | |
| 351 stateReal.diveSettings.future_TTS_minutes = pSettings->future_TTS; | |
| 352 | |
| 662 | 353 stateReal.diveSettings.pscr_lung_ratio = pSettings->pscr_lung_ratio; |
| 354 stateReal.diveSettings.pscr_o2_drop = pSettings->pscr_o2_drop; | |
| 355 | |
| 356 if(stateReal.diveSettings.diveMode == DIVEMODE_PSCR) | |
| 357 { | |
| 358 for(i=0; i<5; i++) | |
| 359 { | |
| 360 stateReal.diveSettings.decogaslist[i].pscr_factor = 1.0 / stateReal.diveSettings.pscr_lung_ratio * stateReal.diveSettings.pscr_o2_drop; | |
| 361 } | |
| 362 } | |
| 363 | |
| 38 | 364 decom_CreateGasChangeList(&stateReal.diveSettings, &stateReal.lifeData); // decogaslist |
| 365 stateReal.diveSettings.internal__pressure_first_stop_ambient_bar_as_upper_limit_for_gf_low_otherwise_zero = 0; | |
| 366 | |
| 367 /* for safety */ | |
| 368 stateReal.diveSettings.input_second_to_last_stop_depth_bar = stateReal.diveSettings.last_stop_depth_bar + stateReal.diveSettings.input_next_stop_increment_depth_bar; | |
| 369 /* and the proper calc */ | |
| 662 | 370 for(i = 1; i <10; i++) |
| 38 | 371 { |
| 372 if(stateReal.diveSettings.input_next_stop_increment_depth_bar * i > stateReal.diveSettings.last_stop_depth_bar) | |
| 373 { | |
| 374 stateReal.diveSettings.input_second_to_last_stop_depth_bar = stateReal.diveSettings.input_next_stop_increment_depth_bar * i; | |
| 375 break; | |
| 376 } | |
| 377 } | |
| 378 } | |
| 379 | |
| 380 | |
| 381 void copyDiveSettingsToSim(void) | |
| 382 { | |
| 383 memcpy(&stateSim, &stateReal, sizeof(stateReal)); | |
| 384 } | |
| 385 | |
| 386 | |
| 387 void copyVpmRepetetiveDataToSim(void) | |
| 388 { | |
| 389 SDiveState * pSimData = stateSimGetPointerWrite(); | |
| 390 const SVpmRepetitiveData * pVpmData = stateVpmRepetitiveDataGetPointer(); | |
| 391 | |
| 392 if(pVpmData->is_data_from_RTE_CPU) | |
| 393 { | |
| 394 for(int i=0; i<16;i++) | |
| 395 { | |
| 396 pSimData->vpm.adjusted_critical_radius_he[i] = pVpmData->adjusted_critical_radius_he[i]; | |
| 397 pSimData->vpm.adjusted_critical_radius_n2[i] = pVpmData->adjusted_critical_radius_n2[i]; | |
| 398 | |
| 399 pSimData->vpm.adjusted_crushing_pressure_he[i] = pVpmData->adjusted_crushing_pressure_he[i]; | |
| 400 pSimData->vpm.adjusted_crushing_pressure_n2[i] = pVpmData->adjusted_crushing_pressure_n2[i]; | |
| 401 | |
| 402 pSimData->vpm.initial_allowable_gradient_he[i] = pVpmData->initial_allowable_gradient_he[i]; | |
| 403 pSimData->vpm.initial_allowable_gradient_n2[i] = pVpmData->initial_allowable_gradient_n2[i]; | |
| 404 | |
| 405 pSimData->vpm.max_actual_gradient[i] = pVpmData->max_actual_gradient[i]; | |
| 406 } | |
| 407 pSimData->vpm.repetitive_variables_not_valid = pVpmData->repetitive_variables_not_valid; | |
| 408 } | |
| 409 } | |
| 410 | |
| 411 | |
| 662 | 412 |
| 413 | |
| 38 | 414 void updateSetpointStateUsed(void) |
| 415 { | |
| 682 | 416 if(!isLoopMode(stateUsed->diveSettings.diveMode)) |
| 38 | 417 { |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
418 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
|
419 stateUsedWrite->lifeData.ppO2 = decom_calc_ppO2(stateUsed->lifeData.pressure_ambient_bar, &stateUsed->lifeData.actualGas); |
| 38 | 420 } |
| 421 else | |
| 422 { | |
| 423 if(stateUsed->diveSettings.CCR_Mode == CCRMODE_Sensors) | |
| 424 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
425 stateUsedWrite->lifeData.actualGas.setPoint_cbar = get_ppO2SensorWeightedResult_cbar(); |
| 38 | 426 } |
| 662 | 427 #ifdef ENABLE_PSCR_MODE |
| 428 if(stateUsed->diveSettings.diveMode == DIVEMODE_PSCR) /* calculate a ppO2 value based on assumptions ( transfered approach from hwos code) */ | |
| 429 { | |
| 430 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); | |
| 431 if(stateUsed->diveSettings.CCR_Mode == CCRMODE_Simulation) | |
| 432 { | |
| 433 stateUsedWrite->lifeData.actualGas.setPoint_cbar = stateUsedWrite->lifeData.ppo2Simulated_bar * 100; | |
| 434 } | |
| 435 } | |
| 436 #endif | |
| 437 /* limit calculated value to the physically possible if needed */ | |
| 38 | 438 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
|
439 stateUsedWrite->lifeData.ppO2 = stateUsed->lifeData.pressure_ambient_bar; |
| 38 | 440 else |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
441 stateUsedWrite->lifeData.ppO2 = ((float)stateUsed->lifeData.actualGas.setPoint_cbar) / 100; |
| 38 | 442 } |
| 443 } | |
| 444 | |
| 445 void setActualGasFirst(SLifeData *lifeData) | |
| 446 { | |
| 447 SSettings* pSettings = settingsGetPointer(); | |
| 448 uint8_t start = 0; | |
| 449 uint8_t gasId = 0; | |
| 450 uint8_t setpoint_cbar = 0; | |
| 451 | |
| 662 | 452 if(isLoopMode(pSettings->dive_mode)) |
| 38 | 453 { |
| 454 setpoint_cbar = pSettings->setpoint[1].setpoint_cbar; | |
| 455 start = NUM_OFFSET_DILUENT+1; | |
| 456 } | |
| 457 else | |
| 458 { | |
| 459 setpoint_cbar = 0; | |
| 460 start = 1; | |
| 461 } | |
| 462 | |
| 463 gasId = start; | |
| 464 for(int i=start;i<=NUM_GASES+start;i++) | |
| 465 { | |
| 466 if(pSettings->gas[i].note.ub.first) | |
| 467 { | |
| 468 gasId = i; | |
| 469 break; | |
| 470 } | |
| 471 } | |
| 472 setActualGas(lifeData, gasId, setpoint_cbar); | |
| 473 } | |
| 474 | |
| 475 void setActualGasAir(SLifeData *lifeData) | |
| 476 { | |
| 477 uint8_t nitrogen; | |
| 478 nitrogen = 79; | |
| 479 lifeData->actualGas.GasIdInSettings = 0; | |
| 480 lifeData->actualGas.nitrogen_percentage = nitrogen; | |
| 481 lifeData->actualGas.helium_percentage =0; | |
| 482 lifeData->actualGas.setPoint_cbar = 0; | |
| 483 lifeData->actualGas.change_during_ascent_depth_meter_otherwise_zero = 0; | |
| 662 | 484 lifeData->actualGas.AppliedDiveMode = stateUsed->diveSettings.diveMode; |
| 38 | 485 } |
| 486 | |
| 487 | |
| 488 void setActualGas(SLifeData *lifeData, uint8_t gasId, uint8_t setpoint_cbar) | |
| 489 { | |
| 490 SSettings* pSettings = settingsGetPointer(); | |
| 491 uint8_t nitrogen; | |
| 492 | |
| 493 nitrogen = 100; | |
| 494 nitrogen -= pSettings->gas[gasId].oxygen_percentage; | |
| 495 nitrogen -= pSettings->gas[gasId].helium_percentage; | |
| 496 | |
| 497 lifeData->actualGas.GasIdInSettings = gasId; | |
| 498 lifeData->actualGas.nitrogen_percentage = nitrogen; | |
| 499 lifeData->actualGas.helium_percentage = pSettings->gas[gasId].helium_percentage; | |
| 500 lifeData->actualGas.setPoint_cbar = setpoint_cbar; | |
| 501 lifeData->actualGas.change_during_ascent_depth_meter_otherwise_zero = 0; | |
| 682 | 502 lifeData->actualGas.AppliedDiveMode = stateUsed->diveSettings.diveMode; |
| 662 | 503 lifeData->actualGas.pscr_factor = 1.0 / pSettings->pscr_lung_ratio * pSettings->pscr_o2_drop; |
| 504 if(isLoopMode(pSettings->dive_mode) && (gasId > NUM_OFFSET_DILUENT)) | |
| 38 | 505 lifeData->lastDiluent_GasIdInSettings = gasId; |
| 506 } | |
| 507 | |
| 508 | |
| 509 void setActualGas_DM(SLifeData *lifeData, uint8_t gasId, uint8_t setpoint_cbar) | |
| 510 { | |
| 511 if(stateUsed->diveSettings.ccrOption && gasId < 6) | |
| 512 { | |
| 513 if(lifeData->actualGas.GasIdInSettings != gasId) | |
| 514 { | |
| 515 SSettings* pSettings = settingsGetPointer(); | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
516 stateUsedWrite->events.bailout = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
517 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
|
518 stateUsedWrite->events.info_bailoutHe = pSettings->gas[gasId].helium_percentage; |
| 38 | 519 } |
| 520 } | |
| 521 else | |
| 522 { | |
| 523 if(lifeData->actualGas.GasIdInSettings != gasId) | |
| 524 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
525 stateUsedWrite->events.gasChange = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
526 stateUsedWrite->events.info_GasChange = gasId; |
| 38 | 527 } |
| 528 if( lifeData->actualGas.setPoint_cbar != setpoint_cbar) | |
| 529 { | |
| 530 // 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
|
531 stateUsedWrite->events.setpointChange = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
532 stateUsedWrite->events.info_SetpointChange = setpoint_cbar; |
| 38 | 533 } |
| 534 } | |
| 535 setActualGas(lifeData, gasId, setpoint_cbar); | |
| 536 } | |
| 537 | |
| 538 void setActualGas_ExtraGas(SLifeData *lifeData, uint8_t oxygen, uint8_t helium, uint8_t setpoint_cbar) | |
| 539 { | |
| 540 uint8_t nitrogen; | |
| 541 | |
| 542 nitrogen = 100; | |
| 543 nitrogen -= oxygen; | |
| 544 nitrogen -= helium; | |
| 545 | |
|
736
8a2337c7af52
Fixed logbook download after switch from CCR to extra (OC) gas if the extra gas
heinrichsweikamp
parents:
683
diff
changeset
|
546 |
|
8a2337c7af52
Fixed logbook download after switch from CCR to extra (OC) gas if the extra gas
heinrichsweikamp
parents:
683
diff
changeset
|
547 if((lifeData->actualGas.nitrogen_percentage != nitrogen) || (lifeData->actualGas.helium_percentage != helium) || (lifeData->actualGas.AppliedDiveMode != DIVEMODE_OC)) |
| 38 | 548 { |
| 281 | 549 stateUsedWrite->events.manualGasSet = 1; |
| 550 stateUsedWrite->events.info_manualGasSetHe = helium; | |
| 551 stateUsedWrite->events.info_manualGasSetO2 = oxygen; | |
| 38 | 552 } |
| 553 if( lifeData->actualGas.setPoint_cbar != setpoint_cbar) | |
| 554 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
555 stateUsedWrite->events.setpointChange = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
556 stateUsedWrite->events.info_SetpointChange = setpoint_cbar; |
| 38 | 557 } |
| 558 lifeData->actualGas.GasIdInSettings = 0; | |
| 559 lifeData->actualGas.nitrogen_percentage = nitrogen; | |
| 560 lifeData->actualGas.helium_percentage = helium; | |
| 561 lifeData->actualGas.setPoint_cbar = setpoint_cbar; | |
| 562 lifeData->actualGas.change_during_ascent_depth_meter_otherwise_zero = 0; | |
| 662 | 563 lifeData->actualGas.AppliedDiveMode = stateUsed->diveSettings.diveMode; |
| 38 | 564 } |
| 565 | |
| 566 void setButtonResponsiveness(uint8_t *ButtonSensitivyList) | |
| 567 { | |
| 568 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 569 | |
| 570 for(int i=0; i<4; i++) | |
| 571 { | |
| 572 pDataOut->data.buttonResponsiveness[i] = settingsHelperButtonSens_translate_percentage_to_hwOS_values(ButtonSensitivyList[i]); | |
| 573 } | |
| 574 pDataOut->setButtonSensitivityNow = 1; | |
| 575 } | |
| 576 | |
| 577 | |
| 578 void setDate(RTC_DateTypeDef Sdate) | |
| 579 { | |
| 580 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 581 | |
| 582 pDataOut->data.newDate = Sdate; | |
| 583 pDataOut->setDateNow = 1; | |
| 584 } | |
| 585 | |
| 586 | |
| 587 void setTime(RTC_TimeTypeDef Stime) | |
| 588 { | |
| 589 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 590 | |
| 591 pDataOut->data.newTime = Stime; | |
| 592 pDataOut->setTimeNow = 1; | |
| 593 } | |
| 594 | |
| 595 | |
| 596 void setBatteryPercentage(uint8_t newChargePercentage) | |
| 597 { | |
| 598 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 599 | |
| 600 pDataOut->data.newBatteryGaugePercentageFloat = settingsGetPointer()->lastKnownBatteryPercentage; | |
| 601 pDataOut->setBatteryGaugeNow = 1; | |
| 602 } | |
| 603 | |
| 604 | |
| 605 void calibrateCompass(void) | |
| 606 { | |
| 607 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 608 pDataOut->calibrateCompassNow = 1; | |
| 609 } | |
| 610 | |
| 611 | |
| 612 void clearDeco(void) | |
| 613 { | |
| 614 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 615 pDataOut->clearDecoNow = 1; | |
| 616 | |
| 617 stateRealGetPointerWrite()->cnsHigh_at_the_end_of_dive = 0; | |
| 618 stateRealGetPointerWrite()->decoMissed_at_the_end_of_dive = 0; | |
| 619 } | |
| 620 | |
| 621 | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
622 static int32_t helper_days_from_civil(int32_t y, uint32_t m, uint32_t d) |
| 38 | 623 { |
| 624 y += 2000; | |
| 625 y -= m <= 2; | |
| 626 int32_t era = (y >= 0 ? y : y-399) / 400; | |
| 627 uint32_t yoe = (uint32_t)(y - era * 400); // [0, 399] | |
| 628 uint32_t doy = (153*(m + (m > 2 ? -3 : 9)) + 2)/5 + d-1; // [0, 365] | |
| 629 uint32_t doe = yoe * 365 + yoe/4 - yoe/100 + doy; // [0, 146096] | |
| 630 return era * 146097 + (int32_t)(doe) - 719468; | |
| 631 } | |
| 632 | |
| 633 | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
634 static uint8_t helper_weekday_from_days(int32_t z) |
| 38 | 635 { |
| 636 return (uint8_t)(z >= -4 ? (z+4) % 7 : (z+5) % 7 + 6); | |
| 637 } | |
| 638 | |
| 639 | |
| 640 void setWeekday(RTC_DateTypeDef *sDate) | |
| 641 { | |
| 642 uint8_t day; | |
| 643 // [0, 6] -> [Sun, Sat] | |
| 644 day = helper_weekday_from_days(helper_days_from_civil(sDate->Year, sDate->Month, sDate->Date)); | |
| 645 // [1, 7] -> [Mon, Sun] | |
| 646 if(day == 0) | |
| 647 day = 7; | |
| 648 sDate->WeekDay = day; | |
| 649 } | |
| 650 | |
| 651 | |
| 652 void translateDate(uint32_t datetmpreg, RTC_DateTypeDef *sDate) | |
| 653 { | |
| 654 datetmpreg = (uint32_t)(datetmpreg & RTC_DR_RESERVED_MASK); | |
| 655 | |
| 656 /* Fill the structure fields with the read parameters */ | |
| 657 sDate->Year = (uint8_t)((datetmpreg & (RTC_DR_YT | RTC_DR_YU)) >> 16); | |
| 658 sDate->Month = (uint8_t)((datetmpreg & (RTC_DR_MT | RTC_DR_MU)) >> 8); | |
| 659 sDate->Date = (uint8_t)(datetmpreg & (RTC_DR_DT | RTC_DR_DU)); | |
| 660 sDate->WeekDay = (uint8_t)((datetmpreg & (RTC_DR_WDU)) >> 13); | |
| 661 | |
| 662 /* Convert the date structure parameters to Binary format */ | |
| 663 sDate->Year = (uint8_t)RTC_Bcd2ToByte(sDate->Year); | |
| 664 sDate->Month = (uint8_t)RTC_Bcd2ToByte(sDate->Month); | |
| 665 sDate->Date = (uint8_t)RTC_Bcd2ToByte(sDate->Date); | |
| 666 } | |
| 667 | |
| 668 void translateTime(uint32_t tmpreg, RTC_TimeTypeDef *sTime) | |
| 669 { | |
| 670 tmpreg = (uint32_t)(tmpreg & RTC_TR_RESERVED_MASK); | |
| 671 | |
| 672 /* Fill the structure fields with the read parameters */ | |
| 673 sTime->Hours = (uint8_t)((tmpreg & (RTC_TR_HT | RTC_TR_HU)) >> 16); | |
| 674 sTime->Minutes = (uint8_t)((tmpreg & (RTC_TR_MNT | RTC_TR_MNU)) >>8); | |
| 675 sTime->Seconds = (uint8_t)(tmpreg & (RTC_TR_ST | RTC_TR_SU)); | |
| 676 sTime->TimeFormat = (uint8_t)((tmpreg & (RTC_TR_PM)) >> 16); | |
| 677 | |
| 678 /* Convert the time structure parameters to Binary format */ | |
| 679 sTime->Hours = (uint8_t)RTC_Bcd2ToByte(sTime->Hours); | |
| 680 sTime->Minutes = (uint8_t)RTC_Bcd2ToByte(sTime->Minutes); | |
| 681 sTime->Seconds = (uint8_t)RTC_Bcd2ToByte(sTime->Seconds); | |
| 682 sTime->SubSeconds = 0; | |
| 683 } | |
| 684 | |
|
270
2e58a4094770
feature, debug: make simulator write a logbook entry
Jan Mulder <jlmulder@xs4all.nl>
parents:
225
diff
changeset
|
685 void resetEvents(const SDiveState *pStateUsed) |
| 38 | 686 { |
|
270
2e58a4094770
feature, debug: make simulator write a logbook entry
Jan Mulder <jlmulder@xs4all.nl>
parents:
225
diff
changeset
|
687 memset((void *)&pStateUsed->events, 0, sizeof(SEvents)); |
| 38 | 688 } |
| 689 | |
| 690 | |
| 691 uint32_t CRC_CalcBlockCRC_moreThan768000(uint32_t *buffer1, uint32_t *buffer2, uint32_t words) | |
| 692 { | |
| 693 cm_t crc_model; | |
| 694 uint32_t word_to_do; | |
| 695 uint8_t byte_to_do; | |
| 696 int i; | |
| 697 | |
| 698 // Values for the STM32F generator. | |
| 699 | |
| 700 crc_model.cm_width = 32; // 32-bit CRC | |
| 701 crc_model.cm_poly = 0x04C11DB7; // CRC-32 polynomial | |
| 702 crc_model.cm_init = 0xFFFFFFFF; // CRC initialized to 1's | |
| 703 crc_model.cm_refin = FALSE; // CRC calculated MSB first | |
| 704 crc_model.cm_refot = FALSE; // Final result is not bit-reversed | |
| 705 crc_model.cm_xorot = 0x00000000; // Final result XOR'ed with this | |
| 706 | |
| 707 cm_ini(&crc_model); | |
| 708 | |
| 709 while (words--) | |
| 710 { | |
| 711 // The STM32F10x hardware does 32-bit words at a time!!! | |
| 712 if(words > (768000/4)) | |
| 713 word_to_do = *buffer2++; | |
| 714 else | |
| 715 word_to_do = *buffer1++; | |
| 716 | |
| 717 // Do all bytes in the 32-bit word. | |
| 718 | |
| 719 for (i = 0; i < sizeof(word_to_do); i++) | |
| 720 { | |
| 721 // We calculate a *byte* at a time. If the CRC is MSB first we | |
| 722 // do the next MS byte and vica-versa. | |
| 723 | |
| 724 if (crc_model.cm_refin == FALSE) | |
| 725 { | |
| 726 // MSB first. Do the next MS byte. | |
| 727 | |
| 728 byte_to_do = (uint8_t) ((word_to_do & 0xFF000000) >> 24); | |
| 729 word_to_do <<= 8; | |
| 730 } | |
| 731 else | |
| 732 { | |
| 733 // LSB first. Do the next LS byte. | |
| 734 | |
| 735 byte_to_do = (uint8_t) (word_to_do & 0x000000FF); | |
| 736 word_to_do >>= 8; | |
| 737 } | |
| 738 | |
| 739 cm_nxt(&crc_model, byte_to_do); | |
| 740 } | |
| 741 } | |
| 742 | |
| 743 // Return the final result. | |
| 744 | |
| 745 return (cm_crc(&crc_model)); | |
| 746 } | |
| 747 | |
| 748 | |
| 749 uint32_t CRC_CalcBlockCRC(uint32_t *buffer, uint32_t words) | |
| 750 { | |
| 751 cm_t crc_model; | |
| 752 uint32_t word_to_do; | |
| 753 uint8_t byte_to_do; | |
| 754 int i; | |
| 755 | |
| 756 // Values for the STM32F generator. | |
| 757 | |
| 758 crc_model.cm_width = 32; // 32-bit CRC | |
| 759 crc_model.cm_poly = 0x04C11DB7; // CRC-32 polynomial | |
| 760 crc_model.cm_init = 0xFFFFFFFF; // CRC initialized to 1's | |
| 761 crc_model.cm_refin = FALSE; // CRC calculated MSB first | |
| 762 crc_model.cm_refot = FALSE; // Final result is not bit-reversed | |
| 763 crc_model.cm_xorot = 0x00000000; // Final result XOR'ed with this | |
| 764 | |
| 765 cm_ini(&crc_model); | |
| 766 | |
| 767 while (words--) | |
| 768 { | |
| 769 // The STM32F10x hardware does 32-bit words at a time!!! | |
| 770 | |
| 771 word_to_do = *buffer++; | |
| 772 | |
| 773 // Do all bytes in the 32-bit word. | |
| 774 | |
| 775 for (i = 0; i < sizeof(word_to_do); i++) | |
| 776 { | |
| 777 // We calculate a *byte* at a time. If the CRC is MSB first we | |
| 778 // do the next MS byte and vica-versa. | |
| 779 | |
| 780 if (crc_model.cm_refin == FALSE) | |
| 781 { | |
| 782 // MSB first. Do the next MS byte. | |
| 783 | |
| 784 byte_to_do = (uint8_t) ((word_to_do & 0xFF000000) >> 24); | |
| 785 word_to_do <<= 8; | |
| 786 } | |
| 787 else | |
| 788 { | |
| 789 // LSB first. Do the next LS byte. | |
| 790 | |
| 791 byte_to_do = (uint8_t) (word_to_do & 0x000000FF); | |
| 792 word_to_do >>= 8; | |
| 793 } | |
| 794 | |
| 795 cm_nxt(&crc_model, byte_to_do); | |
| 796 } | |
| 797 } | |
| 798 | |
| 799 // Return the final result. | |
| 800 | |
| 801 return (cm_crc(&crc_model)); | |
| 802 } | |
| 803 | |
|
302
eba8d1eb5bef
bugfix, cleanup: keep both is_ambient_pressure_close_to_surface in sync
Jan Mulder <jlmulder@xs4all.nl>
parents:
300
diff
changeset
|
804 // This code is also in RTE. Keep it in sync when editing |
| 38 | 805 _Bool is_ambient_pressure_close_to_surface(SLifeData *lifeData) |
| 806 { | |
|
310
95928ef3986f
Make dive mode detection more advanced
Jan Mulder <jlmulder@xs4all.nl>
parents:
302
diff
changeset
|
807 if (lifeData->pressure_ambient_bar > 1.16) |
|
95928ef3986f
Make dive mode detection more advanced
Jan Mulder <jlmulder@xs4all.nl>
parents:
302
diff
changeset
|
808 return false; |
|
95928ef3986f
Make dive mode detection more advanced
Jan Mulder <jlmulder@xs4all.nl>
parents:
302
diff
changeset
|
809 else if(lifeData->pressure_ambient_bar < (lifeData->pressure_surface_bar + 0.1f)) |
| 38 | 810 return true; |
| 811 else | |
| 812 return false; | |
| 813 } | |
|
539
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 void compass_Inertia(float 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 float newTarget = newHeading; |
|
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 if(settingsGetPointer()->compassInertia == 0) |
|
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 compass_compensated = newHeading; |
|
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 else |
|
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 if((compass_compensated > 270.0) && (newHeading < 90.0)) /* transition passing 0 clockwise */ |
|
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 newTarget = newHeading + 360.0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
828 } |
|
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 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
|
831 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
832 newTarget = newHeading - 360.0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
833 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
834 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
835 compass_compensated = compass_compensated + ((newTarget - compass_compensated) / (COMPASS_FRACTION * (settingsGetPointer()->compassInertia))); |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
836 if(compass_compensated < 0.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 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 if(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 compass_compensated -= 360.0; |
|
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 } |
|
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 float compass_getCompensated() |
|
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 return compass_compensated; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
850 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
851 |
| 662 | 852 uint8_t isLoopMode(uint8_t Mode) |
| 853 { | |
| 854 uint8_t retVal = 0; | |
| 855 if((Mode == DIVEMODE_CCR) || (Mode == DIVEMODE_PSCR)) | |
| 856 { | |
| 857 retVal = 1; | |
| 858 } | |
| 859 return retVal; | |
| 860 } |